《视觉SLAM十四讲精品总结》10.3: 添加g2o优化位姿T

最新推荐文章于 2023-03-21 16:53:26 发布

try_again_later

最新推荐文章于 2023-03-21 16:53:26 发布

阅读量1.4k

点赞数

分类专栏：视觉、激光SLAM

本文链接：https://blog.csdn.net/try_again_later/article/details/82221244

版权

本文介绍了在视觉里程计（VO）中如何使用g2o进行位姿优化。在visual_odometry.cpp的poseEstimationPNP()函数中，通过PNP算法获取初步位姿后，利用g2o对位姿进行优化，以减小重投影误差。同时，文章提及了g2o_types.h和g2o_types.cpp的新增内容，包括EdgeProjectXYZRGBD、EdgeProjectXYZRGBDPoseOnly和EdgeProjectXYZ2UVPoseOnly三种边类型，重点讨论了EdgeProjectXYZ2UVPoseOnly在优化过程中的作用。

摘要由CSDN通过智能技术生成

0.3版本的VO就是在0.2的基础上增加了g2o优化:
1、在visual_odometry.cpp中的poseEstimationPNP()函数中，用PNP求出T_c_r_estimated_后，增加了g2o优化，对位姿进行优化。
2、同时增加的还有g2o相关的头文件和源文件：g2o_types.h和g2o_types.cpp

visual_odometry.cpp

    //使用BA优化估计的位姿T
	//1. 位姿6维，观测点2维
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    //2. 顶点是相机位姿pose
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
        T_c_r_estimated_.rotation_matrix(), 
        T_c_