SLAM本质剖析-Eigen

0. 前言

在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者已从Ceres作为开端，这一篇文章主要对Eigen函数库进行详细的阐述，来方便各位后续的开发。

1. Eigen示例

相较于Ceres而言，Eigen函数库相对较为简单，我们上一篇文章详细描述了Ceres的使用以及注意事项，由于Ceres能够使用ceres::AutoDiffCostFunction这一类的自动求导函数，相对而言更加轻松，所以Eigen更多的是做矩阵运算。这里我们给出上一篇文章最后的Ceres求解的Eigen版本。我们可以看到本质上差别不大，只是Eigen需要自己求解雅克比矩阵J，并在用GN构建增量方程后，使用ldlt求解线性方程HX=g。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ctime>

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

    // 开始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;
//-----------------用GN构建增量方程，HX=g---------------------------------//
        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            // double error = 0; // 填写计算error的表达式
            double error = yi-exp(ae * xi * xi + be * xi + ce);   // 第i个数据点的计算误差
            
            Vector3d J; // 雅可比矩阵,3x1
            J[0] = -xi*xi*exp(ae * xi * xi + be * xi + ce);  // de/da,函数求倒数，-df/da
            J[1] = -xi*exp(ae * xi * xi + be * xi + ce);;  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);;  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;

        //LDL^T Cholesky求解
        // clock_t time_stt2 = clock();
        dx = H.ldlt().solve(b);//Hx=b,,,H.ldlt().solve(b)
        // cout<<"LDL^T分解，耗时:\n"<<(clock()-time_stt2)/(double)
        //     CLOCKS_PER_SEC<<"ms"<<endl;
        cout<<"\n dx:"<<dx.transpose()<<endl;
        // return 0;//一写就死

	// end your code here

        if (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;
}

TIP：值得注意的是Eigen函数库是不存在动态链接库的，我们在CMakeList.txt编译时候只需要引入include即可。

include_directories(BEFORE /home/xxx/eigen3_3/complex_eigen/install/include/eigen3/)

2. Eigen模块和头文件归纳

…详情请参照古月居