Hello,各位朋友,新年快乐!
我们要进入第9讲的解读了,第9讲是纯实践的一讲,我觉得作者的这个布置实在是对初学者太友好了,前面我们学习了前端的所有内容,然后来一个手写前端,一方面复习了前面的内容,另一方面可以让我们简单地窥探一下一个SLAM系统大概长啥样!我还是很希望你好好学习一下这部分代码,如果你是初学者,这可能是见过的最完整的SLAM系统了,如果你掌握了它,你再去看别的优秀开源SLAM框架,就会得心应手了!
解读
文件组织方式
- 1.bin:用于存放可执行的二进制文件;
- 2.include/myslam:用于存放SLAM模块的头文件;
- 3.src:用于存放源代码文件,也就是
.cc
或者.cpp
文件; - 4.test:用于存放测试用的文件;
- 5.lib:用于存放编译好的库文件;
- 6.config:存放配置文件;
- 7.cmake_modules:第三方库的cmake文件;
0.1版本
0.1版本主要是定义了基本的数据结构,主要是定义了5个类:
Camera
:用于存放相机内参和外参,以及一些对于相机像素点的操作,包括像素点转换到空间点,空间点转换到当前相机坐标系下等函数;
Config
:主要负责参数文件的读取,并且可以随时提供参数的值。它为写成了单例模式,它只有一个全局对象;
Frame
:它主要用来保存一帧frame中的重要信息,包括时间戳、相对于参考坐标系的变换矩阵、对应的深度图、彩色图等。它其中还包含一些操作,比方说判断一个点是否在前frame视野中、获得相机中心、获得特征点对应的深度等;
Map
:它用来管理所有的路标点,并负责添加新的路标点、删除不好的路标点。其实Map
就是维护了一个持续的地图,我们一边做SLAM,一边将建立的3D点,添加到地图中,还会将关键的frame添加进map中,它的操作包括添加关键帧、插入地图点;
MapPoint
:它用来表示路标点。地图点也是具有方向和描述子的,所以MapPoint
中会有变量存储这些内容。
如果你还不习惯C++
的编程风格或者不是很了解面向对象编程方法,你可能需要稍微看看C++
的书籍补充一下,这些操作都是很基础的,也是很有效的。
0.1版本只是定义了一些基本数据结构,还没有赋予它功能!
0.2版本
先前我们学习的特征点匹配方法,都是基于前后两帧图像的,我们想想这样有啥缺点呢?我们对于前后两帧的依赖性太高了,如果上一帧位置出现了误差,下一帧误差继续累计,那么误差越来越离谱。
但是就算误差很大,我们通过前后两帧的匹配,还是可以将相机的位姿串起来的!
两两帧之间的特征匹配流程:
- 1.对新来的当前帧,提取关键点和描述子;
- 2.如果系统没有初始化,那么就以该帧为参考帧,根据深度图计算关键点的3D位置,然后返回第一步;
- 3.估计参考帧和当前帧的运动;
- 4.判断上述估计是否成功;
- 5.若成功,把当前帧作为新的参考帧,返回第1步;
- 6.若失败,记录连续丢失帧数。当连续丢失超过一定数量帧数的时候,VO状态设为丢失,算法结束。否则,返回第一步。
V0.2版本和V0.1版本相比,最大的区别,就是前者加入了VisualOdometry
类的实现。
VisualOdometry
类中的成员变量介绍:
enum VOState { //定义该枚举变量,目的是记录VO的当前状态
INITIALIZING=-1,
OK=0,
LOST
};
VOState state_; // current VO status
Map::Ptr map_; //Map类,主要用来存放和管理MapPoint与Frame
Frame::Ptr ref_; // reference frame,参考frame
Frame::Ptr curr_; // current frame ,当前frame
cv::Ptr<cv::ORB> orb_; // orb detector and computer,定义了ORB关键点提取器
vector<cv::Point3f> pts_3d_ref_; // 3d points in reference frame
vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
Mat descriptors_curr_; // descriptor in current frame
Mat descriptors_ref_; // descriptor in reference frame
vector<cv::DMatch> feature_matches_;//特征匹配成功的结果
SE3 T_c_r_estimated_; // the estimated pose of current frame
int num_inliers_; // number of inlier features in icp,在ICP迭代中,有多少内点
int num_lost_; // number of lost times
// parameters
int num_of_features_; // number of features,特征点数量
double scale_factor_; // scale in image pyramid,图像金字塔缩放比例
int level_pyramid_; // number of pyramid levels,特征金字塔层数
float match_ratio_; // ratio for selecting good matches
int max_num_lost_; // max number of continuous lost times
int min_inliers_; // minimum inliers
double key_frame_min_rot; // minimal rotation of two key-frames
double key_frame_min_trans; // minimal translation of two key-frames
VisualOdometry
类中的成员函数介绍:
public: // functions
//添加新的frame
bool addFrame( Frame::Ptr frame ); // add a new frame
protected:
// inner operation
void extractKeyPoints();
void computeDescriptors();
void featureMatching();
void poseEstimationPnP();
void setRef3DPoints();
void addKeyFrame();
bool checkEstimatedPose();
bool checkKeyFrame();
0.2版本的测试主函数,在test/run_vo.cpp
中,它的核心代码:
for ( int i=0; i<rgb_files.size(); i++ )
{
Mat color = cv::imread ( rgb_files[i] );//读取RGB图片
Mat depth = cv::imread ( depth_files[i], -1 );//读取RGB对应的深度图
if ( color.data==nullptr || depth.data==nullptr )
break;
myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();//新建一个指针指向Frame对象
pFrame->camera_ = camera;//将相机内参传入Frame对象中
pFrame->color_ = color;
pFrame->depth_ = depth;
pFrame->time_stamp_ = rgb_times[i];
boost::timer timer;
//向vo中增加frame,增加完之后,会自动提取关键点,计算两帧之间的运动,如果没有初始化也会自动初始化
vo->addFrame ( pFrame );
cout<<"VO costs time: "<<timer.elapsed()<<endl;
if ( vo->state_ == myslam::VisualOdometry::LOST )
break;
SE3 Tcw = pFrame->T_c_w_.inverse();//vo中计算出来变换矩阵,可以直接通过pFrame的指针调用
// show the map and the camera pose
cv::Affine3d M(
cv::Affine3d::Mat3(
Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)
),
cv::Affine3d::Vec3(
Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
)
);
cv::imshow("image", color );
cv::waitKey(1);
vis.setWidgetPose( "Camera", M);
vis.spinOnce(1, false);
}
0.3版本
0.3版本主要是加入了优化过程,因为PnP是采用的RANSAC方法计算的,所以很容易受到噪声影响,于是将RANSAC算出来的解,作为初值,然后使用非线性优化获得更加鲁棒的解。
0.3版本对于0.2版本的改动,主要在于加入了g2o_types.h/.cpp
文件,定义了非线性优化的实现细节,然后将非线性优化加入到了void VisualOdometry::poseEstimationPnP()
函数中,在通过RANSAC方法解出PnP之后,将解作为非线性优化的初始值,然后获得更加鲁棒的解。
g2o
部分的代码还是前几讲的内容,没什么变化,0.3版本的主函数,和0.2版本也是一致的。
0.4版本
在0.3版本实现了一个非常脆弱的VO系统,失败的可能性非常大,通过前后两帧frame匹配计算运动的方法也非常简陋,现在我们想一想另一方法,我们将图像的特征点添加到一个很大的容器中(Map),然后每来一张图片,我就将图片和这个容器中的所有点就行匹配,匹配上之后,再将这张图片的特征点添加进容器中,这样是不是很合理啊!两两帧的VO和地图VO的区别如下图所示:
我不知道你是否明白这其中的道理,我给你稍微解释一下,我们先前的前后两帧求VO,每次求完之后就会丢弃前一帧的特征点,实际上我们可以不丢弃,而是将它们都保存下来,而后的图片都跟这个Map里面的特征点进行求VO,这样可以匹配的点就会更多,即使某一帧失败了,下一帧还可以跟Map进行匹配,这种容错率会更高。
这里的地图只是一个临时的概念,我建议你就将它理解为一个大容器,等学到建图这一讲你就明白了。
上面所说的这种容器,书中称为Map,我也沿用书中这种用法。如果我们从SLAM一开始一直维护这个Map那么这就是全局地图,如果我们只维护一个局部的Map(新进来的一帧图像可以看得见的特征点组成的Map)。
我们先看一下0.4和前面0.3有什么主要区别:
0.4版本中对MapPoint
进行了完善,,然后修改了VIsualOdometry
类中的几个主要函数,主要做的决策有,在第一帧中将所有的特征点都加入到地图中,后续如果地图点不在视野中了,就将其删除掉,在匹配数量减少的时候,添加新点。
简单的说一下0.4版本新送入的图片怎么和Map进行匹配:
- 1.当我们新进入一张图片的时候,首先提出关键点计算描述子;
- 2.然后对地图中的点进行遍历,如果属于当前帧视野内的就取出来;
- 3.将取出来的地图点(3D点)和当前帧图像点求解P3P,计算出位置作为初始值放入g2o进行优化。
关于代码更细节的东西,我将我注释过的代码,上传到GIthub上,方便你可以看我的注释。第9讲 实践:设计前端 0.4代码注释
这一讲0.4版本的代码,值得大家好好学习一下!
如果你的代码void VisualOdometry::poseEstimationPnP()
中的g2o报错,请修改代码如下:
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
std::unique_ptr<Block::LinearSolverType> linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>());
std::unique_ptr<Block> solver_ptr( new Block ( std::move(linearSolver) ));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
让我们只争朝夕,不负韶华,共同迎接2020年的到来。