视觉SLAM十四讲第九章0.4代码注释

首先是 run_vo.cpp

// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/imgproc/imgproc.hpp>

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

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: run_vo parameter_file"<<endl;
        return 1;
    }

    //已修改，这样输入参数只需要./
    myslam::Config::setParameterFile ( string(argv[1])+"default.yaml" );
    //创建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;
   //读取文件，将associate中的数据复制到对应参数中
    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;
    }

    //创建相机对象的时候，会在构造函数里读取config文件中的default文件，来获得相机参数
    myslam::Camera::Ptr camera ( new myslam::Camera );

    // visualization，对于viz的设置
    cv::viz::Viz3d vis ( "Visual Odometry" );
    cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );
    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 );

    
    //先读取rgb文件的数量
    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;
    //读取所有rgb文件
    for ( int i=0; i<rgb_files.size(); i++ )
    {
      //可以用来分隔每次输出的数据，这样看的更清楚
        cout<<"****** loop "<<i<<" ******"<<endl;
       //rgb图像矩阵
	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();
       //camera中的数据在其初始化的时候已经放入
	pFrame->camera_ = camera;
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];

	//将pframe所包含的所有信息都给了vo
        boost::timer timer;
        vo->addFrame ( pFrame );
        cout<<"VO costs time: "<<timer.elapsed() <<endl;

        if ( vo->state_ == myslam::VisualOdometry::LOST )
            break;
	//将求出世界坐标系（第一帧的坐标系）到当前帧的变换矩阵，为了实现viz里面变换部件的位姿
        SE3 Twc = pFrame->T_c_w_.inverse();

        // 这里的Affine3d类型用来调整viz中的部件的位姿
        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_ )
        {
            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;
}

 然后是最主要的visual_odometry.cpp

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

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

namespace myslam
{

  //初始化列表，在创建对象的时候调用
VisualOdometry::VisualOdometry() :
    state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 ), matcher_flann_ ( new cv::flann::LshIndexParams ( 5,10,2 ) )
{
    num_of_features_    = Config::get<int> ( "number_of_features" );
    scale_factor_       = Config::get<double> ( "scale_factor" );
    level_pyramid_      = Config::get<int> ( "level_pyramid" );
    match_ratio_        = Config::get<float> ( "match_ratio" );
    max_num_lost_       = Config::get<float> ( "max_num_lost" );
    min_inliers_        = Config::get<int> ( "min_inliers" );
    key_frame_min_rot   = Config::get<double> ( "keyframe_rotation" );
    key_frame_min_trans = Config::get<double> ( "keyframe_translation" );
    map_point_erase_ratio_ = Config::get<double> ( "map_point_erase_ratio" );
    orb_ = cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ );
}

VisualOdometry::~VisualOdometry()
{

}

//传入的frame包含了rgb，depth，camera，时间这些参数
bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
    switch ( state_ )
    {
    case INITIALIZING:
    {
        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
        {
	  //如果计算的好，将经过BA优化的（当前帧和地图之间的）变换矩阵给curr_->T_c_w_
            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
        {
	  //这一帧的运动估计差，则加1,当连续max_num_lost_帧都差的时候，则估计失败
            num_lost_++;
            if ( num_lost_ > max_num_lost_ )
            {
                state_ = LOST;
            }
            return false;
        }
        break;
    }
    case LOST:
    {
        cout<<"vo has lost."<<endl;
        break;
    }
    }

    return true;
}

void VisualOdometry::extractKeyPoints()
{
    boost::timer timer;
    orb_->detect ( curr_->color_, keypoints_curr_ );
    cout<<"extract keypoints cost time: "<<timer.elapsed() <<endl;
}

void VisualOdometry::computeDescriptors()
{
    boost::timer timer;
    orb_->compute ( curr_->color_, keypoints_curr_, descriptors_curr_ );
    //输出描述子，查看是不是行向量，但是太大了，显示一个矩阵
    //cout<<descriptors_curr_<<endl;
    cout<<"descriptor computation cost time: "<<timer.elapsed() <<endl;
}

void VisualOdometry::featureMatching()
{
    boost::timer timer;
    vector<cv::DMatch> matches;
    
    //建立一个保存描述子的map矩阵，保存匹配需要地图点的描述子，
    //因为描述子是一个行向量
    Mat desp_map;
    // select the candidates in map 
    //candidate的生命周期只限于这个函数，暂时存放在当前帧视野以内的mappoint
    vector<MapPoint::Ptr> candidate;
    //遍历所有地图点，将符合条件的mappoint放入candidate，将描述子信息放入desp_map
    for ( auto& allpoints: map_->map_points_ )
    {
        MapPoint::Ptr& p = allpoints.second;
        // 检测这个点（世界坐标系中）是否在当前帧的视野之内 
        if ( curr_->isInFrame(p->pos_) )
        {
            // add to candidate 
            p->visible_times_++;
            candidate.push_back( p );
	    //Mat也可以利用push_back来插入一行，和vector类似
            desp_map.push_back( p->descriptor_ );
        }
    }
    //采用新的匹配方法FlannBasedMatcher(最近邻近似匹配)，而不是暴力匹配BFMatcher；
    //这步匹配中，第一个参数由原来的参考帧，变成了上面定义的desp_map地图，进行匹配。
    //也就是当前帧直接和地图进行匹配
    matcher_flann_.match ( desp_map, descriptors_curr_, matches );
    // select the best matches
    //找出容器matches中最小的元素的指针或迭代器
    //distance变量返回两个描述子之间的距离
    //这里用到了lambda表达式，也就是这里面的第三个参数
    //可以隐藏返回值的类型，会根据return自动确定，这里又加上了
    float min_dis = std::min_element (
                        matches.begin(), matches.end(),
                         [] ( const cv::DMatch& m1, const cv::DMatch& m2 )->bool
    {
        return m1.distance < m2.distance;
    } )->distance;

    //保存已经匹配的MapPoint（MapPoint的指针类型）
    match_3dpts_.clear();
    //保存已经匹配的2d关键点的索引
    match_2dkp_index_.clear();
    for ( cv::DMatch& m : matches )
    {
      //如果描述子之间的距离小于一个值（30和min_dis*match_ratio_中较大的），则表示匹配成功
        if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
        {
	  //会匹配成功很多，但只有一部分符合条件，将符合条件的这些特征点放入容器
	  //queryIdx表示参考帧的匹配成功的索引；trainIdx表示当前帧的匹配成功的索引
            match_3dpts_.push_back( candidate[m.queryIdx] );
            match_2dkp_index_.push_back( m.trainIdx );
        }
    }
    //平均每帧的好的匹配点大概100~200个左右
    cout<<"good matches: "<<match_3dpts_.size() <<endl;
    cout<<"match cost time: "<<timer.elapsed() <<endl;
}

//这里删除了setRef3DPoints()函数，用不到了，因为我们当前帧不需要和参考帧对比，而是直接和地图对比

//利用和地图匹配得到的信息（match_3dpts_）来进行pnp估计位姿
void VisualOdometry::poseEstimationPnP()
{
    // construct the 3d 2d observations
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;

    //取出当前帧匹配成功的2d点
    for ( int index:match_2dkp_index_ )
    {
        pts2d.push_back ( keypoints_curr_[index].pt );
    }
    //取出参考帧（实际上是map里面）的匹配成功的点
    for ( MapPoint::Ptr pt:match_3dpts_ )
    {
      //getPositionCV为inline函数，因为短小且经常被调用
      //getPositionCV函数得到3d点的坐标，因为MapPoint类里面包含了很多信息，世界坐标系的坐标也在其中
        pts3d.push_back( pt->getPositionCV() );
    }

    Mat K = ( cv::Mat_<double> ( 3,3 ) <<
              ref_->camera_->fx_, 0, ref_->camera_->cx_,
              0, ref_->camera_->fy_, ref_->camera_->cy_,
              0,0,1
            );
    Mat rvec, tvec, inliers;
    //得到rvec就是旋转矢量，输出的inlier就是局内点的索引，也就是符合模型的数据的索引，是一个列向量
    //一般的pnp我们用的是最小二乘法，而这里用的是ransac
    /*参数定义
     * objectPoints，要匹配的3d空间点数组
     * objectPoints，要匹配的2d图像点数组
     * cameraMatrix，相机内参矩阵
     * distCoeffs，相机畸变矩阵
     * rvec，旋转向量输出承接矩阵
     * tvec，平移向量输出承接矩阵
     * 后面的参数跟ransac算法有关。倒是都有默认值
     * useExtrinsicGuess，迭代初始值是否定为提供的rvec和tvec值，这里没有提供，所以用false
     * iterationsCount，迭代次数，ransac算法所必须的迭代次数
     * reprojectionError，重投影误差。ransac算法迭代时也必须要规定的误差阀值，来确定是否为内点。
     * confidence，置信度。ransac算法每次用于更新迭代次数的参数。一般固定选为0.995
     * inliers，内点输出承接数组   
     */
    cv::solvePnPRansac ( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
    //局内点数量
    num_inliers_ = inliers.rows;
    //输出符合模型的数据的个数
    cout<<"pnp inliers: "<<num_inliers_<<endl;
    //看一下
    //cout<<inliers<<endl;
    //将旋转矢量和平移转换成变换矩阵，初始值，之后还要优化
    T_c_w_estimated_ = SE3 (
                           SO3 ( rvec.at<double> ( 0,0 ), rvec.at<double> ( 1,0 ), rvec.at<double> ( 2,0 ) ),
                           Vector3d ( tvec.at<double> ( 0,0 ), tvec.at<double> ( 1,0 ), tvec.at<double> ( 2,0 ) )
                       );

    //上面ransac会输出符合模型的点的索引，下面对这些符合模型的点进行BA
    // using bundle adjustment to optimize the pose
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
        T_c_w_estimated_.rotation_matrix(), T_c_w_estimated_.translation()
    ));
    optimizer.addVertex ( pose );

    // edges
    for ( int i=0; i<inliers.rows; i++ )
    {
        int index = inliers.at<int> ( i,0 );
        // 3D -> 2D projection
        EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
        edge->setId ( i );
        edge->setVertex ( 0, pose );
        edge->camera_ = curr_->camera_.get();
	//index表示局内点的索引，也就是符合模型的点的索引
        edge->point_ = Vector3d ( pts3d[index].x, pts3d[index].y, pts3d[index].z );
        edge->setMeasurement ( Vector2d ( pts2d[index].x, pts2d[index].y ) );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        // set the inlier map points 
	//这个参数用于计算匹配率
        match_3dpts_[index]->matched_times_++;
    }

    optimizer.initializeOptimization();
    optimizer.optimize ( 10 );

    //经过BA优化之后的TCW
    T_c_w_estimated_ = SE3 (
        pose->estimate().rotation(),
        pose->estimate().translation());
    
    //为啥要.matrix(),？？如果不加就变成了2*3的了，第一行表示旋转向量，第二行表示平移向量
    //cout<<"T_c_w_estimated_: "<<endl<<T_c_w_estimated_<<endl;
    cout<<"T_c_w_estimated_.matrix: "<<endl<<T_c_w_estimated_.matrix()<<endl;
}

bool VisualOdometry::checkEstimatedPose()
{
    // check if the estimated pose is good
    if ( num_inliers_ < min_inliers_ )
    {
        cout<<"reject because inlier is too small: "<<num_inliers_<<endl;
        return false;
    }
    // if the motion is too large, it is probably wrong
    //ref表示上一帧，即Trw*Twc=Trc，即上一帧到当前帧的T
    SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
    //求se3李代数
    Sophus::Vector6d d = T_r_c.log();
    if ( d.norm() > 5.0 )
    {
        cout<<"reject because motion is too large: "<<d.norm() <<endl;
        return false;
    }
    return true;
}

bool VisualOdometry::checkKeyFrame()
{
  //ref表示上一帧，即Trw*Twc=Trc，即上一帧到当前帧的T
    SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
    //求se3李代数
    Sophus::Vector6d d = T_r_c.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    //key_frame_min_rot和key_frame_min_trans在vo构造函数中被赋值
    //即只要旋转或者平移超过一定距离就可以被认为是关键帧
    if ( rot.norm() >key_frame_min_rot || trans.norm() >key_frame_min_trans )
        return true;
    return false;
}

//新增，用于增加关键帧，第一帧就是关键帧
//其中的map_成员变量在构造函数中被复制，一开始应该是空的
void VisualOdometry::addKeyFrame()
{
     //keyframes_为map容器类对象，empty（）方法返回容器是否为空
     //第一帧的时候肯定是空的，在提取第一帧的特征点之后，将第一帧的所有特征点放入地图中
    if ( map_->keyframes_.empty() )
    {
        // first key-frame, add all 3d points into map
        //在使用之前，已经检测出来了keypoints_curr_
      for ( size_t i=0; i<keypoints_curr_.size(); i++ )
        {
	  //curr帧对象已经包含了depth_，即深度图像矩阵
	  //算出当前帧的关键点的深度
            double d = curr_->findDepth ( keypoints_curr_[i] );
            if ( d < 0 ) 
                continue;
	    //在第一帧的时候ref=curr=pframe，其中pframe包含了相机参数
	    //一开始的Tcw会自动变成单位矩阵
            Vector3d p_world = ref_->camera_->pixel2world (
                Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), 
							   curr_->T_c_w_, d
            );
	    
	    //好象是参考帧的相机中心到当前的关键点对应的三维点的位移
            Vector3d n = p_world - ref_->getCamCenter();
           //归一化，单位化
	    n.normalize();
	    
	    //createMapPoint函数返回托管Mappoint类的对象的指针
	    //这个用于将第一帧的点坐标，描述子，frame等信息传给MapPoint类，保存在这里
	    // curr_.get()，智能指针的get() 返回curr托管的Frame类的指针
            MapPoint::Ptr map_point = MapPoint::createMapPoint(
                p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
            );
	    //将包含信息的map_point插入容器中
	    //在insertMapPoint这里会输出关键帧的个数
            map_->insertMapPoint( map_point );
        }
    }
    //map有两个功能，管理地图中的点，和关键帧
    //如果不是第一帧直接执行这里
    map_->insertKeyFrame ( curr_ );
    ref_ = curr_;
}

//新增函数，增加地图中的点。局部地图类似于slidewindow一样，随时的增删地图中的点，来跟随运动
//后面如果发现地图中的点的数量不够，会调用它
void VisualOdometry::addMapPoints()
{
    // add the new map points into map
    //创建一个bool型的数组matched，大小为当前keypoints数组大小，值全为false
    vector<bool> matched(keypoints_curr_.size(), false); 
    //首先这个match_2dkp_index_是新来的当前帧跟地图匹配时，好的匹配到的关键点在keypoins数组中的索引
    //在这里将已经匹配的keypoint索引置为true，因为之后增加的肯定是之前没匹配的
    for ( int index:match_2dkp_index_ )
        matched[index] = true;
    
    //遍历当前keypoints数组，然后将深度大于0的关键点都加入地图中
    for ( int i=0; i<keypoints_curr_.size(); i++ )
    {
       //如果为true，说明在地图中找到了匹配，也就意味着地图中已经有这个点了。直接continue
        if ( matched[i] == true )   
            continue;
	
	//如果没有continue的话，说明这个点在地图中没有找到匹配，认定为新的点，
        //下一步就是找到depth数据，构造3D点，然后构造地图点，添加进地图即可。
        double d = ref_->findDepth ( keypoints_curr_[i] );
        if ( d<0 )  
            continue;
        Vector3d p_world = ref_->camera_->pixel2world (
            Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), 
            curr_->T_c_w_, d);
	
        Vector3d n = p_world - ref_->getCamCenter();
        n.normalize();
        MapPoint::Ptr map_point = MapPoint::createMapPoint(
            p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
	//将地图中原来没有的点添加进地图
        map_->insertMapPoint( map_point );
	
    }
}

//新增函数：优化地图。主要是为了维护地图的规模。删除一些地图点，在点过少时增加地图点等操作。
void VisualOdometry::optimizeMap()
{
    // remove the hardly seen and no visible points 
  //利用auto自动为变量选取类型，在for循环的时候适合使用auto
    for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
    {
        
      //iter->second表示map容器中元素的值vlaue，由于值为mappoint类指针，因此还可以指向类的成员变量pos_
      //判断世界坐标系的点是否在当前good_画面的视野中
      //如果点在当前帧都不可见了，说明跑的比较远了，删掉
        if ( !curr_->isInFrame(iter->second->pos_) )
        {
	  //删除掉iter指向的这个元素，返回下一个元素的迭代器
            iter = map_->map_points_.erase(iter);
            continue;
        }
        //定义匹配率，用匹配次数/可见次数，匹配率过低说明经常见但是没有几次匹配。
        //应该是一些比较难识别的点，也就是出来的描述子比价奇葩。所以删掉
        float match_ratio = float(iter->second->matched_times_)/iter->second->visible_times_;
        if ( match_ratio < map_point_erase_ratio_ )
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        
        //得到当前帧和地图点之间的夹角，角度过大，删除
        double angle = getViewAngle( curr_, iter->second );
        if ( angle > M_PI/6. )
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
         //继续，可以根据一些其他条件自己添加要删除点的情况
        if ( iter->second->good_ == false )
        {
            // TODO try triangulate this map point 
        }
        iter++;
    }
    
    //下面说一些增加点的情况，首先当前帧去地图中匹配时，点少于100个了，
    // 一般情况是运动幅度过大了，跟之前的帧没多少交集了，所以增加一下。
    if ( match_2dkp_index_.size()<100 )
        addMapPoints();
    //如果点过多了，多于1000个，适当增加释放率，让地图处于释放点的趋势。
    if ( map_->map_points_.size() > 1000 )  
    {
        // TODO map is too large, remove some one 
        map_point_erase_ratio_ += 0.05;
    }
    //如果没有多于1000个，保持释放率在0.1，维持地图的体量为平衡态
    else 
        map_point_erase_ratio_ = 0.1;
    
    cout<<"map points after optimized: "<<map_->map_points_.size()<<endl;
}

////取得一个空间点在一个帧下的视角角度。返回值是double类型的角度值。
double VisualOdometry::getViewAngle ( Frame::Ptr frame, MapPoint::Ptr point )
{
  //构造发方法是空间点坐标减去相机中心坐标。得到从相机中心指向指向空间点的向量。
    Vector3d n = point->pos_ - frame->getCamCenter();
    //单位化
    n.normalize();
    //返回一个角度，acos()为反余弦，
    //向量*乘为：a*b=|a||b|cos<a,b>
    //所以单位向量的*乘得到的是两个向量的余弦值，再用acos()即得到角度，返回
    //物理意义就是得到匆匆世界坐标系下看空间点和从相机坐标系下看空间点，视角差的角度。
    //norm表示相机的朝向
    return acos( n.transpose()*point->norm_ );
}


}