初谈SLAM十四讲——第9讲代码解析

本文从主函数出发逐步剖析第九讲中project4.0，初学SLAM，个人知识学习有限，望各位大佬批评指正。
以下使用SLAM十四讲中的前端project4.0部分源代码进行注释与标注，逐行注释对代码的理解

主函数

int main ( int argc, char** argv )
{    
    if ( argc != 2 )    
    {        cout<<"usage: run_vo parameter_file"<<endl;        return 1;    }
    myslam::Config::setParameterFile ( argv[1] );    
    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;        
    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 );
      // visualization    
      cv::viz::Viz3d vis ( "Visual Odometry" );    //创建可视化窗口
      cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );    //创建坐标系
      //相机坐标系模型的初始坐标，焦点坐标，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 );  //设置观察视角
      world_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 2.0 );    
      camera_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 1.0 );
      //显示到窗口    
      vis.showWidget ( "World", world_coor );    
      vis.showWidget ( "Camera", camera_coor );
      cout<<"read total "<<rgb_files.size() <<" entries"<<endl;    
      for ( int i=0; i<rgb_files.size(); i++ )    {        
           cout<<"****** loop "<<i<<" ******"<<endl;        
           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;        //VO丢失，停止
           SE3 Twc = pFrame->T_c_w_.inverse();//求逆，求得cam到world
          //相机位姿提取
           cv::Affine3d M (            
           cv::Affine3d::Mat3 (                
           Twc.rotation_matrix() ( 0,0 ), Twc.rotation_matrix() ( 0,1 ), Twc.rotation_matrix() ( 0,2 ),                
           Twc.rotation_matrix() ( 1,0 ), Twc.rotation_matrix() ( 1,1 ), Twc.rotation_matrix() ( 1,2 ),                
           Twc.rotation_matrix() ( 2,0 ), Twc.rotation_matrix() ( 2,1 ), Twc.rotation_matrix() ( 2,2 )            
           ),            
           cv::Affine3d::Vec3 (                
           Twc.translation() ( 0,0 ), Twc.translation() ( 1,0 ), Twc.translation() ( 2,0 )           )        );
           Mat img_show = color.clone();        
           for ( auto& pt:vo->map_->map_points_ )        {
           //此处采用了STL中的unordered_map,得到Map_point的智能指针            
           myslam::MapPoint::Ptr p = pt.second;            
           Vector2d pixel = pFrame->camera_->world2pixel ( p->pos_, pFrame->T_c_w_ );//世界坐标系下三维点转变为此时的像素点
           cv::circle ( img_show, cv::Point2f ( pixel ( 0,0 ),pixel ( 1,0 ) ), 5, cv::Scalar ( 0,255,0 ), 2 );        
           }
        cv::imshow ( "image", img_show );        
        cv::waitKey ( 1 );        
        vis.setWidgetPose ( "Camera", M );        
        vis.spinOnce ( 1, false );        
        cout<<endl;    
        }
    return 0;
    }

addFrame

addFrame是VO运行的核心代码，对帧的处理和相机位姿的计算都在这里进行

bool VisualOdometry::addFrame ( Frame::Ptr frame )
{    
   switch ( state_ )    
   {    
   case INITIALIZING://初始化VO    
   {        
       state_ = OK;        
       curr_ = ref_ = frame;        
       // extract features from first frame and add them into map        
       extractKeyPoints();        
       computeDescriptors();        
       addKeyFrame();      
       // the first frame is a key-frame        
       break;    
   }    
       case OK:    {        
       curr_ = frame;        
       curr_->T_c_w_ = ref_->T_c_w_;        
       extractKeyPoints();//提出特征点        
       computeDescriptors(); //计算描述子       
       featureMatching();  //特征点匹配      
       poseEstimationPnP(); //相机位姿估计       
       if ( checkEstimatedPose() == true ) // a good estimation        
       {            
             curr_->T_c_w_ = T_c_w_estimated_; //位姿旋转矩阵传递给当前帧储存            
             optimizeMap();            
             num_lost_ = 0;            
             if ( checkKeyFrame() == true ) // is a key-frame            
             {                
                  addKeyFrame();            
             }        
        }        
        else // bad estimation due to various reasons        
        {            
             num_lost_++;            
             if ( num_lost_ > max_num_lost_ )            
             {                state_ = LOST;            }            
             return false;        
        }        
             break;    
   }    
   case LOST:    
   {        
      cout<<"vo has lost."<<endl;        
      break;    
   }    
}
    return true;
}

持续更新中……