第6讲非线性优化（偏实践）

YMWM_

已于 2024-02-03 18:25:47 修改

阅读量504

点赞数 2

分类专栏：《视觉SLAM十四讲》代码研读文章标签： slam ubuntu 非线性优化

于 2021-05-09 19:31:40 首次发布

本文链接：https://blog.csdn.net/YMWM_/article/details/116567337

版权

《视觉SLAM十四讲》代码研读专栏收录该内容

17 篇文章 38 订阅

订阅专栏

1 手写高斯牛顿法

对于如下测量方程，
$y=e^{ax^2+bx+c}+w$
其中 $y$ 表示对状态 $a,b,c)^T$ 的测量； $w$ 为噪声，它服从均值为0，方差为1的高斯分布。同时，有如下100个数据点，
$x_{data}[100] = \{0, 0.01, 0.02, …x_i…， 0.99 \}$

$y_{data}(x_i) = e^{ax_i^2+bx_i+c}+Gaussian(0,1)$
请利用高斯牛顿法估计状态 $a,b,c)^T$ 。
解题如下，
上述问题可转变成如下优化问题，
$(a^*,b^*,c^*)= \underset {a,b,c} {argmin} \sum_{i=1}^{100}(y_i-e^{ax_i^2+bx_i+c})^2$
记误差项 $e_i$ 为，
$e_i=y_i-e^{ax_i^2+bx_i+c}$
代价函数为，
$F_{cost} = \sum_{i=1}^{100}{(y_i-e^{ax_i^2+bx_i+c})}^2$
那么，该优化问题的增量方程 $H * d x = b$ 为，

$J_i = \left[ \begin{matrix} {\partial{e_i}} /{a} \\ {\partial{e_i}} /{b} \\ {\partial{e_i}} /{c} \end{matrix} \right] = \left[ \begin{matrix} -x_i^2e^{ax_i^2+bx_i+c} \\ -x_ie^{ax_i^2+bx_i+c} \\ -e^{ax_i^2+bx_i+c} \end{matrix} \right]$

$H=\sum_{i=1}^{100}J_i(\sigma^2)^{-1}J_i^T$

$b=\sum_{i=1}^{100}-J_i(\sigma^2)^{-1}e_i$
其中 $\sigma$ 表示测量噪声的标准差。
求解增量方程 $H * d x = b$ 可以计算出增量 $d x$ ，那么更新状态，
$x_{k+1} = x_k + dx$
当代价函数不再减小时退出迭代！
cpp文件内容如下，

#include <iostream>
#include <ctime>
#include <chrono>

using namespace std;

#include <opencv2/opencv.hpp>

using namespace cv;

#include <Eigen/Core>  //Eigen核心模块
#include <Eigen/Dense>  //Eigen稠密矩阵运算模块

using namespace Eigen;

//本程序用于高斯牛顿法拟合曲线y = exp(a * x^2 + b * x + c)
int main()
{
    double ar = 1.0, br = 2.0, cr = 1.0;  //真实的曲线参数
    double ae = 2.0, be = -1.0, ce = 5.0;  //估计参数值，并赋初始值

    //生成数据点数
    int N = 100;
    double w_sigma = 1.0;  //测量噪声的标准差
    double inv_sigma = 1.0 / w_sigma;
    RNG rng;  //RNG为OpenCV中生成随机数的类，全称是Random Number Generator
    vector<double> x_data, y_data;
    for(int i = 0; i < N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);  //x_data存储0, 0.01, 0.02……，0.99
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
        //用真实值加上高斯白噪声来模拟测量
        //rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数
    }

    //开始Gauss-Newton迭代，也即求ae，be和ce的值，使得代价最小
    int iterations = 100;  //设置迭代次数
    double cost = 0, lastCost = 0;  //cost表示本次迭代的代价，lastCost表示上次迭代的代价
    //cost = error * error，error表示测量方程的残差

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();  //t1表示steady_clock::time_point类型

    for(int iter = 0; iter < iterations; iter++)
    {
        Matrix3d H = Matrix3d::Zero();  //将矩阵H初始化为3*3零矩阵，表示海塞矩阵，H = J * (sigma * sigma).transpose() * J.transpose()
        Vector3d b = Vector3d::Zero();  //将b初始化为3*1零向量，b = -J * (sigma * sigma).transpose() * error，error表示测量方程的残差
        cost = 0;

        //遍历所有数据点计算H，b和cost
        for(int i = 0; i < N; i++)
        {
            double xi = x_data[i], yi = y_data[i];  //第i个数据点
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            cost += error * error;  //计算代价
            Vector3d J;  //定义雅可比矩阵
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);
            J[2] = -exp(ae * xi * xi + be * xi + ce);
            H += J * (inv_sigma * inv_sigma) * J.transpose();
            b += - J * (inv_sigma * inv_sigma) * error;
        }

        //求解线性方程H*dx=b
        Vector3d dx = H.ldlt().solve(b);  //ldlt()表示利用Cholesky分解求dx
        if(isnan(dx[0]))  //isnan()函数判断输入是否为非数字，是非数字返回真，nan全称为not a number
        {
            cout << "dx是非数字！" << endl;
            break;
        }

        if(iter > 0 && cost >= lastCost)  //因为iter要大于0，第1次迭代(iter = 0, cost > lastCost)不执行！
        {
            cout << "已经迭代完" << (iter + 1) << "次，" << "不用迭代完100次，因为代价已经停止减小了。当前代价为"
                 << cost << "大于等于上一时刻代价" << lastCost << "！" << endl;
            break;
        }

        //更新优化变量ae，be和ce！
        ae += dx[0]; be += dx[1]; ce += dx[2];

        lastCost = cost;  //更新上一时刻代价

        cout << "此时为第" << (iter + 1) << "次迭代：" << "此时代价为" << cost << "。更新量为：" << dx[0] << ", " << dx[1] << ", " << dx[2]
             << "。优化变量为：ae等于" << ae << "，be等于" << be << "，ce等于" << ce << "！" << endl;


    }

    cout << "最佳优化变量为：ae等于" << ae << "，be等于" << be << "，ce等于" << ce << "！" << endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行程序花费的时间为" << time_used.count() << "秒！" << endl;

    return 0;
}

CMakeLists.txt文件内容如下，

include_directories("/usr/include/eigen3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

结果如下，

此时为第1次迭代：此时代价为3.19575e+06。更新量为：0.0455771, 0.078164, -0.985329。优化变量为：ae等于2.04558，be等于-0.921836，ce等于4.01467！
此时为第2次迭代：此时代价为376785。更新量为：0.065762, 0.224972, -0.962521。优化变量为：ae等于2.11134，be等于-0.696864，ce等于3.05215！
此时为第3次迭代：此时代价为35673.6。更新量为：-0.0670241, 0.617616, -0.907497。优化变量为：ae等于2.04432，be等于-0.0792484，ce等于2.14465！
此时为第4次迭代：此时代价为2195.01。更新量为：-0.522767, 1.19192, -0.756452。优化变量为：ae等于1.52155，be等于1.11267，ce等于1.3882！
此时为第5次迭代：此时代价为174.853。更新量为：-0.537502, 0.909933, -0.386395。优化变量为：ae等于0.984045，be等于2.0226，ce等于1.00181！
此时为第6次迭代：此时代价为102.78。更新量为：-0.0919666, 0.147331, -0.0573675。优化变量为：ae等于0.892079，be等于2.16994，ce等于0.944438！
此时为第7次迭代：此时代价为101.937。更新量为：-0.00117081, 0.00196749, -0.00081055。优化变量为：ae等于0.890908，be等于2.1719，ce等于0.943628！
此时为第8次迭代：此时代价为101.937。更新量为：3.4312e-06, -4.28555e-06, 1.08348e-06。优化变量为：ae等于0.890912，be等于2.1719，ce等于0.943629！
此时为第9次迭代：此时代价为101.937。更新量为：-2.01204e-08, 2.68928e-08, -7.86602e-09。优化变量为：ae等于0.890912，be等于2.1719，ce等于0.943629！
已经迭代完10次，不用迭代完100次，因为代价已经停止减小了。当前代价为101.937大于等于上一时刻代价101.937！
最佳优化变量为：ae等于0.890912，be等于2.1719，ce等于0.943629！
执行程序花费的时间为0.00727227秒！

2 使用ceres拟合曲线

cpp文件内容如下，

#include <iostream>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>

using namespace cv;

#include <ceres/ceres.h>  //ceres库头文件


//ceres中代价函数的计算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}  //使用初始化列表赋值写法的构造函数！

    //残差的计算
    template<typename T>  //函数模板，使得下面定义的函数可以支持多种不同的形参，避免重载函数的函数体重复设计。
    bool operator()(const T* const abc, T* residual) const //重载运算符()
    {
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
        return true;
        //返回bool类型，计算结果已经存入函数外的residual变量中
    }

    const double _x, _y;  //结构体CURVE_FITTING_COST中的成员变量
};

int main()
{
    double ar = 1.0, br = 2.0, cr = 1.0;  //曲线真实的参数
    double ae = 2.0, be = -1.0, ce = 5.0;  //定义估计的参数值，并赋初始值
    int N = 100;  //数据点数目
    double w_sigma = 1.0;  //测量噪声的标准差
    double inv_sigma = 1.0 / w_sigma;  //标准差的逆

    //生成测量数据
    RNG rng;  //OpenCV随机数产生器
    vector<double> x_data, y_data;
    for(int i = 0; i < N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    //定义优化变量
    double abc[3] = {ae, be, ce};

    //构建最小二乘问题
    ceres::Problem problem;  //定义一个优化问题类problem
    for(int i = 0; i < N; i++)
    {
        problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])), //添加代价函数
                nullptr,  //添加损失函数(即鲁棒核函数)，这里不使用，为空
                abc  //优化变量，3维数组
                );
    }

    //配置ceres中的优化求解器
    ceres::Solver::Options options;  //定义一个配置项集合类options
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  //增量方程H*dx=b的求解方式，本质上是稠密矩阵求逆的加速方法选择
    options.minimizer_progress_to_stdout = true;  //minimizer_progress_to_stdout表示是否向终端输出优化过程信息

    ceres::Solver::Summary summary;  //优化信息

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    ceres::Solve(options, &problem, &summary);  //利用ceres执行优化

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);

    cout << "执行ceres优化所花时间为" << time_used.count() << "秒！" << endl;

    //输出结果
    cout << summary.BriefReport() << endl;
    cout << "最佳的ae,be,ce为：" << abc[0] << ", " << abc[1] << ", " << abc[2] << endl;

    return 0;
}

CMakeLists.txt文件内容如下，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(Ceres REQUIRED)
include_directories(${CERES_DIRECTORIES})  #必须要写成CERES_DIRECTORIES的形式

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} ${CERES_LIBRARIES})

结果如下，

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.597873e+06    0.00e+00    3.52e+06   0.00e+00   0.00e+00  1.00e+04        0    6.97e-04    7.24e-04
   1  1.884440e+05    1.41e+06    4.86e+05   9.88e-01   8.82e-01  1.81e+04        1    7.12e-04    1.49e-03
   2  1.784821e+04    1.71e+05    6.78e+04   9.89e-01   9.06e-01  3.87e+04        1    6.89e-04    2.19e-03
   3  1.099631e+03    1.67e+04    8.58e+03   1.10e+00   9.41e-01  1.16e+05        1    7.26e-04    2.93e-03
   4  8.784938e+01    1.01e+03    6.53e+02   1.51e+00   9.67e-01  3.48e+05        1    6.92e-04    3.64e-03
   5  5.141230e+01    3.64e+01    2.72e+01   1.13e+00   9.90e-01  1.05e+06        1    7.17e-04    4.38e-03
   6  5.096862e+01    4.44e-01    4.27e-01   1.89e-01   9.98e-01  3.14e+06        1    6.89e-04    5.08e-03
   7  5.096851e+01    1.10e-04    9.53e-04   2.84e-03   9.99e-01  9.41e+06        1    7.19e-04    5.81e-03
执行ceres优化所花时间为0.00587958秒！
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
最佳的ae,be,ce为：0.890908, 2.1719, 0.943628

3 使用g2o拟合曲线

使用g2o求解非线性最小二乘问题，它的主要步骤包括：(1)定义顶点和边的类型；(2)构建图；(3)选择优化算法；(4)调用g2o进行优化，返回结果。
cpp文件内容如下，

#include <iostream>
#include <cmath>
#include <chrono>

using namespace std;

#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace g2o;

#include <Eigen/Core>  //Eigen核心模块

using namespace Eigen;

#include <opencv2/core/core.hpp>

using namespace cv;


//曲线模型的顶点
//模板参数：优化变量维度和数据类型
class CurveFittingVertex : public BaseVertex<3, Eigen::Vector3d>
//:表示继承，public表示公有继承；CurveFittingVertex是派生类，BaseVertex<3, Eigen::Vector3d>是基类
{
public:  //以下定义的成员变量和成员函数都是公有的！
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  //解决Eigen库数据结构内存对齐问题

    //重置
    virtual void setToOriginImpl() override  //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
    {
        _estimate << 0, 0, 0;
    }

    //更新
    virtual void oplusImpl(const double* update) override
    {
        _estimate += Eigen::Vector3d(update);
    }

    //存盘和读盘：留空
    virtual bool read(istream &in) {}  //istream类是c++标准输入流的一个基类
    virtual bool write(ostream &out) const {}  //ostream类是c++标准输出流的一个基类
};


//曲线模型的边
//模板参数：观测值维度、类型、连接顶点类型
class CurveFittingEdge : public BaseUnaryEdge<1, double, CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  //解决Eigen库数据结构内存对齐问题

    CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}  //使用列表赋初值

    //计算曲线模型误差
    virtual void computeError() override  //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0, 0) = _measurement - exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
    }

    //计算雅可比矩阵
    virtual void linearizeOplus() override  //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
    }

    virtual bool read(istream& in) {}
    virtual bool write(ostream& out) const {}

public:
    double _x;  //x值，y值为_measurement

};


int main()
{
    double ar = 1.0, br = 2.0, cr = 1.0;  //真实参数值！
    double ae = 2.0, be = -1.0, ce = 5.0;  //估计参数值，并赋初值！

    //生成测量数据
    int N = 100;  //数据点数目
    double w_sigma = 1.0;  //测量噪声标准差
    double inv_sigma = 1.0 / w_sigma;  //测量噪声标准差的逆
    cv::RNG rng;  //OpenCV随机数生成器
    vector<double> x_data, y_data;
    for(int i = 0; i < N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }


    //构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  //起别名，BlockSolverTraits<3, 1>表示误差项维数
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;  //线性求解器类型

    //梯度下降方法，可以从GN、LM、DogLeg中选
    auto solver = new OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    //c++中的make_unique表示智能指针类型，而g2o中的make_unique表示？？？
    g2o::SparseOptimizer optimizer;  //图模型
    optimizer.setAlgorithm(solver);  //设置求解器
    optimizer.setVerbose(true);  //向屏幕上显示优化过程信息

    //往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(ae, be, ce));  //ae、be和ce表示优化变量
    v->setId(0);
    optimizer.addVertex(v);

    //往图中增加边
    for(int i = 0; i < N; i++)
    {
        CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
        edge->setId(i);
        edge->setVertex(0, v);  //设置连接的顶点
        edge->setMeasurement(y_data[i]);  //设置测量值
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma));  //信息矩阵：协方差矩阵之逆
        optimizer.addEdge(edge);
    }

    //执行优化
    cout << "开始优化！" << endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();  //优化过程初始化
    optimizer.optimize(10);  //设置优化的迭代次数
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行优化花费的时间为：" << time_used.count() << "秒！" << endl;

    //输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout << "优化变量ae、be和ce的最佳结果为：" << abc_estimate[0] << ", " << abc_estimate[1] << ", " << abc_estimate[2] << "!" << endl;

    return 0;
}

CMakeLists.txt文件内容如下，

include_directories("/usr/include/eigen3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(g2o REQUIRED)  #必须要写g2o，写G2O会报错
include_directories(${G2O_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} g2o_core g2o_stuff)

最终结果如下，

开始优化！
执行优化花费的时间为：0.00800677秒！
优化变量ae、be和ce的最佳结果为：0.890912, 2.1719, 0.943629!
iteration= 0	 chi2= 376785.128234	 time= 0.000792764	 cumTime= 0.000792764	 edges= 100	 schur= 0
iteration= 1	 chi2= 35673.566018	 time= 0.000690028	 cumTime= 0.00148279	 edges= 100	 schur= 0
iteration= 2	 chi2= 2195.012304	 time= 0.000678225	 cumTime= 0.00216102	 edges= 100	 schur= 0
iteration= 3	 chi2= 174.853126	 time= 0.000677177	 cumTime= 0.00283819	 edges= 100	 schur= 0
iteration= 4	 chi2= 102.779695	 time= 0.000684581	 cumTime= 0.00352277	 edges= 100	 schur= 0
iteration= 5	 chi2= 101.937194	 time= 0.000742549	 cumTime= 0.00426532	 edges= 100	 schur= 0
iteration= 6	 chi2= 101.937020	 time= 0.000679622	 cumTime= 0.00494495	 edges= 100	 schur= 0
iteration= 7	 chi2= 101.937020	 time= 0.000690447	 cumTime= 0.00563539	 edges= 100	 schur= 0
iteration= 8	 chi2= 101.937020	 time= 0.000689469	 cumTime= 0.00632486	 edges= 100	 schur= 0
iteration= 9	 chi2= 101.937020	 time= 0.00068954	 cumTime= 0.0070144	 edges= 100	 schur= 0

其中chi2表示代价函数值。

YMWM_

关注

2
点赞
踩
5

收藏

觉得还不错? 一键收藏
打赏
0
评论
第6讲非线性优化（偏实践）

目录1 手写高斯牛顿法21 手写高斯牛顿法对于如下测量方程，y=eax2+bx+c+wy=e^{ax^2+bx+c}+wy=eax2+bx+c+w其中yyy表示对状态(a,b,c)T(a,b,c)^T(a,b,c)T的测量；www为噪声，它服从均值为0，方差为1的高斯分布。同时，有如下100个数据点，xdata[100]={0,0.01,0.02,…xi…，0.99}x_{data}[100] = \{0, 0.01, 0.02, …x_i…， 0.99 \}xdata[100]=
复制链接

扫一扫