這是基於之前的vo類做的。vo類總結.note
參數文件的直接設置在config目錄下，比如是default.yaml文件，里面會定義dataset_dir,cmera類的fx,fy,cx,cy,VisualOdometry的min_inliers,match_ratio,key_frame_min_rot,key_frame_min_trans,max_num_lost,number_of_features,scale_factor,level_pyramid.這些參數在visual_odometry.cpp里讀取的時候必須參數名要一致，可以從參數文件中一個個復制。如果出錯，編譯不會出錯，但是程序運行會報錯誤，一般是opencv的錯誤。例如
OpenCV Error: Assertion failed (npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F))) in solvePnPRansac, file /home/uuuu/opencv-3.1.0/modules/calib3d/src/solvepnp.cpp, line 230
terminate called after throwing an instance of 'cv::Exception'
what(): /home/uuuu/opencv-3.1.0/modules/calib3d/src/solvepnp.cpp:230: error: (-215) npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function solvePnPRansac
這個錯誤是由於num_of_features_ = Config::get<int> ( "number_of_features" );參數文件里是number_of_features，我寫成num_of_features.
max_num_lost_ = Config::get<float> ( "max_num_lost" );參數文件里是max_num_lost,我給寫成max_num了。
1.參數文件的讀取
在主程序里用Config類的setParameterFile函數設置參數文件。變量是argv[1].
myslam::Config::setParameterFile(argv[1]);
實際運行的時候就是./bin/run_vo config/default.yaml
之所以編譯好的程序會直接放在bin目錄下，是因為在CMakeLIsts.txt文件里有設置
set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
set( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib )
然后定義vo里程計
myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry);
這種指針定義的時候都會（new 什么）
得到dataset_dri就是數據集路徑，可以直接從參數文件中讀取
string dataset_dir=myslam::Config::get<string>("dataset_dir");
跟之前一樣，定義fin為dataset+"\"+"associate.txt")
判斷一下fin,如果打不開這個目錄，輸出錯誤，返回1.
定義rgb_files,depth_files,rgb_times,depth_times,這里是向量形式，因為用來存儲每一個rgb_file文件名等。
一個while循環，while(!fin.eof())不知道這是個什么條件，eof判斷文件是否到末尾，當是的時候返回true,while(!fin.eof())就是當文件還沒有到末尾的時候，fin依次賦值給rgb_time,rgb_file,depth_time,depth_file.
然后依次再放進rgb_times等。只不過時間要經過atof(rgb_time.c_str()),文件名要是完整的路徑，即dataset+"\"+rgb_file.
如果文件沒有打開，跳出while循環，就是fin.good()==false.
定義camera類camera
myslam::Camera::Ptr camera(new myslam::Camera);
2.可視化部分
定義3d可視化為vis,命名為Visual Odometry
cv::viz::Viz3d vis("Visual Odometry");
定義世界坐標系和相機坐標系
cv::viz::WCoordinateSystem world_coor(1.0),camera_coor(0.5);
定義3個點cam_pos,cam_focal_point,cam_y_dir
cv::Point3d cam_pos(0,-1.0,-1.0),cam_focal_point(0,0,0),cam_y_dir(0,1,0);
后面的值不能出錯，之前cam_pos值被我弄成了0,1.0，-1.0就導致只出來兩條線。
前面3個點組成了cam_pose.
cv::Affine3d cam_pose=cv::viz::makeCameraPose(cam_pos,cam_focal_point,cam_y_dir);
設置可視化位姿為cam_pose
vis.setViewerPose(cam_pose)
設置兩個坐標系的渲染屬性rendering property,線長度為2和1.
world_coor.setRenderingProperty(cv::viz::LINE_WIDTH,2.0);
camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH,1.0);
把兩個坐標系添加到vis,一個命名為World,一個命名為Camera.必須首字母大寫，不然識別不出來。
vis.showWidget("World",world_coor);
vis.showWidget("Camera",camera_coor);
3.讀圖
弄一個for循環，用來一張一張讀圖片。
定義color為cv::imread(rgb_files[i]),depth,度深度圖的時候，后面要加-1.
如果color或depth的數據為空指針，就跳出循環。
定義myslam的幀類pFrame,初值為myslam::Frame::createFrame()
這里出了一點小錯是因為我定義frame.cpp的時候忘了在createFrame()標明它所屬的類。
依次定義pFrame的值camera_,color_,depth_,time_stamp_依次為camera,color,depth,rgb_times[i].
幀類以后要注意添加這4個值。camera可以myslam::Camera;:Ptr camera(new myslam::Camera)
因為在myslam::Camera類設置了一個函數camera()，里面就設置了fx,fy,cx,cy,depth_scale的讀取方式。所以只要設置camera值為myslam::Camera;:Ptr camera(new myslam::Camera)，就可以輕松獲得camera的所有值。
設置完camera的所有值后再運行源程序，現在每個vo只需要20ms了。
color_,depth_，time_stamp_都可以讀圖得到。
之前已經設置過vo類了。
把這里的pFrame設置為vo的添加幀的輸入。
vo->addFrame(pFrame);
如果vo的狀態值為LOST,則跳出循環。這里判斷的時候用的是myslam::VisualOdometry::LOST.
因為要顯示的是T_c_w所以定義
SE3 Tcw=pFrame->T_c_w_.inverse().
定義M為Tcw的旋轉矩陣的每一項的cv::Affine3d::Mat3形式和位移的每一項的cv::Affine3d::Vec3組成的。例如Tcw.rotation_matrix()(0,0)
類型同cam_pose,都是cv::Affine3d
這里也可視化了color圖。cv::imshow("image",color)
cv::waitKey(1);這里為1的時候並沒有停頓啊。但因為是和vis.spinOnce(1,false)一起用的，所以不確定。
設置部分位姿，這里設置了相機的為M，和前面的Camera照着。
vis.setWidgetPose("Camera",M)
設置for的循環周期vis.spinOnce(1,false);
簡單的來說就是定義vo,camera,pFrame,color,depth.
vo,camera里的參數在定義的時候就已經讀取了.camera,color,depth都是pFrame幀里的變量。
然后把pFrame當成變量一個個輸入到vo中就可以了。vo->addFrame.