運行vo總結

這是基於以前的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.