视觉SLAM十四讲学习笔记——ch10 后端2

10.1理论部分

推荐参考博文推导:

1. SLAM十四讲-后端2-ch10-代码注释(位姿图优化)
2. SLAM14讲学习笔记（七）后端（BA与图优化，Pose Graph优化的理论与公式详解、因子图优化）

10.2实践部分

10.2.1 李代数上的位姿图优化

代码及详细注释如下:

#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

#include 

using namespace std;
using namespace Eigen;
using Sophus::SE3d;
using Sophus::SO3d;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿图，节点和边的方式为自定义
 * 利用 g2o对sphere.g2o文件进行优化 优化前 用g20——viewer显示为椭球
* 用g2o的话 需要定义顶点和边
* 位姿图优化就是只优化位姿 不优化路标点
* 顶点应该相机的位姿
* 边是相邻两个位姿的变换
* error误差是观测的相邻相机的位姿变换的逆 
* 待优化的相邻相机的位姿变换
* 我们希望这个误差接近I矩阵 给误差取ln后 误差接近 0 
* 该程序用李代数描述误差

* 这里把J矩阵的计算放在JRInv(const SE3d & e)函数里
* 这里的J矩阵还不是雅克比矩阵 具体雅克比见书上公式 p272页 公式10.9 10.10
* 李代数应该是向量形式 
* 李代数的hat 也就是李代数向量变为反对称矩阵

 * **********************************************/

typedef Matrix<double, 6, 6> Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(const SE3d &e) {
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
    J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
    // J = J * 0.5 + Matrix6d::Identity();
    J = Matrix6d::Identity();    // try Identity if you want
    return J;
}

// 李代数顶点
typedef Matrix<double, 6, 1> Vector6d;

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        //读取数据
    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3d(
            Quaterniond(data[6], data[3], data[4], data[5]),
            Vector3d(data[0], data[1], data[2])
        ));
    }
    //将优化的位姿存入内存 
    virtual bool write(ostream &os) const override {
        os << id() << " ";
        Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        //coeffs顺序是 x y z w ,w是实部
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
        return true;
    }

    virtual void setToOriginImpl() override {
        _estimate = SE3d();//李代数
    }

    // 左乘更新
    virtual void oplusImpl(const double *update) override {
        Vector6d upd;//六维向量 upd接收 update
        upd << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(upd) * _estimate;//更新位姿
    }
};

// 两个李代数节点之边
// 定义边  两个李代数顶点的边 边就是两个顶点之间的变换 即位姿之间的变换
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    //读取观测值和构造信息矩阵
    virtual bool read(istream &is) override {
        //这里观测值是位子之间的变换，当然包括旋转和平移 所以 data[]是7维 平移加四元数
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];//流入data[]
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();//归一化
        setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
        for (int i = 0; i < information().rows() && is.good(); i++)
            for (int j = i; j < information().cols() && is.good(); j++) {
                is >> information()(i, j);
                if (i != j)     //不是对角线的地方
                    information()(j, i) = information()(i, j);
            }
        return true;
    }
    //这个函数就是为了把优化好的相机位姿放进指定文件中去
    virtual bool write(ostream &os) const override {
        //v1,V2分别指向两个顶点
        VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]);
        VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";   //把两个定点的编号流入os
        SE3d m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();     //获取单位四元数
        //先传入平移  再传入四元数
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";

        // information matrix   信息矩阵
        for (int i = 0; i < information().rows(); i++)
            for (int j = i; j < information().cols(); j++) {
                
                os << information()(i, j) << " ";
            }
        os << endl;
        return true;
    }

    // 误差计算与书中推导一致
    virtual void computeError() override {
        //v1,V2分别指向两顶点的位姿
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        _error = (_measurement.inverse() * v1.inverse() * v2).log();
    }

    // 雅可比计算
    virtual void linearizeOplus() override {
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3d::exp(_error));          //计算d
        // 尝试把J近似为I
        //雅克比有两个,一个是误差对相机i位姿的雅克比,另一个是误差对相机j位姿的雅克比
        _jacobianOplusXi = -J * v2.inverse().Adj();
        _jacobianOplusXj = J * v2.inverse().Adj();
    }
};

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
        return 1;
    }
    //将sphere.g2o文件流入fin
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; //6,6是顶点和边的维度
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;   //线性求解
    //设置梯度下降的方法
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    //容器 vectices 和edges 存放各个顶点和边
    vector<VertexSE3LieAlgebra *> vectices;
    vector<EdgeSE3LieAlgebra *> edges;
    
    while (!fin.eof()) {
        string name;
        fin >> name;
        //将文件中的顶点数据流入,顶点就是各个相机的位姿
        if (name == "VERTEX_SE3:QUAT") {
            // 顶点
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);   //这里是setEstimate
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;            //顶点的ID
            e->setId(edgeCnt++);        ///设置边的ID
            //设置顶点
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);       //读取观测值
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if (!fin.good()) break;
    }
            //输出边的顶点的合的个数
    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();     //优化初始化
    optimizer.optimize(30);         //迭代次数

    cout << "saving optimization results ..." << endl;

    // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
    // 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
    ofstream fout("result_lie.g2o");
    for (VertexSE3LieAlgebra *v:vectices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);         //把优化的顶点放进 result_lie.g2o
    }
    for (EdgeSE3LieAlgebra *e:edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout); //把优化的边放进 result_lie.g2o
    }
    fout.close();
    return 0;
}

结果如下所示:

read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 674837160.579970	 time= 0.566391	 cumTime= 0.566391	 edges= 9799	 schur= 0	 lambda= 6658.554263	 levenbergIter= 1
iteration= 1	 chi2= 234706314.970484	 time= 0.506822	 cumTime= 1.07321	 edges= 9799	 schur= 0	 lambda= 2219.518088	 levenbergIter= 1
iteration= 2	 chi2= 142146174.348537	 time= 0.502332	 cumTime= 1.57554	 edges= 9799	 schur= 0	 lambda= 739.839363	 levenbergIter= 1
iteration= 3	 chi2= 83834595.145595	 time= 0.617319	 cumTime= 2.19286	 edges= 9799	 schur= 0	 lambda= 246.613121	 levenbergIter= 1
iteration= 4	 chi2= 41878079.903257	 time= 0.542277	 cumTime= 2.73514	 edges= 9799	 schur= 0	 lambda= 82.204374	 levenbergIter= 1
iteration= 5	 chi2= 16598628.119947	 time= 0.519183	 cumTime= 3.25432	 edges= 9799	 schur= 0	 lambda= 27.401458	 levenbergIter= 1
iteration= 6	 chi2= 6137666.739405	 time= 0.53891	 cumTime= 3.79323	 edges= 9799	 schur= 0	 lambda= 9.133819	 levenbergIter= 1
iteration= 7	 chi2= 2182986.250593	 time= 0.535928	 cumTime= 4.32916	 edges= 9799	 schur= 0	 lambda= 3.044606	 levenbergIter= 1
iteration= 8	 chi2= 732676.668220	 time= 0.477907	 cumTime= 4.80707	 edges= 9799	 schur= 0	 lambda= 1.014869	 levenbergIter= 1
iteration= 9	 chi2= 284457.115176	 time= 0.48001	 cumTime= 5.28708	 edges= 9799	 schur= 0	 lambda= 0.338290	 levenbergIter= 1
iteration= 10	 chi2= 170796.109734	 time= 0.497792	 cumTime= 5.78487	 edges= 9799	 schur= 0	 lambda= 0.181974	 levenbergIter= 1
iteration= 11	 chi2= 145466.315841	 time= 0.527085	 cumTime= 6.31196	 edges= 9799	 schur= 0	 lambda= 0.060658	 levenbergIter= 1
iteration= 12	 chi2= 142373.179500	 time= 0.546491	 cumTime= 6.85845	 edges= 9799	 schur= 0	 lambda= 0.020219	 levenbergIter= 1
iteration= 13	 chi2= 137485.756901	 time= 0.544264	 cumTime= 7.40271	 edges= 9799	 schur= 0	 lambda= 0.006740	 levenbergIter= 1
iteration= 14	 chi2= 131202.175668	 time= 0.484477	 cumTime= 7.88719	 edges= 9799	 schur= 0	 lambda= 0.002247	 levenbergIter= 1
iteration= 15	 chi2= 128006.202530	 time= 0.481649	 cumTime= 8.36884	 edges= 9799	 schur= 0	 lambda= 0.000749	 levenbergIter= 1
iteration= 16	 chi2= 127587.860945	 time= 0.707194	 cumTime= 9.07603	 edges= 9799	 schur= 0	 lambda= 0.000250	 levenbergIter= 1
iteration= 17	 chi2= 127578.599359	 time= 0.537201	 cumTime= 9.61323	 edges= 9799	 schur= 0	 lambda= 0.000083	 levenbergIter= 1
iteration= 18	 chi2= 127578.573853	 time= 0.476409	 cumTime= 10.0896	 edges= 9799	 schur= 0	 lambda= 0.000028	 levenbergIter= 1
iteration= 19	 chi2= 127578.573840	 time= 0.504015	 cumTime= 10.5937	 edges= 9799	 schur= 0	 lambda= 0.000018	 levenbergIter= 1
iteration= 20	 chi2= 127578.573840	 time= 0.488356	 cumTime= 11.082	 edges= 9799	 schur= 0	 lambda= 0.000012	 levenbergIter= 1
iteration= 21	 chi2= 127578.573840	 time= 0.486927	 cumTime= 11.5689	 edges= 9799	 schur= 0	 lambda= 0.000008	 levenbergIter= 1
iteration= 22	 chi2= 127578.573840	 time= 1.51253	 cumTime= 13.0815	 edges= 9799	 schur= 0	 lambda= 0.000044	 levenbergIter= 3
iteration= 23	 chi2= 127578.573840	 time= 1.47973	 cumTime= 14.5612	 edges= 9799	 schur= 0	 lambda= 0.000234	 levenbergIter= 3
iteration= 24	 chi2= 127578.573840	 time= 4.95243	 cumTime= 19.5136	 edges= 9799	 schur= 0	 lambda= 5483030743.383683	 levenbergIter= 10
saving optimization results ...

10.2.2 g2o原声位姿图优化

代码及详细注释如下:

#include 
#include 
#include 

#include 
#include 
#include 
#include 

using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

int main(int argc, char **argv) {
      //不用定义顶点和边
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    // 使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; //顶点6维，边6维
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // SE3 顶点
            g2o::VertexSE3 *v = new g2o::VertexSE3();
            int index = 0;
             fin>>index;//编号
            v->setId(index);//设置顶点编号
            v->read(fin);//读取边 就是setEstimate()
            optimizer.addVertex(v);//加入顶点
            vertexCnt++;//顶点个数++
            //设置是否固定，第一帧固定
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            g2o::EdgeSE3 *e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            //设置idx所对应的顶点
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);       //读取观测数据
            optimizer.addEdge(e);       //加入边
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();         //优化初始化
    optimizer.optimize(30);         //迭代次数

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");//保存优化后的文件

    return 0;
}

结果如下所示:

read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 1023011093.967642	 time= 0.422639	 cumTime= 0.422639	 edges= 9799 schur= 0	 lambda= 805.622433	 levenbergIter= 1
iteration= 1	 chi2= 385118688.233188	 time= 0.382178	 cumTime= 0.804817	 edges= 9799	 schur= 0	 lambda= 537.081622	 levenbergIter= 1
iteration= 2	 chi2= 166223726.693658	 time= 0.42988	 cumTime= 1.2347	 edges= 9799	 schur= 0	 lambda= 358.054415	 levenbergIter= 1
iteration= 3	 chi2= 86610874.269316	 time= 0.462892	 cumTime= 1.69759	 edges= 9799	 schur= 0	 lambda= 238.702943	 levenbergIter= 1
iteration= 4	 chi2= 40582782.710190	 time= 0.420185	 cumTime= 2.11777	 edges= 9799	 schur= 0	 lambda= 159.135295	 levenbergIter= 1
iteration= 5	 chi2= 15055383.753040	 time= 0.39671	 cumTime= 2.51448	 edges= 9799	 schur= 0	 lambda= 101.425210	 levenbergIter= 1
iteration= 6	 chi2= 6715193.487654	 time= 0.377734	 cumTime= 2.89222	 edges= 9799	 schur= 0	 lambda= 37.664667	 levenbergIter= 1
iteration= 7	 chi2= 2171949.168383	 time= 0.405097	 cumTime= 3.29732	 edges= 9799	 schur= 0	 lambda= 12.554889	 levenbergIter= 1
iteration= 8	 chi2= 740566.827049	 time= 0.370052	 cumTime= 3.66737	 edges= 9799	 schur= 0	 lambda= 4.184963	 levenbergIter= 1
iteration= 9	 chi2= 313641.802464	 time= 0.360452	 cumTime= 4.02782	 edges= 9799	 schur= 0	 lambda= 2.583432	 levenbergIter= 1
iteration= 10	 chi2= 82659.743578	 time= 0.367851	 cumTime= 4.39567	 edges= 9799	 schur= 0	 lambda= 0.861144	 levenbergIter= 1
iteration= 11	 chi2= 58220.369189	 time= 0.384526	 cumTime= 4.7802	 edges= 9799	 schur= 0	 lambda= 0.287048	 levenbergIter= 1
iteration= 12	 chi2= 52214.188561	 time= 0.36656	 cumTime= 5.14676	 edges= 9799	 schur= 0	 lambda= 0.095683	 levenbergIter= 1
iteration= 13	 chi2= 50948.580336	 time= 0.382879	 cumTime= 5.52963	 edges= 9799	 schur= 0	 lambda= 0.031894	 levenbergIter= 1
iteration= 14	 chi2= 50587.776729	 time= 0.3974	 cumTime= 5.92703	 edges= 9799	 schur= 0	 lambda= 0.016436	 levenbergIter= 1
iteration= 15	 chi2= 50233.038802	 time= 0.388748	 cumTime= 6.31578	 edges= 9799	 schur= 0	 lambda= 0.010957	 levenbergIter= 1
iteration= 16	 chi2= 49995.082836	 time= 0.41957	 cumTime= 6.73535	 edges= 9799	 schur= 0	 lambda= 0.007305	 levenbergIter= 1
iteration= 17	 chi2= 48876.738968	 time= 0.757956	 cumTime= 7.49331	 edges= 9799	 schur= 0	 lambda= 0.009298	 levenbergIter= 2
iteration= 18	 chi2= 48806.625520	 time= 0.373177	 cumTime= 7.86648	 edges= 9799	 schur= 0	 lambda= 0.006199	 levenbergIter= 1
iteration= 19	 chi2= 47790.891374	 time= 0.788389	 cumTime= 8.65487	 edges= 9799	 schur= 0	 lambda= 0.008265	 levenbergIter= 2
iteration= 20	 chi2= 47713.626578	 time= 0.427388	 cumTime= 9.08226	 edges= 9799	 schur= 0	 lambda= 0.005510	 levenbergIter= 1
iteration= 21	 chi2= 46869.323691	 time= 0.799491	 cumTime= 9.88175	 edges= 9799	 schur= 0	 lambda= 0.007347	 levenbergIter= 2
iteration= 22	 chi2= 46802.585509	 time= 0.393055	 cumTime= 10.2748	 edges= 9799	 schur= 0	 lambda= 0.004898	 levenbergIter= 1
iteration= 23	 chi2= 46128.758046	 time= 0.736299	 cumTime= 11.0111	 edges= 9799	 schur= 0	 lambda= 0.006489	 levenbergIter= 2
iteration= 24	 chi2= 46069.133544	 time= 0.389972	 cumTime= 11.4011	 edges= 9799	 schur= 0	 lambda= 0.004326	 levenbergIter= 1
iteration= 25	 chi2= 45553.862168	 time= 0.960659	 cumTime= 12.3617	 edges= 9799	 schur= 0	 lambda= 0.005595	 levenbergIter= 2
iteration= 26	 chi2= 45511.762622	 time= 0.480517	 cumTime= 12.8423	 edges= 9799	 schur= 0	 lambda= 0.003730	 levenbergIter= 1
iteration= 27	 chi2= 45122.763002	 time= 0.701183	 cumTime= 13.5434	 edges= 9799	 schur= 0	 lambda= 0.004690	 levenbergIter= 2
iteration= 28	 chi2= 45095.174401	 time= 0.426408	 cumTime= 13.9698	 edges= 9799	 schur= 0	 lambda= 0.003127	 levenbergIter= 1
iteration= 29	 chi2= 44811.248507	 time= 0.788535	 cumTime= 14.7584	 edges= 9799	 schur= 0	 lambda= 0.003785	 levenbergIter= 2
saving optimization results ...

利用g2o_viewer 显示结果如下:(注意文件路径)

g2o_viewer  sphere.g2o
g2o_viewer  result_lie.g2o

sphere.g2o
result_lie.g2o

调试遇到问题bug

本章调试遇到bug和第8章基本一致,此外还遇到fmt报错问题,都可以通过修改CmakeList调试通过
修改如下:set(CMAKE_CXX_FLAGS "-O3 -std=c++11")改为set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4"),在每一个target_link_libraries末尾加上 fmt.

cmake_minimum_required(VERSION 2.8)
project(pose_graph)

set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Eigen
include_directories("/usr/include/eigen3")

# sophus 
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(pose_graph_g2o_SE3 pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3
        g2o_core g2o_stuff g2o_types_slam3d ${CHOLMOD_LIBRARIES}  fmt
        )

add_executable(pose_graph_g2o_lie pose_graph_g2o_lie_algebra.cpp)
target_link_libraries(pose_graph_g2o_lie
        g2o_core g2o_stuff
        ${CHOLMOD_LIBRARIES}
        ${Sophus_LIBRARIES}  fmt
        )

参考博客

1. SLAM十四讲-后端2-ch10-代码注释(位姿图优化)
2. SLAM14讲学习笔记（七）后端（BA与图优化，Pose Graph优化的理论与公式详解、因子图优化）