第九讲 0.3

当然同时增加的还有g2o相关的头文件和源文件：g2o_types.h和g2o_types.cpp

EdgeProjectXYZ2UVPoseOnly

0.3版本只用到了第三种，也就是3D2D的重投影误差，所以只说一下第三种，其他两种也差不多：

#ifndef MYSLAM_G2O_TYPES_H #define MYSLAM_G2O_TYPES_H  #include "myslam/common_include.h" #include "camera.h"  #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/types/sba/types_six_dof_expmap.h> #include <g2o/solvers/dense/linear_solver_dense.h> #include <g2o/core/robust_kernel.h> #include <g2o/core/robust_kernel_impl.h>  namespace myslam { class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap> {...};  // only to optimize the pose, no point class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap > {...};  class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap > { public:     EIGEN_MAKE_ALIGNED_OPERATOR_NEW      //还是边类型定义中最核心的老两样：     //误差计算函数，实现误差计算方法     virtual void computeError();     //线性增量函数，也就是雅克比矩阵J的计算方法     virtual void linearizeOplus();      //读写功能函数，这里没用到，所以只是定义了，并没有在源文件中实现。     virtual bool read( std::istream& in ){}     virtual bool write(std::ostream& os) const {};      //把三维点和相机模型写成员变量，方便误差计算和J计算，因为都需要这两项数据     Vector3d point_;     Camera* camera_; };  }  #endif // MYSLAM_G2O_TYPES_H

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47

g2o_types.cpp中实现了头文件中定义的函数，也是对应的有三种，这里只说一下用到的第三种：

#include "myslam/g2o_types.h"  namespace myslam { //省略的前两种对于误差计算和J函数的实现。 ... //第三种，重投影误差 void EdgeProjectXYZ2UVPoseOnly::computeError() {     //顶点数组中取出顶点，转换成位姿指针类型，其实左边的pose类型可以写为auto     const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );     //误差计算，测量值减去估计值，也就是重投影误差     //估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。     _error = _measurement - camera_->camera2pixel(pose->estimate().map(point_) ); }  void EdgeProjectXYZ2UVPoseOnly::linearizeOplus() {     /**      * 这里说一下整体思路：      * 重投影误差的雅克比矩阵在书中P164页式7.45已经呈现，所以这里就是直接构造，      * 构造时发现需要变换后的空间点坐标，所以需要先求出。      */      //首先还是从顶点取出位姿     g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );     //这由位姿构造一个四元数形式T     g2o::SE3Quat T ( pose->estimate() );     //用T求得变换后的3D点坐标。T*p     Vector3d xyz_trans = T.map ( point_ );     //OK，到这步，变换后的3D点xyz坐标就分别求出来了，后面的z平方，纯粹是为了后面构造J时方便定义的，因为需要多处用到     double x = xyz_trans[0];     double y = xyz_trans[1];     double z = xyz_trans[2];     double z_2 = z*z;      //直接各个元素构造J就好了，对照式7.45是一模一样的，2*6的矩阵。     _jacobianOplusXi ( 0,0 ) =  x*y/z_2 *camera_->fx_;     _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;     _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;     _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;     _jacobianOplusXi ( 0,4 ) = 0;     _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;      _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;     _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;     _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;     _jacobianOplusXi ( 1,3 ) = 0;     _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;     _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_; } }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52

第一，顶点是顶点，位姿是位姿。虽然上面的顶点表示的就是位姿，但是，两种是不同的类型，所以使用时要进行类型转换：

g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );

• 1

取出顶点，将其强制类型转换为位姿类型。

第二是误差计算时的那句：

_error = _measurement - camera_->camera2pixel(pose->estimate().map(point_) );

• 1

里面有个map()函数。

看一下这个函数的源码：

      Vector3D map(const Vector3D & xyz) const       {         return _r*xyz + _t;       }

• 1
• 2
• 3
• 4

在g2o中，用SE3Quat类型表示变换T，此类型中有个成员函数就是map()，作用为对一个3D点进行坐标变换，例如：pose.map(a_point),就求得了变换后的坐标（看起来比较别扭而已，不如*直观）。

最后看一下poseEstimationPNP()函数中，用PNP求出T_c_r_estimated_后，增加的g2o部分，很常规，就是初始化，增加顶点和边，然后开始优化，最后用优化的结果对T_c_r_estimated_更新。

...     // using bundle adjustment to optimize the pose      //初始化，都是老套路，注意由于更新所需要的unique指针问题。     typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;     Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();     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 );      //添加顶点，一帧只有一个位姿，也就是只有一个顶点     g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();     pose->setId ( 0 );     pose->setEstimate ( g2o::SE3Quat (         T_c_r_estimated_.rotation_matrix(),          T_c_r_estimated_.translation()     ) );     optimizer.addVertex ( pose );      // edges边有许多，每个特征点都对应一个重投影误差，也就有一个边。     for ( int i=0; i<inliers.rows; i++ )     {         int index = inliers.at<int>(i,0);         // 3D -> 2D projection         EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();         edge->setId(i);         edge->setVertex(0, pose);         edge->camera_ = curr_->camera_.get();         edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );         edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );         edge->setInformation( Eigen::Matrix2d::Identity() );         optimizer.addEdge( edge );     }      //开始优化     optimizer.initializeOptimization();     //设置迭代次数     optimizer.optimize(10);      //这步就是将优化后的结果，赋值给T_c_r_estimated_     T_c_r_estimated_ = SE3 (         pose->estimate().rotation(),         pose->estimate().translation()     );     ...

文章来源: 第九讲 0.3