视觉SLAM十四讲：最小二乘和非线性优化相关课后程序理解

1. 使用ceres进行曲线的拟合

#include 
#include 
#include 
#include 
#include 

using namespace std;

// 误差函数的计算模型定义(每一组观测数据的误差计算)，这里的模型是作为ceres的自动求导类的模板参数的，所以需要由一定的可是规定
struct CURVE_FITTING_COST{

    const double _x, _y;    // x,y数据

    // 结构体构建
    CURVE_FITTING_COST(double x, double y): _x(x), _y(y){}

    // 参差计算规则，即我们由观测模型演变过来的误差模型
    // 这里函数的输入和输出应该是ceres求解时的特性决定的，所以接口是死的，但是待估计状态变量的形式和误差计算是根据状态估计问题而不同的
    template 
    bool operator()(const T* const abc, T* residual) const {
        // const T* const abc: 指针指向的常量不能改变并且指针本身的值也不能改变； 函数后面的const：隐含了任意修改它所在的类的成员的操作都是不允许的
        // abc：待估计的状态； residual：每一个观测点对应的误差,residual的维度，对应输出的误差的维度，这里的误差的维度为1
        residual[0] = T(_y) - ceres::exp(abc[0]*T(_x)*T(_x) + abc[1]*T(_x) + abc[2]);   // y-exp(ax^2+bx+c)
        return true;
    }
};

int main() {

    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0};                // abc参数的估计值

    vector x_data, y_data;      // 数据

    cout<<"generating data."<(new CURVE_FITTING_COST(x_data[i],y_data[i])),
                nullptr,         // 核函数，这里不使用，为空
                abc              // 待估计参数（状态）
        );
    }

    // 配置求解器
    ceres::Solver::Options options;                     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;       // 增量方程如何求解(结算出状态增量的一个步骤)
    options.minimizer_progress_to_stdout = true;        // 输出到cout

    ceres::Solver::Summary summary;                     // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration time_used = chrono::duration_cast>(t2-t1);
    cout<<"solve time cost = "<

 

2. 使用g2o进行曲线拟合

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

// 抽象成的图模型的顶点类（待估计的状态），继承基础顶点类而来，需要指定基础顶点类相应的模板参数：待优化的变量的维度和数据类型
// 这里模板参数中的数据类型为 Eigen::Vector3d 和我们创建时候用的数组类型是不同的，说明这里的Eigen::Vector3d是类内部运算时候的数据类型，
// 同时也说明了，我们外部自己设定的状态变量的数据类型和类定义时为状态变量指定的数据类型可以是不同的，只要是能正确表达状态就行
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>{

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW         // 不用管这句话，大概就是和Eigen的矩阵对齐有关，这已经是Eigen运算时候的一些优化了

    // 这里设置成虚拟函数，明显就是为了重置了（C++多态的性质），这里的_esitimate变量可以确定就是基础顶点类中的公有成员变量了，
    // 这个公有的成员变量的类型就明显由模板参数中的待优化变量的数据类型确定，表达了待估计的状态变量
    virtual void setToOriginImpl(){
        // 设置初始状态
        _estimate<<0,0,0;
    }

    // 同理，这里是对状态变量更新操作的重置，由我们实际问题中状态更新的规则确定
    virtual void oplusImpl(const double* update){
        _estimate += Eigen::Vector3d(update);       // 这里状态的更新就是简单的个维度叠加增量（update），注意，Eigen::Vector3d的初始化是可以由数组完成的，只要数据长度队的上就可
    }

    // 存盘和读盘：留空
    virtual bool read(istream& in){}
    virtual bool write(ostream& out) const {}
};


// 误差模型，继承自基础的单元边，需要指定基础单元边的误差维度和数据类型，以及要链接的顶点类型
// 每一条边代表一个观测点
class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>{

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

    // 构造函数
    CurveFittingEdge(double x): BaseUnaryEdge(), _x(x) {}

    // 误差定义(这个肯定是要自己写的，这里的误差求解涉及到具体的求解场景)
    void computeError(){
        // 取得与边链接的顶点（这里由于边是一元边，所以顶点就只有一个，即状态只有一个，所以  _vertices 只有一个元素，是g2o::BaseUnaryEdge的公有成员变量）
        const CurveFittingVertex* v = static_cast(_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();      // 得到顶点的状态值
        // 曲线场景下的误差计算公式， _measurement对应y值，这里的 _error 应该是BaseUnaryEdge的公有成员变量，猜测是一个向量，这里观测误差只有一维，所以只需要计算向量的第一位
        _error(0,0) = _measurement - std::exp(abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0));
    }

    // 着下面两个函数猜不出来有什么名堂,重载了一下，但是猜不出来怎么用
    virtual bool read(istream& in){}
    virtual bool write(ostream& out) const {}
};


int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;     // 真实参数值
    int N=100;                      // 数据点
    double w_sigma=1.0;             // 噪声Sigma值
    cv::RNG rng;                    // OpenCV随机数产生器
    double abc[3]={0};              // abc参数的估计值(待估计状态)

    vector x_data, y_data;  // 观测数据

    cout<<"generating data: "<> Block;
    // 指定线性方程求解器（即增量方程求解器）
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
    // 生成矩阵块求解器
    Block* solver_ptr = new Block(linearSolver);

    // 为块求解器选定具体的梯度下降优化方法
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    // 图优化器
    g2o::SparseOptimizer optimizer;
    // 设置优化器
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);     // 打开调试输出


    // 往构建的图中添加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(0,0,0));     // 设定顶点的初始状态，这里状态的数据类型是Vector3d，恰好就是基础顶点模板参数中设定的状态类型
    v->setId(0);    // 设定当前顶点的id，这里只有一个顶点所以就只设置0好了
    optimizer.addVertex(v);     // 向构建的图中加入设置好的顶点


    // 往构建的图中添加边（有多少观测就添加多少边）
    for(int i=0; isetId(0);
        edge->setVertex(0, v);              // 设置连接的顶点，这里面0表示顶点v的id或者说是索引值
        edge->setMeasurement(y_data[i]);    // 设置每一条边的观测数值
        edge->setInformation(Eigen::Matrix::Identity()*1/(w_sigma*w_sigma));        // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge(edge);
    }


    // 执行优化，得到利用最小二乘法优化得到的待估计状态
    cout<<"start optimization!"< time_used = chrono::duration_cast>(t2-t1);
    cout<<"Solve time cost = "<estimate();
    cout<<"estimated model: "<

 

3. 高斯牛顿法的曲线拟合（底层实现）

//
// Created by g214-j on 18-8-6.
//

//
// Created by 高翔 on 2017/12/15.
//
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    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;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector 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));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0;   // 第i个数据点的计算误差
            // 填写计算error的表达式(每一个观测的观测误差)
            error = yi - exp(ae * xi * xi + be * xi + ce);

            Vector3d J; // 雅可比矩阵（误差关于状态变量的导数）
            double tmp = exp(ae * xi * xi + be * xi + ce);
            J[0] = - tmp * pow(xi,2);  // de/da
            J[1] = - tmp * xi;  // de/db
            J[2] = - tmp;  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;        // 偏差，H*dx=g,g就是这里的b
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
        // start your code here
        Vector3d dx;
        dx = H.ldlt().solve(b);
        // end your code here

        if (std::isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值，状态更新
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}