地圖VO
之前計算位姿的方法是，兩兩幀間運用特徵匹配，PnP方法計算位姿並用g2o進行單元邊的3d-2d的優化。
雖說用g2o優化已經能夠避免一部分噪聲對RANSAC的PnP的影響。
但是，如果存在幀丟失的情況，可能距離比較遠的兩幀進行特徵匹配和PnP求解的話可能匹配上的點比較少。因此位姿計算可能不夠精確。並最終會出現追蹤丟失Lost狀態。
此外，全用兩幀間計算的話誤差累計漂移是個很嚴重的問題，
因此我們加入局部地圖的概念（local mapping），每一幀都爲VO維護的map_中提供新的地圖點（攜帶着自身的描述子），計算位姿時通過2d關鍵點與3d攜帶描述子的地圖點進行匹配，然後再進行3d-2d的PnP方法，能夠在一定程度上解決VO的LOST問題。
由於地圖點的座標直接採用世界座標，因此我們可以直接求出當前幀在世界座標系中的位姿Tcw，而不是Tcf (或者說而不是Tij)
這樣就講得通了，爲什麼說位姿圖中，誤差項爲Tij^(-1) * Ti * Tj，Ti和Tj是前端的結果，Tij是用特徵點法或者直接法或者IMU或者GPS得到的。終於明白了！！！！！！！！
下面着重考慮局部地圖 local mapping的VO，被稱爲——地圖VO
這裏的局部地圖和slam的建圖不一樣，暫時還沒有考慮全局地圖。這裏只是局部地圖，簡單的把一些比較近的、視野內的特徵點緩存到一個地方，是一個小地圖。而slam建圖過程中的全局地圖卻儲存了所有的特徵點。但是直接在全局上定位，計算量很大（全局地圖主要用於迴環檢測和地圖表達）。因此在這裏我們維護一個小的局部地圖，從而實現通過小地圖（局部地圖）進行定位當前位姿。
是否使用局部地圖，是出於對效率和精度這一矛盾體的仔細考量：
根據看代碼，有下面幾點發現：
（1）addKeyFrame，增加關鍵幀的時候，提供了一個功能：
因爲我們讓第一幀成爲了關鍵幀，所以加入第一幀的時候，把第一幀的特徵點加入到地圖中，在普通VO也確實是這麼做的，但是是根據setRef3DPoints這個單獨的函數做的。這裏是把功能集成到addKeyFrame中。
（2）Mat竟然也有尾插，這是很讓人興奮的:
desp_map.push_back( p->descriptor_ ); // 插上一行能觀察到的地圖點的描述子
（3）直接與地圖匹配，是地圖VO的特點，並且用了cv::FlannBasedMatcher matcher_flann_; 匹配器
matcher_flann_.match ( desp_map, descriptors_curr_, matches ); // flann matcher將嫩觀測到的地圖點與當前幀關鍵點進行匹配，注意，這裏和普通VO是不一樣的
（4）向地圖中增加關鍵幀的機制是：當前幀的關鍵點一旦和地圖點匹配上了，那麼就無視他。沒有匹配上的關鍵點需要映射到世界座標系中，增加局部地圖點的規模。具體看函數：void Map_VisualOdometry::addMapPoints()
（5）void Map_VisualOdometry::optimizeMap()這個函數實現了添加新點（嵌套着addMapPoints()），並且實現了控制局部地圖的規模，控制手段如下：
（i）根據匹配率篩選：
匹配率 = 匹配上次數/觀測到的次數
有些三維點被觀測到了很多次（isInFrame），但是匹配上的次數很少，不知道原因爲何，那既然匹配的少，就直接刪掉吧。
（ii）根據視角刪除點
angle = getViewAngle( curr_, iter->second );獲得視角，這個點如果在當前幀視野中太偏，即處在邊緣位置，那麼就刪掉。
（iii）當前幀與路標點的匹配少於100個，才加入地圖，否則地圖規模容易太大— 但我覺得問題不大，因爲有下面這一步：
（iv）直接控制地圖點規模，當地圖點增加到1000個以上的時候，迭代增加門檻map_point_erase_ratio_，這樣有更多匹配率比較低的點被刪除，一直刪到總地圖點小於1000個。------這一塊代碼是我自己補充的。
後話
由於我一直在0.1上增刪代碼，沒有直接用後面的0.2 0.3 0.4版本的代碼，所以直接繼承是一個好方法：
map_visual_odometry.h
繼承部分如下。子類叫做：map_visual_odometry
#ifndef MAP_VISUALODOMETRY_H
#define MAP_VISUALODOMETRY_H
#include "visual_odometry.h"
// 在這裏繼承一下父類(VisualOdometry)，實現地圖VO
namespace myslam
{
class Map_VisualOdometry:public VisualOdometry // 繼承一下，避免大量修改原始VO代碼
{
public:
typedef shared_ptr<Map_VisualOdometry> Ptr;
附上優化地圖的代碼：（有我自己寫的一點） Map_VisualOdometry::optimizeMap()
void Map_VisualOdometry::optimizeMap() // 加入新點，清理一波觀測次數太少的、太邊緣的點
{
// remove the hardly seen and no visible points
for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
{
if ( !curr_->isInFrame(iter->second->pos_) ) // 位姿估計完畢以後，如果當前幀看不到的地圖點，那麼清除掉
{
iter = map_->map_points_.erase(iter);
continue;
}
float match_ratio = float(iter->second->matched_times_)/iter->second->visible_times_; // 這個點的，匹配上次數/可觀測到的次數
if ( match_ratio < map_point_erase_ratio_ ) // map_point_erase_ratio_ = 0.1，爲yaml讀入
{
iter = map_->map_points_.erase(iter); // 在map_中去除這個點，因爲這個點匹配率太低了，可以控制局部地圖的規模
continue;
}
double angle = getViewAngle( curr_, iter->second );
if ( angle > M_PI/6. )
{
iter = map_->map_points_.erase(iter); // 太邊緣的點也給擦掉
continue;
}
if ( iter->second->good_ == false )
{
// TODO try triangulate this map point
// 我覺得暫時不用——直接在上面3p-2p的g2o處同時優化位姿和對應的特徵點即可
// 也就是用二元邊，這個以後再加代碼吧
}
iter++;
}
if ( match_2dkp_index_.size()<100 ) // 匹配上少於100個再加入地圖，否則地圖規模容易太大
addMapPoints(); // 在這裏加入新的地圖點
if ( map_->map_points_.size() > 1000 ) // 控制地圖點規模，迭代增加門檻map_point_erase_ratio_
{
// TODO map is too large, remove some one
map_point_erase_ratio_ += 0.05;
while (map_->map_points_.size() > 1000)
{
Map_VisualOdometry::optimizeMap(); // 提高門檻，調用自己
}
}
else
map_point_erase_ratio_ = 0.1;
cout<<"map points: "<<map_->map_points_.size()<<endl;
}
至此前端完成。（普通VO + 地圖VO的實現）
暫時還沒有用到關鍵幀，僅僅是當前後兩幀距離較遠的時候插入關鍵幀。
關鍵幀主要是後端用的。
可以聯繫我獲取完整的帶註釋的和有我自己補充的、適應各種新版本庫（比如模板類sophus和新版本g2o）的代碼。