这是基于之前的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.