位姿图优化代码

g2o原生位姿图

#include<iostream>
#include<fstream>
#include<string>

#include <g2o/types/slam3d/types_slam3d.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/optimization_algorithm_levenberg.h>
#include<g2o/solvers/eigen/linear_solver_eigen.h>

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
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    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);
              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++);
            e->setVertex(0, optimizer.vertices()[idx1]);//这里的0对应_vertices[0]即VertexSE3，这里面用的顶点类型相同，不同时对应的是第一个绑定的顶点类型
            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;
 }

李代数上的位姿图优化

#include<iostream>
#include<fstream>
#include<string>
#include<Eigen/Core>

#include<g2o/core/base_vertex.h>
#include<g2o/core/base_binary_edge.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include<sophus/se3.hpp>

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

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿图，节点和边的方式为自定义
 * **********************************************/

 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()<<" ";
              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[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{
              double data[7];
              for(int i=0;i<7;i++)
                     is>>data[i];
              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{
              VertexSE3LieAlgebra *v1=static_cast<VertexSE3LieAlgebra *>(_vertices[0]);
              VertexSE3LieAlgebra *v2=static_cast<VertexSE3LieAlgebra *>(_vertices[1]);

              os<<v1->id()<<" "<<v2->id()<<" ";
              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{
              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));
              //尝试把J近似为I？
              _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;
    }
    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;
    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; // 顶点和边的数量
    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);
            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;
            e->setId(edgeCnt++);
            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);
    }
    for (EdgeSE3LieAlgebra *e:edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pose_graph)

set(CMAKE_BUILD_TYPE "Release")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
list(APPEND CMAKE_MODULE_PATH /home/xxx/MyLibs/g2o-master/cmake_modules)
set(G2O_ROOT /usr/local/include/g2o)
set(G2O_LIBS /usr/local/include/g2o)

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

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
        )