之前用的是RANSAC的PnP求解，opencv一行命令就搞定了。
但是容易受到噪声影响。除了可以光流法搞一下（如同第二版），但是maybe用g2o会更好
因此这里加入g2o优化，以ransac的pnp结果为图优化的初始值
具体为3d-2d的BA，单元边
g2o_type.h
（1）定义了三种误差边
（i）3d-3d的二元边
（ii）3d-3d的一元边
（iii）3d-2d的一元边
（2）然后注释掉计算雅克比的函数：因为计算雅克比矩阵是个费力不讨好的活。。。。。。
这样，g2o直接提供一个数值计算的雅克比函数
（3）在所有虚继承的函数中加上override，比如 virtual void computeError() override;
以作个提醒。
#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> // 用3d-3d的g2o二元边优化
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError() override;
// virtual void linearizeOplus() override; // 喜欢提供数值计算的雅克比函数，所以果断注释
virtual bool read( std::istream& in ) override{}
virtual bool write( std::ostream& out) const override {}
};
// only to optimize the pose, no point
class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap > // 用3d-3d的g2o优化
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Error: measure = R*point+t
virtual void computeError() override; // 计算误差
// virtual void linearizeOplus() override; // 计算雅克比矩阵
virtual bool read( std::istream& in ) override{}
virtual bool write( std::ostream& out) const override{}
Vector3d point_;
};
class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap > // 3d-2d的g2o优化
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void computeError() override;
// virtual void linearizeOplus() override;
virtual bool read( std::istream& in ) override{}
virtual bool write(std::ostream& os) const override{};
Vector3d point_;
Camera* camera_; // 涉及内参的，相机内部的变换，因此需要有个成员变量为相机类
};
}
#endif // MYSLAM_G2O_TYPES_H
g2o_type.cpp
（1）这里需要修改一下g2o的初始化方式（因为g2o版本迭代的问题，第一版代码会有一些莫名其妙的bug）
（2）然后我将计算雅克比的函数实现注释掉
#include "myslam/g2o_types.h"
namespace myslam
{
void EdgeProjectXYZRGBD::computeError() // 二元边，所以两个顶点。// 误差为3d向量
{
const g2o::VertexSBAPointXYZ* point = static_cast<const g2o::VertexSBAPointXYZ*> ( _vertices[0] ); // 点是左系的点
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[1] );
_error = _measurement - pose->estimate().map ( point->estimate() ); // pose目测是左系转到右系Tcr
}
// void EdgeProjectXYZRGBD::linearizeOplus()
// {
// g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *> ( _vertices[1] );
// g2o::SE3Quat T ( pose->estimate() ); // 这个初始化方法值得探究
// g2o::VertexSBAPointXYZ* point = static_cast<g2o::VertexSBAPointXYZ*> ( _vertices[0] );
// Eigen::Vector3d xyz = point->estimate();
// Eigen::Vector3d xyz_trans = T.map ( xyz ); // 右系的投影点
// double x = xyz_trans[0];
// double y = xyz_trans[1];
// double z = xyz_trans[2];
//
// _jacobianOplusXi = - T.rotation().toRotationMatrix();
//
// _jacobianOplusXj ( 0,0 ) = 0;
// _jacobianOplusXj ( 0,1 ) = -z;
// _jacobianOplusXj ( 0,2 ) = y;
// _jacobianOplusXj ( 0,3 ) = -1;
// _jacobianOplusXj ( 0,4 ) = 0;
// _jacobianOplusXj ( 0,5 ) = 0;
//
// _jacobianOplusXj ( 1,0 ) = z;
// _jacobianOplusXj ( 1,1 ) = 0;
// _jacobianOplusXj ( 1,2 ) = -x;
// _jacobianOplusXj ( 1,3 ) = 0;
// _jacobianOplusXj ( 1,4 ) = -1;
// _jacobianOplusXj ( 1,5 ) = 0;
//
// _jacobianOplusXj ( 2,0 ) = -y;
// _jacobianOplusXj ( 2,1 ) = x;
// _jacobianOplusXj ( 2,2 ) = 0;
// _jacobianOplusXj ( 2,3 ) = 0;
// _jacobianOplusXj ( 2,4 ) = 0;
// _jacobianOplusXj ( 2,5 ) = -1;
// }
void EdgeProjectXYZRGBDPoseOnly::computeError() // 误差为3d向量
{
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
_error = _measurement - pose->estimate().map ( point_ );
}
// void EdgeProjectXYZRGBDPoseOnly::linearizeOplus()
// {
// g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
// g2o::SE3Quat T ( pose->estimate() );
// Vector3d xyz_trans = T.map ( point_ );
// double x = xyz_trans[0];
// double y = xyz_trans[1];
// double z = xyz_trans[2];
//
// _jacobianOplusXi ( 0,0 ) = 0;
// _jacobianOplusXi ( 0,1 ) = -z;
// _jacobianOplusXi ( 0,2 ) = y;
// _jacobianOplusXi ( 0,3 ) = -1;
// _jacobianOplusXi ( 0,4 ) = 0;
// _jacobianOplusXi ( 0,5 ) = 0;
//
// _jacobianOplusXi ( 1,0 ) = z;
// _jacobianOplusXi ( 1,1 ) = 0;
// _jacobianOplusXi ( 1,2 ) = -x;
// _jacobianOplusXi ( 1,3 ) = 0;
// _jacobianOplusXi ( 1,4 ) = -1;
// _jacobianOplusXi ( 1,5 ) = 0;
//
// _jacobianOplusXi ( 2,0 ) = -y;
// _jacobianOplusXi ( 2,1 ) = x;
// _jacobianOplusXi ( 2,2 ) = 0;
// _jacobianOplusXi ( 2,3 ) = 0;
// _jacobianOplusXi ( 2,4 ) = 0;
// _jacobianOplusXi ( 2,5 ) = -1;
// }
void EdgeProjectXYZ2UVPoseOnly::computeError() // 误差为2d向量
{
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
_error = _measurement - camera_->camera2pixel (
pose->estimate().map(point_) );
}
// void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
// {
// g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
// g2o::SE3Quat T ( pose->estimate() );
// Vector3d xyz_trans = T.map ( point_ );
// double x = xyz_trans[0];
// double y = xyz_trans[1];
// double z = xyz_trans[2];
// double z_2 = z*z;
//
// _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_;
// }
}
然后在visual_odometry.cpp中改写位姿估计函数
void VisualOdometry::poseEstimationPnP()
{
// construct the 3d 2d observations
vector<cv::Point3f> pts3d;
vector<cv::Point2f> pts2d;
for ( cv::DMatch m:feature_matches_ )
{
pts3d.push_back( pts_3d_ref_[m.queryIdx] );
pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
}
Mat K = ( cv::Mat_<double>(3,3)<<
ref_->camera_->fx_, 0, ref_->camera_->cx_,
0, ref_->camera_->fy_, ref_->camera_->cy_,
0,0,1
);
Mat rvec, tvec, inliers;
cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers ); // 用BA也可以，可以用默认顶点和边（单元边） // rvec, tvec,为旋转和平移
num_inliers_ = inliers.rows; // 多少个局内点（相当于用上了多少3d-2d作为局内点）
cout<<"pnp inliers: "<<num_inliers_<<endl;
T_c_r_estimated_ = SE3(
SO3::exp(Vector3d(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0))), // 原先的SO3改掉
Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
);
// 加入g2o优化 *******************************************************************
// using bundle adjustment to optimize the pose
// 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 );
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; // pose is 6, landmark is 3 位姿和路标点的维度（相当于顶点们的维度，当然，这里路标点没算做顶点）
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat ( // 设置初始值为ransac的结果
T_c_r_estimated_.rotationMatrix(),
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(); // 3d-2d的单元边，仅仅优化位姿
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_ = SE3 (
pose->estimate().rotation(), // 四元数
pose->estimate().translation() // 平移
);
}
最后记得CMakeLists中加上g2o，要注意，因为使用了默认的g2o顶点，因此需要这样（g2o_types_sba决定了你能够使用g2o默认顶点）：
# G2O
LIST( APPEND CMAKE_MODULE_PATH /home/mjy/slambook2/3rdparty/g2o/g2o/cmake_modules )
SET( G2O_ROOT /usr/local/include/g2o )
find_package( G2O REQUIRED )
include_directories( ${G2O_INCLUDE_DIRS} )
set( THIRD_PARTY_LIBS
${OpenCV_LIBS}
${Sophus_LIBRARIES}
g2o_core g2o_stuff g2o_types_sba)
此外，直接运行可能会报错：double free or corruption (out)，需要更改CMakelists.txt
将set( CMAKE_CXX_FLAGS “-std=c++11 -march=native -O3” )改成set(
CMAKE_CXX_FLAGS “-std=c++11 -O3” )
结果发现迭代error变化很小，可能是因为迭代一次用了很多点，所以第一次迭代就搞定了。而且初始值也不错。
但是仍然有缺陷，比如：
中间某帧丢失的话可能会出现比如第一帧和第五帧进行PnP。这样匹配的特征点可能比较少。因此ransac的PnP效果不是太好。
因此考虑加入局部地图：local mapping
就是VO维护的map_地图
这是下一篇要讲述的