vo類總結
https://www.saowen.com/a/655e71f2f815afb595601d95ab319038dbf517e64a24c401e2aefbb7e5f77bb2
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】
關鍵詞：函式 座標 一個 如果 定義 返回 計算 當前 變數 引數
相關推薦：