第九章0.2
0.2版本实现一个基本的VO，也就是两两帧间的无结构VO，它的任务是根据输入的图像，计算相机运动和特征点位置。在这里我们只关心运动，不关心结构，也就是说只要通过特征点成功的求出了运动，我们就不再需要这一帧的特征点了。
整体逻辑如下图：
这种VO工作方式非常简单。在匹配特征点的方式中，最重要的是参考帧与当前帧之间的特征匹配关系。流程如下：
visual_odometry.h类为算法实现。对比发现，0.1版本的此类是空的。所以在0.2版本要写入两两帧VO功能。
#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H
#include "myslam/common_include.h"
#include "myslam/map.h"
#include Ptr;
//定义枚举来表征VO状态，分别为：初始化、正常、丢失
enum VOState
{
INITIALIZING=-1,
OK=0,
LOST
};
//这里为两两帧VO所用到的参考帧和当前帧。还有VO状态和整个地图。
VOState state_; // current VO status
Map::Ptr map_; // map with all frames and map points
Frame::Ptr ref_; // reference frame
Frame::Ptr curr_; // current frame
//这里是两帧匹配需要的：keypoints，descriptors，matches
cv::Ptr orb_; // orb detector and computer
vector 类实现，visual_odometry.cpp
#include break;
}
}
return true;
}
//提取keypoint
void VisualOdometry::extractKeyPoints()
{
orb_->detect ( curr_->color_, keypoints_curr_ );
}
//计算描述子
void VisualOdometry::computeDescriptors()
{
orb_->compute ( curr_->color_, keypoints_curr_, descriptors_curr_ );
}
//特征匹配
void VisualOdometry::featureMatching()
{
// match desp_ref and desp_curr, use OpenCV's brute force match
vector//说一下这个函数，新的帧来的时候，是一个2D数据，因为PNP需要的是参考帧的3D，当前帧的2D。
//所以在当前帧迭代为参考帧时，有个工作就是加上depth数据。也就是设置参考帧的3D点。
void VisualOdometry::setRef3DPoints()
{
// select the features with depth measurements
//3D点数组先清空，后面重新装入
pts_3d_ref_.clear();
//参考帧的描述子也是构建个空Mat。
descriptors_ref_ = Mat();
//对当前keypoints数组进行遍历
for ( size_t i=0; i//找到对应的depth数据赋值给d
double d = ref_->findDepth(keypoints_curr_[i]);
//如果>0说明depth数据正确，进行构造
if ( d > 0)
{
//由像素坐标求得相机下3D坐标
Vector3d p_cam = ref_->camera_->pixel2camera(Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d);
//由于列向量，所以按行构造Point3f，push_back进参考帧的3D点。
pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) ));
//参考帧描述子这里就按照当前帧描述子按行push_back。这里也可以发现，算出来的Mat类型的描述子，是按行存储为一列，读取时需要遍历行。
descriptors_ref_.push_back(descriptors_curr_.row(i));
}
}
}
//核心功能函数，用PnP估计位姿
void VisualOdometry::poseEstimationPnP()
{
// construct the 3d 2d observations
vector//根据旋转和平移构造出当前帧相对于参考帧的T，这样一个T计算完成了。循环计算就能得到轨迹。
T_c_r_estimated_ = SE3(
SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
);
}
//简单的位姿检验函数，整体思路就是匹配点不能过少，运动不能过大。
bool VisualOdometry::checkEstimatedPose()
{
// check if the estimated pose is good
//这里简单的做一下位姿估计判断，主要有两个，一就是匹配点太少的话，直接false，或者变换向量模长太大的话，也直接false
if ( num_inliers_ < min_inliers_ )
{
cout<<"reject because inlier is too small: "<return false;
}
// if the motion is too large, it is probably wrong
//将变换矩阵取log操作得到变换向量。
Sophus::Vector6d d = T_c_r_estimated_.log();
//根据变换向量的模长来判断运动的大小。过大的话返回false
if ( d.norm() > 5.0 )
{
cout<<"reject because motion is too large: "<return false;
}
//如果让面两项都没return，说明内点量不少，运动也没过大，return true
return true;
}
bool VisualOdometry::checkKeyFrame()
{
//说一下这个是否为关键帧的判断，也很简单，
//关键帧并不是之前理解的轨迹比较长了，隔一段选取一个，而还是每一帧的T都判断一下，比较大就说明为关键帧，说明在这一帧中，要么平移比较大，要么拐弯导致旋转比较大，所以添加，如果在运动上一直就是小运动，运动多久都不会添加为关键帧。
//另外上方的判断T计算错误也是运动过大，但是量级不一样，判断计算错误是要大于5，而关键帧，在配置文件中看只需要0.1就定义为关键帧了，所以0.1到5的差距，导致这两个函数并不冲突
Sophus::Vector6d d = T_c_r_estimated_.log();
Vector3d trans = d.head<3>();
Vector3d rot = d.tail<3>();
if ( rot.norm() >key_frame_min_rot || trans.norm() >key_frame_min_trans )
return true;
return false;
}
//关键帧添加，直接调用insertKeyFrame()将当前帧插入就好了。
void VisualOdometry::addKeyFrame()
{
cout<<"adding a key-frame"<insertKeyFrame ( curr_ );
}
} 本来0.2版本中还包含一个VO的测试程序，一是这个测试程序具有通用性，后续版本都是这个，二来这个测试程序里面包含了OpenCV的viz模块使用，所以单开一篇总结。