改進地圖的vo類

如今的地圖只是各幀特徵點的集合。
建立地圖：局部，全局。
局部：只相機位置附近的特徵點，用來和當前幀匹配求解相機位置的。
全局：不定位，迴環檢測和地圖表達。
重點或麻煩：維護局部地圖的規模。爲了實時，保證地圖規模不能太大。
1.修改地圖點類MapPoint
增長變量和函數形式。
1.1類
變量倒也簡單。兩個id,一個id_,一個factory_id_,主要用的是factory_id.
向量形式的pos_,norm_,一個是路標點的世界座標，一個是路標點被觀測到的方向。
good_,判斷路標點是好是壞的指標。
descriptor_路標點的描述子，由於要用路標點的描述子和當前幀的描述子進行匹配。
次數matched_times visible_times_,在位姿估計中成爲內點的次數和在當前幀中可見的次數。
還有幀列表observed_frames_,能夠觀測到路標點的關鍵幀。
類中還有一個內聯函數getPointCV,它是用來返回pos_的Point3f形式。
1.2類函數
MapPoint函數的兩種形式，主要用於new MapPoint()賦值。迭代的每一次賦值。
一個是沒有變量，把id賦值爲-1，次數都爲0，good_是true,pos_,norm爲零向量。這應該是初始化的時候。
一個主要做用是觀測幀列表中加入新的幀。變量有id,pos_,norm,descriptor_,frame_，這和下面的createMapPoint相呼應。
createMapPoint函數的兩種形式。它主要返回MapPoint的指針。一個也是沒有變量，返回的是new MapPoint(factory_id++,Vector3d(0,0,0),Vector3d(0,0,0));
一個有前面的幾個變量，返回的是MapPoint(factory_id_++,pos_world,norm,frame,descriptor);
還定義了factor_id_初值爲0.
2.visualOdometry類的變化
2.1增長的變量
cv::FlannBasedMatcher matcher_flann_，這是匹配子，它的match函數能夠用來作desp_map和當前幀的描述子來作匹配。
vector<MapPoint::Ptr> match_3dpts_，這是有MapPoint的智能指針組成的向量，通過兩次選擇，一次是從地圖中的路標點中選，選世界座標轉像素座標在當前幀範圍內的。這裏的T_c_w用的是參考幀的。第二次是把desp_map和當前幀的描述子進行匹配，選擇匹配距離小於mis_dis*match*ratio和30的描述子對應的路標點。
vector<int> match_2dkp_index_篩選匹配後當前幀的特徵點的索引。
T_c_w_estimated_一塊兒估計的都是當前幀和參考幀之間的，因此用的是相機座標，這裏用世界座標，計算的是T_c_w的孤寂值。
閾值map_point_erase_ratio_，應該是路標點的匹配比例，若是solvePostiationPnP中有用到的話，路標點匹配次數會加1，若是路標點轉成像素座標後在當前幀裏，觀測次數加1.匹配次數除以觀測次數獲得匹配比例，這個值默認爲0.1,若是匹配比例小於它，會把該路標點從當前地圖中刪除。
2.2修改的函數
2.2.1特徵匹配函數featureMatching()
做用：構建求解相機位姿的局部地圖的特徵點的世界座標match_3dpts_，通過cv::Point3f變過以後就是pts3d和當前幀的像素座標的索引match_2dkp_index_.
過程：(1)構建局部地圖的描述子矩陣desp_map，和存放可見路標點指針的矩陣candidate.
對於地圖中的全部路標點都作一個判斷，若是路標點的世界座標通過變換獲得的像素座標在當前幀上，即if(curr_.isInFrame(p->pos_))，那麼這個路標點的可見次數加1，candidate存放這個路標點，desp_map存放這個路標點的描述子。
（2）對desp_map和當前幀的描述子矩陣用flann法進行匹配，獲得matches,而後獲得matches的最小距離min_dis.篩選匹配，假設經過的匹配是m,那麼match_3dpts_路標向量存放candidate裏經過匹配的路標，match_2dkp_index_裏存放經過匹配的當前幀的特徵點的索引。
2.2.2位姿估計函數poseEstimationPnP()
做用：求解Ｔ_c_w_estimated並進行g2o優化。
過程：獲得pts3d,pts2d,K,等，而後用cv::solvePnPRansac函數求解獲得rvec,tvec,inliers.而後獲得T_c_w_esitmated_,而後把它當作初值進行優化，優化邊和位姿跟以前同樣，在添加一條條邊的時候，真正用於匹配的特徵點的索引會存放在inliers中，能夠經過inliers.at<int> ( i,0 )獲得。這樣用於匹配的路標點的匹配次數就會加1.
2.2.3添加關鍵幀函數addKeyFrame
做用：通常是在判斷對當前幀進行檢測以後，若是檢測經過，添加關鍵幀。這裏多加了一步，就是在初始化的時候，把當前幀的全部特徵點都當作路標點加入到地圖。
過程：（1）若是地圖裏的關鍵幀爲空的話map_->keyFrame.empty()，作一個for循環,根據當前幀的關鍵點的大小keypoints_curr_.size(),獲得d,若是d<0,跳出循環，由d獲得p_world,是根據參考幀裏cmaera裏的函數，用的倒是當前幀的T_c_w.p_world和參考幀的相機中心相減獲得n.
n.normaiize(）,而後p_world,規範化以後的n，就是觀測方向，當前幀的描述子的clone,當前幀，組合獲得路標指針map_point,.
用地圖類中的insertMapPoint函數把這些路標點插入到地圖中。
(2)不然地圖插入關鍵幀，參考幀變成當前幀。
2.3增長的函數
2.3.1添加路標點函數addMapPoints
做用：往地圖上添加路標點，是當前幀和他可見的路標點進行匹配，篩選匹配後的特徵點小於100的時候，添加路標點。並且添加的是當前幀的特徵點。
過程：造一個跟當前幀的關鍵點數量相同的bool向量matched，先全賦值爲false.
對於匹配中用到的當前幀的特徵點，對應的索引在matched設爲true.
造一個for循環，根據當前幀的關鍵點的數量來定。
對於matched==true的那些i，不作處理。就是匹配中用到的那些特徵點，不作處理。
其餘的特徵點，求d,而後p_world,而後n,n.normalize,map_point,而後地圖把這裏路標點給插入進去。
2.3.2優化地圖函數optimizeMap()
做用：移除當前幀已經看不見的路標點和匹配比例很差的路標點
過程：（1）作一個for循環，全部路標點進入循環，iter表明地圖中的一個路標點，但iter.second纔是路標點的值。若是路標點的位姿通過轉換以後不在當前幀範圍以內，刪除路標點。
計算匹配比例，就是匹配次數除以可見次數，若是比例小於閾值map_point_erase_ratio_,刪除此路標點。
刪除是iter=map_->map_points_.erase(iter);
計算觀測角angle,根據當前幀和路標點位姿計算到的，就是路標點的位姿減去當前幀的相機中心獲得n,n規範化以後，n.transpose()*point->norm_,就是n的逆乘以路標點的觀測方向再通過acos變換獲得的角度。
若是這個角度大於M_Pi/6,那麼就刪除此路標點。
（2）若是篩選匹配後的當前幀的特徵點小於100，執行添加路標點操做，供下一次使用。
若是路標點的數量大於1000，那麼map_point_erase_ratio_加上0.05.若是不大於1000，就仍是0.1.
2.3.3獲得觀測角函數getViewAngle()
做用：變量是當前幀和路標點。路標點的世界座標減去當前幀的相機中心獲得n.n經normalize以後，
acos(n.transpose()*point->norm_)就獲得了觀測角。
觀測角主要在優化地圖的時候用到，若是觀測角大於pi/6,說明已經快看不見路標點 了，把路標點刪除。
2.4總函數addFrame()
輸入變量爲幀類
過程：初始化的時候，對輸入幀提取關鍵點，計算描述子和添加關鍵幀，此時，輸入幀即爲參考幀，又爲當前幀。
狀態ok的時候，輸入幀爲當前幀，把參考幀的T_c_w當作當前幀的T_c_w的初值。提取當前幀的額關鍵點，描述子，把地圖上路標點的描述子和當前幀的描述子進行匹配，desp_map只可能小於當前幀的描述子數量，不可能大於。並篩選匹配。而後根據匹配結果獲得pts3d,pts2d，根據pnp求解出當前幀的T_c_w_estimated.
若是位姿估計檢驗經過，就是內點數不能太少，運動不能太大。當前幀的T_c_w就等於它的估計值。執行優化地圖操做，就是對不在當前幀可見範圍內，匹配比例太小，觀測角度過大的路標點進行刪除。若是篩選匹配後當前幀的特徵點太小，添加路標點，若是總路標點數量太大，增長刪除比例。
若是關鍵幀檢驗經過的話，就是旋轉或位移有一個大於最小值的話，添加關鍵幀。
若是關鍵幀檢驗沒有經過，num_lost+1,若是num_lost大於閾值，vo狀態設爲丟失。、
若是爲丟失，跳出循環。輸出vo已丟失。