vo類總結

1.Camera類

camera類裏面，首先camera有5個變量，fx_,fy_,cx_,cy_,depth_scale_5個變量,由外部傳fx,fy,cx,cy,depth_scale給它。
定義了一個智能指針，如今還不知道這個智能指針有什麼用
typedef std::shared_ptr<Camera> Ptr,之後傳遞參數時使用Camera::Ptr就能夠了。
camera.h文件裏定義6個函數的聲明。包括相機座標系和世界座標系之間的轉換，相機座標和像素座標，世界座標和像素座標。世界座標和像素座標之間的轉換是先把它轉成中間項相機座標，再轉的。
位姿都是T_c_w.若是是相機轉世界座標系，用T_c_w的逆矩陣就能夠了。
像素座標和相機座標之間轉，定義了一個depth,z座標直接是這個depth.

2.Frame類
變量有6個，id,時間戳，T_c_w,Camera類的智能指針，彩色圖和深度圖，
由於用到了Camera::Ptr,因此要#include "../myslam/camera.h"
id,時間戳time_stamp是用來記錄幀信息的，T_c_w幀計算時會用到，好比計算幀的像素座標，而Camera::Ptr用這個指針能夠訪問類Camera裏的任意變量和函數，能夠任意使用它的函數。直接camera->就能夠訪問了。彩色圖和深度圖，是用來獲得深度和關鍵點的。
裏面有3個函數，一個是
（1）createFrame(),沒有變量，裏面只是定義factory_id=0,而後返回Frame::Ptr(new Frame(factory_id++)
彷佛返回的是一個新的指針。
（2）findDepth是用來發現關鍵點的深度的，返回值是double類型，變量是const cv::KeyPoint& kp
這個函數首先獲得kp的像素座標x,y.
int x=cvRound(kp.pt.x)
int y=cvRound(kp.pt.y)
而後在深度圖上讀取ushort d.若是d!=0，就能夠返回深度double(d)/camera_->depth_scale_;
這裏的深度是要除以深度尺度因子的。
若是讀到的深度爲0，那麼依次用(y)[x-1],(y-1)[x],(y)[x+1],(y+1)[x]來代替深度d,若是d不爲0，返回double(d)/camera_->depth_scale_;
默認返回-1.0;
（3）函數計算相機光心座標，類型Vector3d。沒有變量，直接是T_c_w_.inverse().translation();
(4)知道點的世界座標系座標，判斷這個點是否在幀上，返回bool類型。
變量就是const Vecotr3d& pt_world)
首先計算pt_cam,若是它的z座標小於0，就返回false.
再計算pt_pixel,若是它的像素座標知足0<u<color_.cols,0<v<color_.rows，則返回true.
3MapPoint類
MapPoint是路標點，會提取當前幀的特徵點，而後與它匹配，來估計相機的運動。由於要計算匹配，因此還要存儲路標點的描述子。
還要記錄路標點被觀測到的次數和被匹配的次數，做爲評價其好壞程度的指標。
差很少6個變量吧，路標點id,世界座標系座標，norm好像是觀測的方向，它的描述子，不過它的類函數中沒有描述子。只有id,pos,norm,還把觀測次數和被匹配次數初始化爲0.
並且沒有傳變量的時候，把id賦值爲-1，pos,norm,observed_times,correct_times都設爲0.
裏面只有一個createMapPoint函數，定義factory_id=0,返回值類型是MapPoint的智能指針。返回的是new MapPoint(factory_id++,Vector3d(0,0,0),Vector3d(0,0,0).
4Map類
Map管理的是路標點和關鍵幀。VO的匹配過程只須要和Map打交道。
Map做爲類，只定義了兩個量，一個是全部的路標點
unordered_map<unsigned long,MapPoint::Ptr> map_points_;
一個是全部的關鍵幀
unordered_map<unsigned long,Frame::Ptr> keyframes_;
由於用到了路標點和關鍵幀，全部要include map.h和mappoint.h.
也只定義了兩個函數，一個是插入關鍵幀 insertKeyFrame
先輸出當前關鍵幀的數量。
而後判斷要插入的幀frame的id_是否是和keyframes_.end()相等，若是相等，說明須要插入，把frame的id號和frame一塊兒打包插入
keyframes._.insert(make_pair(frame->id_,frame));
插入路標點的函數同理。
map類是經過散列Hash來存儲的，方便隨機訪問和插入、刪除。
5.config類
這個類主要作的是文件的讀取。並且在程序中能夠隨時提供參數的值。這裏把config類寫成單件(singleton)模式，它只有一個全局對象，當咱們設置參數文件的時候，建立該對象並讀取參數文件，隨後就能夠在任意地方訪問參數值，最後在程序結束時自動銷燬。

Config類負責參數文件的讀取，並在程序的任意地方均可隨時提供參數的值。因此咱們把config寫成單件(singleton)模式，它只有一個全局對象，當咱們設置參數文件時，建立該對象並讀取參數文件，隨後就能夠在任意地方訪問參數值，最後在程序結束時自動銷燬。自動銷燬？
把指針定義成config_,爲何不定義成Ptr了
咱們把構造函數聲明爲私有，防止這個類的對象在別處創建，它只能在setParameterFile時構造，實際構造的對象是config的智能指針：static std::shared_ptr<Config> config_.用智能指針的緣由是能夠自動析構，免得咱們再調一個別的函數來作析構。
2.在文件讀取方面，咱們使用OpenCV提供的FileStorage類，它能夠讀取一個YAML文件，且能夠訪問其中任意一個字段，因爲參數實質值可能爲整數、浮點數或字符串，因此咱們經過一個模板函數get來得到任意類型的參數值。
模板其實就是一個類型啊。
下面是config的實現，注意，咱們把單例模式的全局指針定義在此源文件中了。
在實現中，咱們只要判斷一下參數文件是否存在便可。定義了這個Config類以後，咱們能夠在任何地方獲取參數文件裏的參數，例如，當想要定義相機的焦距fx時，按照以下步驟操做便可：
1.在參數文件中加入」Camera.fx:500"
2.在代碼中使用：
myslam::Config::setParameterFile("parameter.yaml");
double fx=myslam::Config::get<double>("Camera.fx");
就能得到fx的值了。
固然，參數文件的實現不僅這一種，咱們主要從程序開發的便利角度來考慮這個實現，讀者固然也能夠用更簡單的方式來實現參數的配置。
把智能指針定義成了config_,還有一個file_
兩個函數，一個是setParameterFile,變量是文件名。
一個是get函數，返回類型根據get<>後面參數決定，返回值是config_->file_[key]
setParameterFile,若是config_是空指針，就
config_=shared_ptr<Config>(new Config)
就爲新的Config指針？
config_->file_就定義爲cv::FileStorage形式，變量是filename.c_str(),參數是cv::Filestorage::READ.是隻讀模式，不修改。
若是文件打不開，就用std::cerr輸出錯誤信息。
發佈release.
而~Config()函數，裏面判斷了file_,若是file_可以打開，就發佈file_
6.VisualOdometry類
6.1變量
首先它也有一個智能指針，這樣以後以後就能夠經過定義VisualOdometry::Ptr vo;而後經過vo->來訪問它的變量還有函數了。先來看它的變量。
6.1.1vo的狀態變量state_
它總共就三個值INITIALIZING=0,OK=1,LOST,以後會根據vo的不一樣狀態進行不一樣的計算。
6.1.2用於計算的變量
map_,ref_,cur_,orb_
地圖是一個總變量，用來看幀是否是關鍵幀，若是是，就把它添加到地圖裏。還能夠輸出地圖裏關鍵幀的總數。至於ref_,cur_就是用來計算的兩個幀，計算它兩個之間的T_c_r_estimated_,orb_是個提取orb特徵的智能指針，把當前幀的彩色圖放進去，能夠計算當前幀的關鍵點和描述子。
6.1.3計算出來的中間變量
pts_3d_ref_,這個是參考幀的特徵點的世界座標系的座標，每次換參考幀，它都得更改一下。
keypoints_curr_,當前幀的關鍵點，也能夠說是特徵點。
descriptors_ref_,descriptors_curr_,pts_3d_ref,和descriptors_ref是一塊兒計算的，這兩個描述子是用來計算匹配的，匹配通過篩選以後變成feature_matches_.
根據pts_3d_ref_,由當前幀的像素座標有pts_2d,由pnp能夠計算出T_c_r_estimated_.
num_inliers_,pnp函數返回值有一個inliers,num_inliers_=inliers.rows,用來和min_inliers作比較的。和T_c_r_estimated.log()的範數一塊兒，用來檢測位姿的。
num_lost_,若是位姿估計不經過的話，num_lost_就會加1，若是num_lost_>max_lost_，vo的狀態就會變成LOST.
6.1.4閾值，用來作比較的值
match_ratio_,用來篩選匹配的，若是匹配距離小於(min_dist*match_ratio_,30.0)，匹配是好匹配。
max_num_lost_,用來判斷vo狀態，若是位姿檢測不經過數達到必定值，vo的值定爲LOST.
min_liners_,若是有效特徵點數num_inliers小於min_liners,那麼位姿估計不經過。
key_frame_min_rot_,最小旋轉，用來檢測關鍵幀，當前幀和參考幀的變換的範數只有大於最小旋轉或最小位移，當前幀纔是關鍵幀。也就是關鍵幀檢驗才經過。
key_frame_min_trans_,最小位移。
6.1.5建立orb_的變量。
num_of_features_,應該每一個幀提取的特徵數應該是固定的。
scale_factor_,圖像金字塔的尺度因子，應該提取圖像特徵的時候會用到。
level_pyramid_圖像金子塔的層級。
把這三個變量值放進cv::ORB::create()函數裏就能夠直接建立orb_了。
orb_=cv::ORB::create(num_of_features_,scale_factor_,level_pyramid_).
閾值和建立orb_的三個變量值都是經過讀取config類的參數文件獲得的。直接Config::get<參數類型>("參數文件名")
參數類型，帶num的都是整數，金字塔層級也是整數，匹配比例float,最小位移，最小旋轉和尺度比例都是double類型。

6.2函數
它有8個函數。來看每一個函數的具體實現。
6.2.1extractKeyPoints()
返回值爲空，沒有變量。
做用，提取當前幀的彩色圖的關鍵點，獲得keypoints_curr_.
6.2.2computeDescriptors()
返回值爲空，沒有變量。
做用：計算當前幀的特徵點的描述子，獲得descriptors_curr_.
6.2.3featureMatching
返回值爲空，沒有變量。
做用：計算參考幀和當前幀之間的匹配，獲得篩選後的匹配feature_matches,輸出篩選後的匹配數。
過程：計算參考幀和當前幀之間的匹配，返回索引和匹配距離。用的是cv::BFMatcher,距離用的是漢明距離NORM_HAMMING,求最小距離用的是std::min_element,篩選匹配用的是min_dis*match_ratio_,30.0,篩選以後的匹配放入feature_matches_.
6.2.4setRef3DPoints()
返回值爲空，沒有變量。
做用：獲得參考幀的特徵點的相機座標和參考幀的描述子矩陣。
過程：製做一個for循環，讀取參考幀上每一個特徵點的深度值，ref_是幀，幀類有發現深度函數findDepth.
double d=ref->findDepth(keypoints_curr[i].pt),只因此這裏用的是keypoints_curr_,是由於參考幀仍是當前幀，計算的時候。ref_=curr_.
若是深度大於0，根據幀類有cmera_指針，cmera類有各類座標轉換函數。能夠把參考幀的像素座標加深度轉成相機座標。
把相機座標都放到pts_3d_ref_.
再計算一下描述子矩陣。
6.2.5poseEstimationPnP()
返回值爲空，沒有變量。
做用：獲得有效特徵點數num_inliers和當前幀和參考幀之間的位姿估計值T_c_r_estimated_
過程,輸出有效特徵點數
定義pts3d,pts2d,一個放的是參考幀的相機座標值，一個放的是當前幀的特徵點的像素座標值。
定義相機內參矩陣K.至於裏面的fx_,fy_,cx_,cy_都在幀類的cmera類的變量值裏有定義。
使用函數cv::solvePnPRansac(pts3d,pts2d,K,Mat(),r_vec,t_vec,false,100,4.0,0.99,inliers)
rvec,tvec,inliers都是返回值，false是不用外參，100是迭代次數，4.0是重投影偏差，0,99是可信度。
由inliers獲得num_inliers_.num_inliers_=inliers.rows.
由rvec,tvec獲得T_c_r_estimated_，是SE3,李代數形式。
輸出有效特徵點數。
6.2.6checkEstimationPose()
返回bool值，沒有變量。
做用：對估計出來的位姿進行檢測，若是有效特徵點數太少或者運動太大，檢測不經過。
過程：若是num_inliers<min_inliers_,那輸出拒絕由於有效特徵點數太少,並輸出有效特徵點數。
定義d爲位姿估計值的對數形式。
Sophus::Vector6d d=T_c_r_estimated_.log()
若是d.norm()>5.0,那麼輸出拒絕由於運動太大，並輸出d.norm().
6.2.7checkKeyFrame()
返回true或false,沒有變量。
做用：檢測當前幀是否是關鍵幀。若是當前幀相對於參考幀移動的位移大於最小位移或旋轉大於最小旋轉，能夠把當前幀當作關鍵幀加如地圖。
過程：
一樣定義d爲估計值的對數形式。d是位移在前，旋轉在後
trans取d的前3個值，rot取d的後3個值，若是trans.norm()>key_frame_min_trans_或者rot.norm()>key_frame_min_rot_，就說明檢測經過。
6.2.8addKeyFrame()
返回值爲空，沒有變量。
做用：用於把當前幀添加到地圖裏，並輸出加入當前幀的提示。
6.2.9總函數addFrame()
添加幀函數。
返回true或false,變量是Frame::Ptr frame.
做用：對輸入的幀作處理，同時地圖，vo的狀態值作相應的變換。
過程：
是一個case.用來判斷vo的狀態值state_,根據state_的不一樣值作不一樣的處理。
當state_爲INITIALIZING的時候，狀態值變爲OK，參考幀當前幀都爲輸入幀，地圖把輸入幀之間加入地圖，不作檢測。而後對輸入幀提取關鍵點，計算描述子，此時輸入幀已是參考幀了，設置它的特徵點的相機座標和描述子。依次用的函數是extractKeyPoints();computeDescriptors();setRef3DPoints().
當state_爲OK的時候，輸入幀被當作是當前幀，而後對當前幀提取關鍵點，計算描述子，和參考幀的描述子進行匹配並篩選匹配，而後用參考幀的相機座標和當前幀的像素座標求解pnp,獲得T_c_r_estimated_.依次用的函數是 extractKeyPoints();computeDescriptors(); featureMatching();
poseEstimationPnP();而後對位姿估計值進行檢測，若是檢測經過，計算T_c_w_,而後當前幀就變成了參考幀，設置新一輪的參考幀的相機座標和描述子。用的函數是checkEstimationPose(),setRefPoints().
而後進行關鍵幀檢測，若是關鍵幀檢測經過，地圖上把當前幀給加進去。用的函數checkKeyFrame(),addKeyFrame().
若是位姿估計值檢驗沒有經過的話，num_lost_+1,若是num_lost_>max_num_lost_,就把state_狀態值設爲LOST.返回False.
當state_狀態值爲LOST的時候，輸出vo狀態丟失。

7.g2o_types.h類

爲了避免跟以前的重合，實際定義時
定義成#ifndef MYSLAM_G2O_TYPES_H來着。
一個h文件能夠定義多個類。裏面就定義了3個類。由於都是邊類，因此都是以EdgeProjectXYZ開頭。
若是隻是RGBD的話，名字EdgeProjectXYZRGBD,3d-3d,繼承自二元邊，3,測量值類型Eigen::Vector3d,兩個頂點，一個是g2o::VertexSBAPointXYZ,一個是g2o::VertexSE3Expmap，就是位姿。
若是RGBD只考慮位姿，名字EdgeProjectXYZRGBDPoseOnly，繼承自一元邊，2，測量值Vector3d,頂點類型g2o::VertexSE3Expmap.
若是不用RGBD，就是3d-2d的話。名字EdgeProjectXYZ2UVPoseOnly,繼承自一元邊，測量值Vector2d,就是像素座標，頂點類型g2o::VertexSE3Expmap,就是位姿了。
裏面基本上都是先EIGEN_MAKE什麼，而後聲明一下四個虛函數，分別是計算偏差函數computeError(),求雅克比矩陣函數linearlizeOplus(),讀寫函數read,write.讀寫返回bool,前兩個返回空。
形式以下：
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void computeError();
virtual void linearizeOplus();

virtual bool read(std::istream& in){}
virtual bool write(std::ostream& os)const {}
只是類EdgeProjectRGBDPoseOnly多了一個變量point_,EdgeProjectXYZ2UVPoseOnly由於要計算像素座標還多了一個camera_.
須要咱們寫就兩個函數computeError()和linearlizeOplus()函數。
（1）computeError(）函數
第一步都是定義頂點，而後根據頂點的估計值算出估計值，而後和測量值一塊兒算偏差。
g2o::VertexSE3Expmap* pose= static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
這裏沒有new.它是把_vertices[0]賦值給頂點，就是位姿。而後根據位姿的估計值對點進行映射成相機座標再算出像素座標值，和測量值進行比較就獲得偏差了。
_error=_measurement-camera_->camera2pixel(pose->estimate().map(point_));
也是由於須要用到camera_,point_，因此才定義了這兩個變量。
（2）linearizeOplus函數
仍是先把_vertices[0]賦值給pose.前後把T定義爲pose的估計值，而後把xyz_trans定義爲T.map(point_).
而後xyz_trans[0],xyz_trans[1],xyz_trans[2]分別爲x,y,z，用來計算雅克比矩陣。
雅克比矩陣爲像素u對空間點P求導再乘以P對yi*李代數求導。
前一個2*3,後一個3*6,爲[I,-空間點的反對稱矩陣】實際計算的時候把空間點的反對稱矩陣放在前面，並且求負。【空間點的反對稱矩陣，-I】