【SLAM】g2o使用步骤

不进前十不改名

于 2023-05-19 17:59:40 发布

阅读量323

点赞数

分类专栏： slam ROS 文章标签：算法 c++ 开发语言人工智能机器人

本文链接：https://blog.csdn.net/weixin_44764376/article/details/130770247

版权

slam 同时被 2 个专栏收录

12 篇文章 0 订阅

订阅专栏

ROS

3 篇文章 0 订阅

订阅专栏

目标

用g2o 实现一个PnP的重投影误差估计当前帧的位姿代码。

g2o框架

在这里插入图片描述

实现思路与过程

1. 定义块求解器类型：

求解位姿是6维（so3,t）,残差维度是2维。

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;

2. 确定线性方程分解形式（LinearSolver）：

在迭代时候需要求解线性方程 $H\delta t = b$ ，这里确定求解方式。（PCG，Cholmod等都是线性方程，他们之间的差异在于求解求解速度和精度）

Block::LinearSolverType* LinearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

3. 确定块求解器：

将线性方程的形式放入到块求解器中。

Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSolver));

4. 确定优化器迭代求解的算法：

确定迭代的方式，这里使用的是LM法，然后将块求解器放入其中。

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));

5. 优化器和设置求解算法：

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );

6. 定义顶点

顶点是需要求解的对象，本文中需要求解的对象是帧位姿

	// 6：求解对象的维度；Eigen::Matrix<double,6,1>：存放估计值的类型
	class PoseVertex : public g2o::BaseVertex<6,Eigen::Matrix<double,6,1>>{
    public:
    	// 宏定义 为EIGEN自定义一个分配器，使用EIGEN的类基本上都需要添加
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        // 需要重写4个函数：setToOriginImpl(),oplusImpl(),read(),write()
        
        // 设置初始状态
        void setToOriginImpl() override{
            _estimate.setZero();
        }
		// 定义优化值的更新方式
        void oplusImpl(const double* update) override{
			// 这里是旋转量的更新
			//1、李代数转变成旋转矩阵之后再相乘，最后变回李代数
            Eigen::Vector3d old_se3(_estimate[0],_estimate[1],_estimate[2]);
            Sophus::SO3d R = Sophus::SO3d::exp(old_se3);// 使用SO3时候要定义类型SO3d
            Eigen::Vector3d upd_se3(update[0],update[1],update[2]);
            Sophus::SO3d upd_R = Sophus::SO3d::exp(upd_se3);
            R *= upd_R;
            Eigen::Vector3d se3 = R.log();
            _estimate[0] = se3[0];
            _estimate[1] = se3[1];
            _estimate[2] = se3[2];
			//2、位移量的优化
            Eigen::Vector3d old_t(_estimate[3],_estimate[4],_estimate[5]);
            Eigen::Vector3d t_upd(update[3],update[4],update[5]);
			// 这里不确定，公式有没有问题
            old_t +=  R*t_upd;
            _estimate[3] = old_t[0];
            _estimate[4] = old_t[1];
            _estimate[5] = old_t[2];
        }
		// 读写操作（不确定作用）
        virtual bool read(std::istream& is) {return true;}
        virtual bool write(std::ostream& os) const { return true;}
    };

定义完顶点之后需要在BA函数中创建：

   	PoseVertex* vertex = new PoseVertex();
    // 这里使用(0)初始化的话，会到时resize错误
    vertex->setEstimate(Eigen::Matrix<double,6,1>::Zero());
    vertex->setId(0);
    optimizer.addVertex(vertex);

7. 边定义与创建

边表示的是最小二乘中的残差值，也就是约束值。
本文中的例子优化的对象只有一个，所以是一元边。同理，如果待优化的对象是两个就是二元边。

	// BaseUnaryEdge 表示一元边 
	// 2 表示残差维度；Eigen::Vector2d 表示容器；PoseVertex顶点类型
    class EdgeProjectPoseOnly : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,PoseVertex>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        // 计算残差值
        void computeError() override {
            // _vertices 是用通用容器来装vertex，并没有estimate这个函数的。需要重新转为自定义的类型
            Eigen::Matrix<double,6,1> pose = static_cast<const PoseVertex*>(_vertices[0])->estimate();
            Eigen::Vector3d p = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])) * point + Eigen::Vector3d(pose[3],pose[4],pose[5]);
            Eigen::Vector2d proj(fx*p[0]/p[2] + cx,fy*p[1]/p[2] + cy);
            _error = _measurement - proj;
        }
        // 设置雅可比
        void linearizeOplus() override{
            Eigen::Matrix<double,6,1> pose  = static_cast<const PoseVertex*>( _vertices[0])->estimate();
            Eigen::Vector3d p = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])) * point + Eigen::Vector3d(pose[3],pose[4],pose[5]);
            double x = p[0];
            double y = p[1];
            double z = p[2];
            //  雅可比矩阵 【se3，t】
            // | 0 1 2 3 4  5  |
            // | 6 7 8 9 10 11 |
            //  顺序写法是错的，需要指定位置
            _jacobianOplusXi(0,0) = fx*x*y / z*z;
            _jacobianOplusXi(0,1) = -fx - fx*x*x/z*z;
            _jacobianOplusXi(0,2) = fx*y / z;
            _jacobianOplusXi(0,3) = -fx/z;
            _jacobianOplusXi(0,4) = 0;
            _jacobianOplusXi(0,5) = fx*x/z*z;

            _jacobianOplusXi(1,0) = fy + fy*y*y/z*z;
            _jacobianOplusXi(1,1) = -fy*x*y/z*z;
            _jacobianOplusXi(1,2) = -fy*x /z;
            _jacobianOplusXi(1,3) = 0;
            _jacobianOplusXi(1,4) = -fy/z;
            _jacobianOplusXi(1,5) = fy*y/z*z;
        }
		// 与上面一样
        bool read(istream& is) override { return true; }
        bool write(ostream& os) const override { return true; }
    public:
    	//	因为需要用内参和对应点的空间坐标，所以也需要在边中预先定义
        double fx,fy;
        double cx,cy;
        Eigen::Vector3d point;
    };

创建边，连接到顶点上，并添加到优化器中。

    for(int i=0;i<points_3d.size();i++){
        EdgeProjectPoseOnly* e = new EdgeProjectPoseOnly();
        e->setId(i);
        // 设置顶点，0表示边0号顶点（1号就为1）；optimizer.vertex(0)取优化器中的id：0顶点
        e->setVertex(0,optimizer.vertex(0));
        // 特征点值
        e->setMeasurement(Eigen::Vector2d(points_2d[i].x,points_2d[i].y));
        // 信息矩阵，因为雅可比是2*6，优化变量是6维，所以是2维
        e->setInformation(Eigen::Matrix2d::Identity());
        // 设置核函数
        g2o::RobustKernelHuber * rk = new g2o::RobustKernelHuber();
        e->setRobustKernel(rk);
        rk->setDelta(sqrt(5.991));
        // 定义相关参数
        //  at需要添加提取的类型
        e->fx = K.at<double>(0,0);
        e->fy = K.at<double>(1,1);
        e->cx = K.at<double>(0,2);
        e->cy = K.at<double>(1,2);
        e->point = Eigen::Vector3d(points_3d[i].x,points_3d[i].y,points_3d[i].z);
        optimizer.addEdge(e);
    }

8. 其他设置及确定迭代次数

	// 什么作用？输出信息
    optimizer.setVerbose( true );
    // 初始化
    optimizer.initializeOptimization();
    // 开始优化 10 迭代次数
    optimizer.optimize(10);

9. 获取优化结果

// 转换为变换矩阵
Eigen::Matrix<double,4,4> getTranslation(Eigen::Matrix<double,6,1> pose){
    // 将SO3赋值给Matrix 需要进行 .martix()操作
    Eigen::Matrix<double,3,3> R = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])).matrix();
    Eigen::Vector3d t(pose[3],pose[4],pose[5]);
    Eigen::Matrix<double,4,4> T = Eigen::Matrix<double,4,4>::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = t;
    return T;
}
// 结果在vertex 的estimate（）中
cout<<"T="<<endl<<getTranslation(vertex->estimate())<<endl;

代码总览

#include <vector>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#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/solvers/csparse/linear_solver_csparse.h>
#include "g2o/core/robust_kernel.h"
#include "g2o/core/robust_kernel_impl.h"
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <sophus/so3.hpp>
using namespace Eigen;

using namespace cv;
using namespace std;


string p3d_file = "../p3d.txt"; //需要自己修改路径
string p2d_file = "../p2d.txt"; //需要自己修改路径

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K );
    class PoseVertex : public g2o::BaseVertex<6,Eigen::Matrix<double,6,1>>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        
        // 设置初始状态
        void setToOriginImpl() override{
            _estimate.setZero();
        }

        void oplusImpl(const double* update) override{
            // Eigen::Matrix<double,6,1> upd;
            Eigen::Vector3d old_se3(_estimate[0],_estimate[1],_estimate[2]);
            Sophus::SO3d R = Sophus::SO3d::exp(old_se3);// SO3 需要tenplate 定义类型
            Eigen::Vector3d upd_se3(update[0],update[1],update[2]);
            Sophus::SO3d upd_R = Sophus::SO3d::exp(upd_se3);
            R *= upd_R;
            Eigen::Vector3d se3 = R.log();
            _estimate[0] = se3[0];
            _estimate[1] = se3[1];
            _estimate[2] = se3[2];

            Eigen::Vector3d old_t(_estimate[3],_estimate[4],_estimate[5]);
            Eigen::Vector3d t_upd(update[3],update[4],update[5]);
            old_t +=  R.inverse()*t_upd;
            _estimate[3] = old_t[0];
            _estimate[4] = old_t[1];
            _estimate[5] = old_t[2];
        }

        virtual bool read(std::istream& is) {return true;}
        virtual bool write(std::ostream& os) const { return true;}
    };   
    class EdgeProjectPoseOnly : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,PoseVertex>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        // 残差值
        void computeError() override {
            // _vertices 是用通用容器来装vertex，并没有estimate这个函数的。需要重新转为自定义的类型
            Eigen::Matrix<double,6,1> pose = static_cast<const PoseVertex*>(_vertices[0])->estimate();
            Eigen::Vector3d p = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])) * point + Eigen::Vector3d(pose[3],pose[4],pose[5]);
            Eigen::Vector2d proj(fx*p[0]/p[2] + cx,fy*p[1]/p[2] + cy);
            _error = _measurement - proj;
        }
        // 设置雅可比
        void linearizeOplus() override{
            Eigen::Matrix<double,6,1> pose  = static_cast<const PoseVertex*>( _vertices[0])->estimate();
            Eigen::Vector3d p = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])) * point + Eigen::Vector3d(pose[3],pose[4],pose[5]);
            double x = p[0];
            double y = p[1];
            double z = p[2];
            //  雅可比矩阵 【se3，t】
            // | 0 1 2 3 4  5  |
            // | 6 7 8 9 10 11 |
            //  顺序写法是错的，需要指定位置
            _jacobianOplusXi(0,0) = fx*x*y / z*z;
            _jacobianOplusXi(0,1) = -fx - fx*x*x/z*z;
            _jacobianOplusXi(0,2) = fx*y / z;
            _jacobianOplusXi(0,3) = -fx/z;
            _jacobianOplusXi(0,4) = 0;
            _jacobianOplusXi(0,5) = fx*x/z*z;

            _jacobianOplusXi(1,0) = fy + fy*y*y/z*z;
            _jacobianOplusXi(1,1) = -fy*x*y/z*z;
            _jacobianOplusXi(1,2) = -fy*x /z;
            _jacobianOplusXi(1,3) = 0;
            _jacobianOplusXi(1,4) = -fy/z;
            _jacobianOplusXi(1,5) = fy*y/z*z;
        }

        bool read(istream& is) override { return true; }
        bool write(ostream& os) const override { return true; }
    public:
        double fx,fy;
        double cx,cy;
        Eigen::Vector3d point;
    };

int main(int argc, char **argv) {


    vector< Point3f > p3d;
    vector< Point2f > p2d;

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K );
    return 0;
}

Eigen::Matrix<double,4,4> getTranslation(Eigen::Matrix<double,6,1> pose){
    // 将SO3赋值给Matrix 需要进行 .martix()操作
    Eigen::Matrix<double,3,3> R = Sophus::SO3d::exp(Eigen::Vector3d(pose[0],pose[1],pose[2])).matrix();
    Eigen::Vector3d t(pose[3],pose[4],pose[5]);
    Eigen::Matrix<double,4,4> T = Eigen::Matrix<double,4,4>::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = t;
    return T;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{

    // typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // // 第1步：创建一个线性求解器LinearSolver
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    // Block* solver_ptr = new Block (  std::unique_ptr<Block::LinearSolverType>(linearSolver) );

    // // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );

    // // 第4步：创建稀疏优化器
    // g2o::SparseOptimizer optimizer;
    // optimizer.setAlgorithm ( solver );

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* LinearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
    // 下面两个一定要unique_ptr？？为什么？
    Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSolver));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
    //  6维 (se3，t)
    
    // 第5步：定义图的顶点（需要补充代码）
	// ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵
    PoseVertex* vertex = new PoseVertex();
    // 这里使用(0)初始化的话，会到时resize错误
    vertex->setEstimate(Eigen::Matrix<double,6,1>::Zero());
    vertex->setId(0);
    optimizer.addVertex(vertex);

    for(int i=0;i<points_3d.size();i++){
        EdgeProjectPoseOnly* e = new EdgeProjectPoseOnly();
        e->setId(i);
        e->setVertex(0,optimizer.vertex(0));
        e->setMeasurement(Eigen::Vector2d(points_2d[i].x,points_2d[i].y));
        e->setInformation(Eigen::Matrix2d::Identity());
        g2o::RobustKernelHuber * rk = new g2o::RobustKernelHuber();
        e->setRobustKernel(rk);
        rk->setDelta(sqrt(5.991));
        //  at需要添加提取的类型
        e->fx = K.at<double>(0,0);
        e->fy = K.at<double>(1,1);
        e->cx = K.at<double>(0,2);
        e->cy = K.at<double>(1,2);
        e->point = Eigen::Vector3d(points_3d[i].x,points_3d[i].y,points_3d[i].z);
        optimizer.addEdge(e);
    }
    optimizer.setVerbose( true );// 什么作用？输出信息
    optimizer.initializeOptimization();
    optimizer.optimize(10);
 
	// // ----------------------结束你的代码

    // // 第6步：设置相机内参
    // g2o::CameraParameters* camera = new g2o::CameraParameters (
    //         K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    // camera->setId ( 0 );
    // optimizer.addParameter ( camera );

    // // 第7步：设置边（需要补充代码）
    // // ----------------------开始你的代码：设置并添加边




    // // ----------------------结束你的代码

    // // 第8步：设置优化参数，开始执行优化
    // optimizer.setVerbose ( true );
    // optimizer.initializeOptimization();
    // optimizer.optimize ( 100 );

    // // 输出优化结果
    // cout<<endl<<"after optimization:"<<endl;

    cout<<"T="<<endl<<getTranslation(vertex->estimate())<<endl;
}