0.3版本的VO就是在0.2的基础上增加了g2o优化:
在visual_odometry.cpp中的poseEstimationPNP()函数中,用PNP求出T_c_r_estimated_后,增加了g2o优化,对位姿进行优化。
当然同时增加的还有g2o相关的头文件和源文件:g2o_types.h和g2o_types.cpp
高博的g2o_types.h头文件中定义了三种类型的边,分别是:
EdgeProjectXYZRGBD
EdgeProjectXYZRGBDPoseOnly
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
传入一个3D点,返回r*p+t,很明显就是求变换后点的坐标。
在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()
);
...