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_r_estimated_.translation()
) );
optimizer.addVertex ( pose );
// 3. 边是重投影误差
for ( int i=0; i<inliers.rows; i++ )
{
int index = inliers.at<int>(i,0);
// 3D -> 2D 投影
EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
edge->setId(i);
edge->setVertex(0, pose);
//相机参数
edge->camera_ = curr_->camera_.get();
//3D点
edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );
//测量值是2维
edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );
edge->setInformation( Eigen::Matrix2d::Identity() );
optimizer.addEdge( edge );
}
// 4. 执行优化
optimizer.initializeOptimization();
optimizer.optimize(10);
T_c_r_estimated_ = SE3 (
pose->estimate().rotation(),
pose->estimate().translation()
);
}
目标是优化位姿T,最小化重投影误差。
高博的g2o_types.h头文件中定义了三种类型的边,分别是:
EdgeProjectXYZRGBD
EdgeProjectXYZRGBDPoseOnly
EdgeProjectXYZ2UVPoseOnly(重点介绍)
.h
#ifndef MYSLAM_G2O_TYPES_H
#define MYSLAM_G2O_TYPES_H
#include "myslam/common_include.h"
#include "camera.h"
#include <g2o/g2o.h>
namespace myslam
{
class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError();
virtual void linearizeOplus();
virtual bool read( std::istream& in ){}
virtual bool write( std::ostream& out) const {}
};
class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Error: measure = R*point+t
virtual void computeError();
virtual void linearizeOplus();
virtual bool read( std::istream& in ){}
virtual bool write( std::ostream& out) const {}
Vector3d point_;
};
//只优化位姿pose,没有优化点 3D—2D
class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//计算误差和线性增量函数(雅克比矩阵J)
virtual void computeError();
virtual void linearizeOplus();
virtual bool read( std::istream& in ){}
virtual bool write(std::ostream& os) const {};
// 3D点和相机模型为成员变量
Vector3d point_;
Camera* camera_;
};
}
#endif // MYSLAM_G2O_TYPES_H
.cpp
#include "myslam/g2o_types.h"
namespace myslam
//1. 计算重投影误差
void EdgeProjectXYZ2UVPoseOnly::computeError()
{
//1. 相机位姿为顶点
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
//2. 误差是测量值减去估计值,估计值T*p,
_error = _measurement - camera_->camera2pixel (
pose->estimate().map(point_) );
}
//2. 计算线性增量函数,雅克比矩阵J
void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
{
//顶点取出位姿
g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
//位姿构造四元数形式T
g2o::SE3Quat T ( pose->estimate() );
//变换后的3D坐标 T*p
Vector3d xyz_trans = T.map ( point_ );
//变幻后的3D点xyz坐标
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
// 雅克比矩阵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_;
}
}