之前討論的都是兩兩幀間的位姿估計，然而我們將發現僅憑兩幀的估計是不夠的。
解決辦法是：
我們把特徵點緩存成爲一個小地圖，計算當前幀與地圖之間的位置關係。
這樣的好處是：比如兩幀之間，能夠提取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的命令就能完成）