之前讨论的都是两两帧间的位姿估计，然而我们将发现仅凭两帧的估计是不够的。
解决办法是：
我们把特征点缓存成为一个小地图，计算当前帧与地图之间的位置关系。
这样的好处是：比如两帧之间，能够提取50个特征点，然后两两匹配，RGBD的话就可以用ICP+g2o的方式进行优化，但是50个特征点可能比较少。因此我们想用更多的特征点。那么哪里来呢？由更靠前的帧来。更前面的帧提取出来的关键点可以加入到Map类中，这样Map类就维护了一个包含一大堆地图点的局部地图，比如有100个，每个地图点都携带自身的描述子，那么拿当前帧的关键点与维护的小地图中的所有的地图点进行描述子匹配，那么利用了更多的地图点，那么就可以增加位姿估计的精度（位姿估计的方法是PnP，3D->2D）。 此外，在形成小地图Map后，需要筛选一下不合适的地图点（匹配率低的、靠边缘的等等），并且要控制地图点的规模（比如小于1000个点），所以说，相对于包含了所有特征点的全局地图而言，他是一个“小地图”。（这一块见0.4—地图VO）
但是我们依然从简单的看起：两两帧的视觉里程计
1.两两帧的视觉里程计
只关心两个帧间的运动估计，并不优化特征点的位置，把估计得到的位姿串起来，也能得到一条运动轨迹。
这个叫做：
两两帧间——无结构的VO
原理如下：
参考帧（reference）-------当前帧（current）
有如下关系：
Tcw = Tcf * Tfw ----- 发现，一般记录的都是从前往后的变换（比如上一帧到下一帧的变换，而不是反过来）
即以t-1为参考，求取t时刻的运动。可以通过特征点匹配、光流或者直接法得到。
这里我们只关心运动，不关心结构换句话说，只要通过特征点成功求出了运动，就不再需要这一帧的特征点了，就扔掉了。（暂时还不用考虑，我们之前说的利用特征点构建一个小map）
这样虽然会有缺陷，但是计算量小。此后由t求取t+1，t+1再求取t+2，以此类推，形成一条运动轨迹
这里用特征点匹配的PnP来实现一遍（当然光流法就更简单了，一行opencv的代码即可）
工作过称为：
系统初始化很好理解，第一帧来了，暂时不用估计位姿，只需要提取关键点，然后用rgbd的深度信息得到关键点的深度们即可，这样就获得了一批3d点，那么第二帧来了以后，才能用第一帧得到的3d点来进行3d-2d的PnP求取位姿。
接下里定义了一个视觉里程计类（visual_odometry.h）
成员变量
typedef shared_ptr<VisualOdometry> Ptr;
enum VOState {
INITIALIZING=-1,
OK=0,
LOST
};
VOState state_; // current VO status 是初始化状态？追踪成功状态？追踪失败状态？
Map::Ptr map_; // map with all frames and map points 维护一个map地图
Frame::Ptr ref_; // reference frame 参考帧
Frame::Ptr curr_; // current frame 当前帧
cv::Ptr<cv::ORB> orb_; // orb detector and computer
vector<cv::Point3f> pts_3d_ref_; // 3d points in reference frame vector容器存储参考帧的3d点
vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame vector容器存储当前帧的关键点
Mat descriptors_curr_; // descriptor in current frame Mat存储当前帧的描述子
Mat descriptors_ref_; // descriptor in reference frame Mat存储参考帧的描述子
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 // 内点数目
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 两帧间的最小平移
成员函数：
public: // functions
VisualOdometry();
~VisualOdometry();
bool addFrame( Frame::Ptr frame ); // add a new frame 添加帧，并完成位姿估计（根据Vo的三种状态，完成不同的操作）
protected:
// inner operation
void extractKeyPoints();
void computeDescriptors();
void featureMatching();
void poseEstimationPnP();
void setRef3DPoints();
void addKeyFrame();
bool checkEstimatedPose();
bool checkKeyFrame();
着重看一下，根据状态完成位姿估计，并在合适的情况下插入关键这的函数addFrame()
bool VisualOdometry::addFrame ( Frame::Ptr frame ) // add a new frame 添加帧，并完成位姿估计（根据Vo的三种状态，完成不同的操作）
{
switch ( state_ )
{
case INITIALIZING:
{
state_ = OK;
curr_ = ref_ = frame;
map_->insertKeyFrame ( frame ); // 维护的map插入一帧关键帧，即keyframes_这个unorderedmap容器插入一个元素
// extract features from first frame
extractKeyPoints(); // 将特征点提取到keypoints_curr_容器
computeDescriptors(); // 计算描述子放入容器descriptors_curr_
// compute the 3d position of features in ref frame
setRef3DPoints(); // 计算参考帧3d点，推满pts_3d_ref_这个参考帧3d点容器和推满 descriptors_ref_参考帧描述子容器，一行代表一个3d点的描述子
break;
}
case OK:
{
curr_ = frame;
extractKeyPoints(); // 将（当前帧）特征点提取到keypoints_curr_容器
computeDescriptors(); // 计算（当前帧）描述子放入容器descriptors_curr_
featureMatching(); // 填满装有匹配点 vector<cv::DMatch> feature_matches_ 这个容器
poseEstimationPnP(); // 得到T_c_r_estimated_和内点个数
if ( checkEstimatedPose() == true ) // a good estimation 根据局内点个数不能过小以及李代数不能过大判断位姿估计是否成功
{
curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_; // T_c_w = T_c_r*T_r_w 看出连乘的思想了吧！！！！！！！！一直猜测的Ti是用连乘得到的终于得到了印证
ref_ = curr_; // 更一下
setRef3DPoints(); // 当前帧变为参考帧以后，设置下一帧的参考3d点
num_lost_ = 0; // 连续追踪失败数目置零
if ( checkKeyFrame() == true ) // is a key-frame 如果是关键帧的话
{
addKeyFrame(); // 在维护的小地图map中插上一帧关键帧
}
}
else // bad estimation due to various reasons 位姿估计失败
{
num_lost_++; // 连续失败追踪数目+1
if ( num_lost_ > max_num_lost_ ) // 连续失败追踪10次，状态置lost
{
state_ = LOST;
}
return false;
}
break;
}
case LOST:
{
cout<<"vo has lost."<<endl;
break;
}
}
return true;
}
2.常识补充：
（1）随机抽样一致性算法（RANSAC）
一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。
参考链接：https://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.html
在这里的应用是用在PnP求解中：
cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
比如随机采一些上一帧的3d点（pts3d的子集），与之对应的这一帧的2d点（pts2d的子集），这些3d-2d数据作为内点。用他们估计一个Trc，然后看看其他的3d-2d数据是不是近似满足这个Trc，如果近似满足，这些3d-2d也被当做内点。然后用所有的这些内点重新估计一个模型，如果这个模型对于所有内点的误差之和很小，则认为这个Trc是不错的。（实际上代码中是：T_c_r_estimated_）
（2）slam十四讲187-188页讲了如何处理TUM数据集同步的问题：
TUM数据集见：https://blog.csdn.net/weixin_44684139/article/details/105258750
（3）最终运行程序会报错，是因为
- 相机new出来的时候没加内参
- 没有加入字节对齐 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
3.总结
（1）RANSAC 求出的 PnP 易收到噪声影响，所以需要用优化的方法进行改进
（2）由于现在的 VO 是无结构的,特征点的 3D 位置被当作真值来估计运动。但实际上,RGB-D 的深度图必定带有一定的误差,特别是那些深度过近或过远的地方。并且,由于特征点往往位于物体的边缘处,那些地方的深度测量值通常是不准确的。所以现在的做法不够精确,我们需要把特征点也放在一起优化。
也就是说，为什么需要同时优化特征点： rgbd直接读出来的三维点有误差，体现在过近、过远、靠近边缘处
（3）因为两帧间比较可能会受到追踪丢失、误差累计等影响。因此更自然的方式是比较当前帧和地图,而不是比较当前帧和参考帧。于是,我们要关心如何把当前帧与地图进行匹配,以及如何优化地图点的问题。
（4）特征点的提取和匹配占据了绝大多数的计算时间,而看似复杂的 PnP 优化,计算量与之相比基本可以忽略。因此,如何提高特征提取、匹配算法的速度,将是特征点方法的一个重要的主题。一种可预见的方式是使用直接法/光流,可有效地避开繁重的特征计算工作。（新版代码就是光流法，直接一个opencv的命令就能完成）