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】

 


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM