《视觉SLAM十四讲精品总结》10 测试

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

int main ( int argc, char** argv )
{
	//链接参数文件
    myslam::Config::setParameterFile ( argv[1] );
	//构造VO，类型就是定义指向自身类型的指针
    myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );
	//读取数据文件地址以及文件
    string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
    cout<<"dataset: "<<dataset_dir<<endl;
    ifstream fin ( dataset_dir+"/associate.txt" );
    if ( !fin )
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }
	//定义图片名数组和时间戳数组
    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while ( !fin.eof() )
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
		//atof（）把字符串转换成浮点数
        rgb_times.push_back ( atof ( rgb_time.c_str() ) );
        depth_times.push_back ( atof ( depth_time.c_str() ) );
        rgb_files.push_back ( dataset_dir+"/"+rgb_file );
        depth_files.push_back ( dataset_dir+"/"+depth_file );

        if ( fin.good() == false )
            break;
    }
	//创建相机
    myslam::Camera::Ptr camera ( new myslam::Camera );
    
    // 可视化，viz模块
	//1. 创建可视化窗口
    cv::viz::Viz3d vis("Visual Odometry");
	//2. 创建坐标系部件，参数是坐标系长度
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
	// 渲染属性，第一个参数是枚举这里是线宽，后面数值
	world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
	camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);
	//showWeiget函数将部件添加到窗口内
	vis.showWidget("World", world_coor);
	vis.showWidget("Camera", camera_coor);
	//3. （可选）设置视角，相机位置坐标，相机焦点坐标，相机y轴朝向
    cv::Point3d cam_pos( 0, -1.0, -1.0 ), cam_focal_point(0,0,0), cam_y_dir(0,1,0);
	// 视角位姿
    cv::Affine3d cam_pose = cv::viz::makeCameraPose( cam_pos, cam_focal_point, cam_y_dir );
	// 设置观看视角
    vis.setViewerPose( cam_pose );
    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;
	//画面的快速刷新呈现动态，由循环控制
    for ( int i=0; i<rgb_files.size(); i++ )
    {
        Mat color = cv::imread ( rgb_files[i] );
        Mat depth = cv::imread ( depth_files[i], -1 );
        if ( color.data==nullptr || depth.data==nullptr )
            break;
		//创建帧
        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
        pFrame->camera_ = camera;
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];
		//每帧的运算时间，看实时性
        boost::timer timer;
		//将帧添加进去
        vo->addFrame ( pFrame );
        cout<<"VO costs time: "<<timer.elapsed()<<endl;
        
        if ( vo->state_ == myslam::VisualOdometry::LOST )
            break;
		//可视化窗口动的是相机坐标系，求相机坐标系下的点在世界坐标系下的坐标
        SE3 Tcw = pFrame->T_c_w_.inverse();
        
        //T 
        cv::Affine3d M(
            cv::Affine3d::Mat3( 
                Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
                Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
                Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)
            ), 
            cv::Affine3d::Vec3(
                Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
            )
        );
        
        cv::imshow("image", color );
        cv::waitKey(1);
		//viz可视化窗口
        vis.setWidgetPose( "Camera", M);
        vis.spinOnce(1, false);
    }

    return 0;
}

 1、调用之前各大类指针Ptr并生成指针对象为：

typedef shared_ptr<VisualOdometry> Ptr;//之后用VisualOdometry::Ptr调用指针
myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry) ;//生成类指针Ptr的对象
vo->addFrame(pFrame);//指针调用成员函数
//相机
myslam::Camera::Ptr camera(new myslam::Camera);
//帧
myslam::Frame::Ptr pFrame=myslam::Frame::createFrame();
static Frame::Ptr createFrame();
Frame::Ptr Frame::createFrame(){
     static long factory_id=0;
     return Frame::Ptr(new Frame(factory_id++));//创建只有编号的帧
}

2、不用shared_ptr，直接的new

//声明相机、VO类以及构造函数
class PinholeCamera{
   public:
         PinholeCamera(double width, double height,
		double fx, double fy, double cx, double cy,
		double k1 = 0.0, double k2 = 0.0, double p1 = 0.0, double p2 = 0.0, double k3 = 0.0);
};
class VisualOdometry{
public:
    VisualOdometry(PinholeCamera* cam);
}
----------------------------------------------------------
//调用。直接new，返回是指针类型cam
PinholeCamera *cam = new PinholeCamera(1241.0, 376.0,
		718.8560, 718.8560, 607.1928, 185.2157);
VisualOdometry vo(cam);