视觉SLAM从理论到实践：设计前端（2）

点击阅读第一部分：
视觉SLAM从理论到实践：设计前端（1）

基本的VO：特征提取和匹配

下面我们来实现VO，先来考虑特征点法。它的任务是，根据输入的图像，计算相机运动和特征点位置。前面我们讨论的都是在两两帧间的位姿估计，然而我们将发现，仅凭两帧的估计是不够的。我们会把特征点缓存成一个小地图，计算当前帧与地图之间的位置关系。但那样程序会复杂一些，所以，让我们先订个小目标，暂时从两两帧间的运动估计出发。

两两帧的视觉里程计

如果像前面两讲一样，只关心两个帧之间的运动估计，并且不优化特征点的位置。然后把估得的位姿“串”起来，也能得到一条运动轨迹。这种方式可以看成两两帧间的（Pairwise）无结构（Structureless）的VO，实现起来最为简单，但是效果不佳。为什么不佳呢？我们一起来体验一下。记该工程为0.2版本。
两两帧之间的VO工作示意图如 所示。在这种VO里，我们定义了参考帧（Reference）和当前帧（Current）这两个概念。以参考帧为坐标系，我们把当前帧与它进行特征匹配，并估计运动关系。假设参考帧相对世界坐标的变换矩阵为T_rw，当前帧与世界坐标系间为T_cw，则待估计的运动与这两个帧的变换矩阵构成左乘关系：

T_cr，s.t.T_cw=T_crT_cw.

在t-1到t时刻，我们以t-1为参考，求取t时刻的运动。这可以通过特征点匹配、光流或直接法得到，但这里我们只关心运动，不关心结构。换句话说，只要通过特征点成功求出了运动，我们就不再需要这一帧的特征点了。这种做法当然会有缺陷，但是忽略掉数量庞大的特征点可以节省许多的计算量。然后，在t到t+1时刻，我们又以t时刻为参考帧，考虑t到t+1间的运动关系。如此往复，就得到了一条运动轨迹。

图4 两两帧的VO示意图

这种VO的工作方式是简单的，不过实现也可以有若干种。我们以传统的匹配特征点——求PnP的方法为例实现一遍。希望读者能够结合之前几讲的知识，自己实现一下光流/直接法或ICP求运动的VO。在匹配特征点的方式中，最重要的是参考帧与当前帧之间的特征匹配关系，它的流程可归纳如下：

1. 对新来的当前帧，提取关键点和描述子。
2. 如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的3D位置，返回第1步。
3. 估计参考帧与当前帧间的运动。
4. 判断上述估计是否成功。
5. 若成功，把当前帧作为新的参考帧，返回第1步。
6. 若失败，计录连续丢失帧数。当连续丢失超过一定帧数时，置VO状态为丢失，算法结束。若未超过，返回第1步。

VisualOdometry类给出了上述算法的实现。

class VisualOdometry
{
public:
    typedef shared_ptr<VisualOdometry> Ptr;
    enum VOState {
        INITIALIZING=-1,
        OK=0,
        LOST
    };

    VOState     state_; // current VO status
    Map::Ptr    map_; // map with all frames and map points
    Frame::Ptr  ref_; // reference frame 
    Frame::Ptr  curr_; // current frame 

    cv::Ptr<cv::ORB> orb_; // orb detector and computer 
    vector<cv::Point3f> pts_3d_ref_; // 3d points in reference frame 
    vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
    Mat descriptors_curr_; // descriptor in current frame 
    Mat descriptors_ref_; // descriptor in reference frame 
    vector<cv::DMatch> feature_matches_;

    SE3 T_c_r_estimated_; // the estimated pose of current frame 
    int num_inliers_; // number of inlier features in icp
    int num_lost_; // number of lost times

    // parameters 
    int num_of_features_; // number of features
    double scale_factor_; // scale in image pyramid
    int level_pyramid_; // number of pyramid levels
    float match_ratio_; // ratio for selecting  good matches
    int max_num_lost_; // max number of continuous lost times
    int min_inliers_; // minimum inliers

    double key_frame_min_rot; // minimal rotation of two key-frames
    double key_frame_min_trans; // minimal translation of two key-frames

public: // functions 
    VisualOdometry();
    ~VisualOdometry();

    bool addFrame( Frame::Ptr frame );      // add a new frame 

protected:  
    // inner operation 
    void extractKeyPoints();
    void computeDescriptors(); 
    void featureMatching();
    void poseEstimationPnP(); 
    void setRef3DPoints();

    void addKeyFrame();
    bool checkEstimatedPose(); 
    bool checkKeyFrame();
};

关于这个VisualOdometry类，有几点需要解释：

• VO本身有若干种状态：设定第一帧、顺利跟踪或丢失，你可以把它看成一个有限状态机（Finite State Machine，FSM）。当然状态也可以有更多种，例如，单目VO至少还有一个初始化状态。在我们的实现中，考虑最简单的三个状态：初始化、正常、丢失。
• 我们把一些中间变量定义在类中，这样可省去复杂的参数传递。因为它们都是定义在类内部的，所以各个函数都可以访问它们。
• 特征提取和匹配当中的参数从参数文件中读取。例如：

    VisualOdometry::VisualOdometry() :
    state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 )
{
    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" );
    ...
}

• addFrame函数是外部调用的接口。使用VO时，将图像数据装入Frame类后，调用addFrame估计其位姿。该函数根据VO所处的状态实现不同的操作：

    bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
    switch ( state_ )
    {
    case INITIALIZING:
    {
        state_ = OK;
        curr_ = ref_ = frame;
        map_->insertKeyFrame ( frame );
        // extract features from first frame 
        extractKeyPoints();
        computeDescriptors();
        // compute the 3d position of features in ref frame 
        setRef3DPoints();
        break;
    }
    case OK:
    {
        curr_ = frame;
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP();
        if ( checkEstimatedPose() == true ) // a good estimation
        {
            curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;  // T_c_w = T_c_r*T_r_w 
            ref_ = curr_;
            setRef3DPoints();
            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;
}

值得一提的是，由于各种原因，我们设计的上述VO算法，每一步都有可能失败。例如，图片中不易提特征、特征点缺少深度值、误匹配、运动估计出错，等等。因此，要设计一个健壮的VO，必须（最好是显式地）考虑到上述所有可能出错的地方——那自然会使程序变得非常复杂。我们在checkEstimatedPose中根据内点（inlier）的数量及运动的大小做一个简单的检测：认为内点不可太少，而运动不可能过大。当然，读者也可以思考其他检测问题的手段，尝试一下效果。

我们略去VisualOdometry类其余的实现，读者可在GitHub上找到所有的源代码。最后，我们在test中加入该VO的测试程序，使用数据集观察估计的运动效果：

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);
    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++ )
    {
        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();

        // show the map and the camera pose 
        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);
        vis.setWidgetPose( "Camera", M);
        vis.spinOnce(1, false);
    }
    return 0;
}

为了运行这个程序，需要做几件事：

1. 因为我们用OpenCV 3的viz模块显示估计位姿，请确保你安装的是OpenCV 3，并且viz模块也已编译安装。
2. 准备TUM数据集中的其中一个。简单起见，笔者推荐fr1_xyz那一个。请使用associate.py生成一个配对文件associate.txt。关于TUM数据集格式，在 [sec:LKFlow] 节中已介绍。
3. 在config/default.yaml中填写你的数据集所在路径，参照笔者的写法即可。然后，用
   bin/run_vo config/default.yaml
   执行程序，就可以看到实时的演示了，如图5所示。

图5 0.2版本的VO演示

在演示程序中，你可以看到当前帧的图像与它的估计位置。我们画出了世界坐标系的坐标轴（大）与当前帧的坐标轴（小），颜色与轴的对应关系为：蓝色—Z，红，绿色—Y。你可以直观地感受到相机的运动，它与我们人类的感觉是大致相符的，尽管效果距离预想还有一定的差距。程序还输出了VO单次计算的用时，在笔者的机器上，能够以每次花费30ms左右的速度运行。减少特征点数量可以提高运算速度。读者可以修改运行参数和数据集，看看它在各种情况下的表现。

改进：优化PnP的结果

接下来，我们沿着之前的内容，尝试一些改进VO的方法。本节中，我们来尝试RANSAC PnP加上迭代优化的方式估计相机位姿，看看是否对前一节的效果有所改进。

非线性优化问题的求解，已经在第6讲和第7讲介绍过了。由于本节的目标是估计位姿而非结构，我们以相机位姿为优化变量，通过最小化重投影误差来构建优化问题。与之前一样，我们自定义一个g2o中的优化边。它只优化一个位姿，因此是一个一元边。

class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void computeError();
    virtual void linearizeOplus();

    virtual bool read( std::istream& in ){}
    virtual bool write(std::ostream& os) const {};

    Vector3d point_;
    Camera* camera_;
};

把三维点和相机模型放入它的成员变量中，方便计算重投影误差和雅可比矩阵：

void EdgeProjectXYZ2UVPoseOnly::computeError()
{
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
    _error = _measurement - camera_->camera2pixel ( 
        pose->estimate().map(point_) 
    );
}

void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
{
    g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
    g2o::SE3Quat T ( pose->estimate() );
    Vector3d xyz_trans = T.map ( point_ );
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z*z;

    _jacobianOplusXi ( 0,0 ) =  x*y/z_2 *camera_->fx_;
    _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
    _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
    _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
    _jacobianOplusXi ( 0,4 ) = 0;
    _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;

    _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
    _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
    _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
    _jacobianOplusXi ( 1,3 ) = 0;
    _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
    _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;
}

然后，在之前的PoseEstimationPnP函数中，修改成以RANSAC PnP结果为初值，再调用g2o进行优化的形式：

void VisualOdometry::poseEstimationPnP()
{
    ... 
    // 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_r_estimated_.rotation_matrix(), T_c_r_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();
        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 );
    }

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

    T_c_r_estimated_ = SE3 (
        pose->estimate().rotation(),
        pose->estimate().translation()
    );

请读者运行此程序，对比之前的结果。你将发现估计的运动明显稳定了很多。同时，由于新增的优化仍是无结构的，规模很小，对计算时间的影响基本可以忽略不计。整体的视觉里程计计算时间仍在30ms左右。

改进：局部地图

本节我们将VO匹配到的特征点放到地图中，并将当前帧与地图点进行匹配，计算位姿。这种做法与之前的差异如图6所示。

图6 两两帧VO与地图VO工作原理的差异

在两两帧间比较时，我们只计算参考帧与当前帧之间的特征匹配和运动关系，在计算之后把当前帧设为新的参考帧。而在使用地图的VO中，每个帧为地图贡献一些信息，比如，添加新的特征点或更新旧特征点的位置估计。地图中的特征点位置往往是使用世界坐标的。因此，当前帧到来时，我们求它和地图之间的特征匹配与运动关系，即直接计算。

这样做的好处是，我们能够维护一个不断更新的地图。只要地图是正确的，即使中间某帧出了差错，仍有希望求出之后那些帧的正确位置。请注意，我们现在还没有详细地讨论SLAM的建图问题，所以这里的地图仅是一个临时性的概念，指的是把各帧特征点缓存到一个地方构成的特征点的集合。

地图又可以分为局部（Local）地图和全局（Global）地图两种，由于用途不同，往往分开讨论。顾名思义，局部地图描述了附近的特征点信息——我们只保留离相机当前位置较近的特征点，而把远的或视野外的特征点丢掉。这些特征点是用来和当前帧匹配来求相机位置的，所以我们希望它能够做得比较快。另一方面，全局地图则记录了从SLAM运行以来的所有特征点。显然它的规模要大一些，主要用来表达整个环境，但是直接在全局地图上定位，对计算机的负担就太大了。它主要用于回环检测和地图表达。

在视觉里程计中，我们更关心可以直接用于定位的局部地图（如果决心要用地图的话）。所以本讲我们来维护一个局部地图。随着相机运动，我们往地图里添加新的特征点。我们仍然要提醒读者：是否使用地图取决于你对精度—效率这个矛盾的把握。我们完全可以出于效率的考量，使用两两无结构式的VO；也可以为了更好的精度，构建局部地图乃至考虑地图的优化。

局部地图的一件麻烦事是维护它的规模。为了保证实时性，我们需要保证地图规模不至于太大（否则匹配会消耗大量的时间）。此外，单个帧与地图的特征匹配存在着一些加速手段，但由于技术上比较复杂，我们的例程中就不给出了。

现在，来实现地图点类吧。我们稍加完善之前没有用到的MapPoint类，主要是它的构造函数和生成函数。

class MapPoint
{
    public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long      id_; // ID
    static unsigned long factory_id_; // factory id
    bool  good_; // wheter a good point 
    Vector3d pos_; // Position in world
    Vector3d norm_; // Normal of viewing direction 
    Mat descriptor_; // Descriptor for matching 

    list<Frame*> observed_frames_;   // key-frames that can observe this point 

    int matched_times_; // being an inliner in pose estimation
    int visible_times_;  // being visible in current frame 

    MapPoint();
    MapPoint ( 
        unsigned long id, 
        const Vector3d& position, 
        const Vector3d& norm, 
        Frame* frame=nullptr, 
        const Mat& descriptor=Mat() 
    );

    inline cv::Point3f getPositionCV() const {
        return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );
    }

    static MapPoint::Ptr createMapPoint();
    static MapPoint::Ptr createMapPoint ( 
        const Vector3d& pos_world, 
        const Vector3d& norm_,
        const Mat& descriptor,
        Frame* frame 
    );
};

主要的修改在VisualOdometry类上。由于工作流程的改变，我们修改了它的几个主要函数，例如，每次循环中要对地图进行增删、统计每个地图点被观测到的次数，等等 。这些事情是比较琐碎的，所以我们还是建议读者仔细看看GitHub上提供的源代码。重点观察以下几项：

• 在提取第一帧的特征点之后，将第一帧的所有特征点全部放入地图中：

    void VisualOdometry::addKeyFrame()
{
    if ( map_->keyframes_.empty() )
    {
        // first key-frame, add all 3d points into map
        for ( size_t i=0; i<keypoints_curr_.size(); i++ )
        {
            double d = curr_->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 );
        }
    }
    map_->insertKeyFrame ( curr_ );
    ref_ = curr_;
}

• 后续的帧中，使用OptimizeMap函数对地图进行优化。包括删除不在视野内的点，在匹配数量减少时添加新点，等等。

    void VisualOdometry::optimizeMap()
{
    // remove the hardly seen and no visible points 
    for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
    {
        if ( !curr_->isInFrame(iter->second->pos_) )
        {
            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++;
    }

    if ( match_2dkp_index_.size()<100 )
        addMapPoints();
    if ( map_->map_points_.size() > 1000 )  
    {
        // TODO map is too large, remove some one 
        map_point_erase_ratio_ += 0.05;
    }
    else 
        map_point_erase_ratio_ = 0.1;
    cout<<"map points: "<<map_->map_points_.size()<<endl;
}

我们刻意留空了一些地方，请感兴趣的读者自行完成。例如，你可以使用三角化来更新特征点的世界坐标，或者考虑更好地动态管理地图规模的策略。这些问题都是开放性的。

• 特征匹配代码。匹配之前，我们从地图中拿出一些候选点（出现在视野内的点），然后将它们与当前帧的特征描述子进行匹配。

    void VisualOdometry::featureMatching()
{
    boost::timer timer;
    vector<cv::DMatch> matches;
    // select the candidates in map 
    Mat desp_map;
    vector<MapPoint::Ptr> candidate;
    for ( auto& allpoints: map_->map_points_ )
    {
        MapPoint::Ptr& p = allpoints.second;
        // check if p in curr frame image 
        if ( curr_->isInFrame(p->pos_) )
        {
            // add to candidate 
            p->visible_times_++;
            candidate.push_back( p );
            desp_map.push_back( p->descriptor_ );
        }
    }

    matcher_flann_.match ( desp_map, descriptors_curr_, matches );
    // select the best matches
    float min_dis = std::min_element (
        matches.begin(), matches.end(),
        [] ( const cv::DMatch& m1, const cv::DMatch& m2 )
        {
            return m1.distance < m2.distance;
        } )->distance;

    match_3dpts_.clear();
    match_2dkp_index_.clear();
    for ( cv::DMatch& m : matches )
    {
        if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
        {
            match_3dpts_.push_back( candidate[m.queryIdx] );
            match_2dkp_index_.push_back( m.trainIdx );
        }
    }
    cout<<"good matches: "<<match_3dpts_.size() <<endl;
    cout<<"match cost time: "<<timer.elapsed() <<endl;
}

除了现有的地图之外，我们还引入了“关键帧”（Key-frame）的概念。关键帧在许多视觉SLAM中都会用到，不过这个概念主要是给后端用的，所以我们在后面几讲再讨论对关键帧的详细处理。在实践中，我们肯定不希望对每个图像都做详细的优化和回环检测，那样毕竟太耗费资源。至少相机搁在原地不动时，我们不希望整个模型（地图也好、轨迹也好）变得越来越大。因此，后端优化的主要对象就是关键帧。

关键帧是相机运动过程当中某几个特殊的帧，这里“特殊”的意义是可以由我们自己指定的。常见的做法时，每当相机运动经过一定间隔，就取一个新的关键帧并保存起来 。这些关键帧的位姿将被仔细优化，而位于两个关键帧之间的那些东西，除了对地图贡献一些地图点外，就被理所当然地忽略掉了。

本节的实现也会提取一些关键帧，为后端的优化做一些数据上的准备。现在，读者可以编译这个工程，看看它的运行结果。本节的例程会把局部地图的点投影到图像平面并显示出来。如果位姿估计正确，它们看起来应该像是固定在空间中一样。反之，如果你感觉到某个特征点不自然地运动，那可能是相机位姿估计不够准确，或特征点的位置不够准确。

我们在0.4版没有提供对地图的优化，建议读者自行尝试一下。用到的原理主要是最小二乘和三角化，在前两讲已介绍过，不会太困难。

图7 0.4版VO的运行截图，在两个不同时刻标记了路标投影点

小结

作为实践，本讲带领读者从零开始实现了一个简单的视觉里程计，为的是让读者对前面两讲介绍的算法有一个经验上的认识。如果没有本讲，你很难亲身体会到例如“特征点的VO大约能够实时处理多少个ORB特征点”这样的问题 。我们看到，视觉里程计能够估算局部时间内的相机运动及特征点的位置，但是这种局部的方式有明显的缺点：

1. 容易丢失。一旦丢失，我们要么“等待相机转回来”（保存参考帧并与新的帧比较），要么重置整个VO以跟踪新的图像数据。
2. 轨迹漂移。主要原因是每次估计的误差会累积至下一次估计，导致长时间轨迹不准确。大一点的局部地图可以缓解这种现象，但它始终是存在的。

值得一提的是，如果只关心短时间内的运动，或者VO的精度已经满足应用需求，那么有时候你可能需要的仅仅就是一个视觉里程计，而不用完全的SLAM。比如，某些无人机控制或AR游戏的应用中，用户并不需要一个全局一致的地图，那么轻便的VO可能是更好的选择。不过，本书的目标是介绍整个SLAM，所以我们还要走得更远一些，看看后端和回环检测是如何工作的。

作者：高翔，张涛，刘毅，颜沁睿。
编者按：本文节选自图书《视觉SLAM十四讲：从理论到实践》，系统介绍了视觉SLAM（同时定位与地图构建）所需的基本知识与核心算法，既包括数学理论基础，又包括计算机视觉的算法实现。此外，还提供了大量的实例代码供读者学习研究，从而更深入地掌握这些内容。

---

在线直播 | 人工智能核心技术解析与应用实战峰会由CSDN学院倾力打造，力邀一线公司技术骨干做深度解读。本期直播（5月13日）邀请来自阿里巴巴、思必驰、第四范式、一点资讯、58集团、PercepIn等在AI领域有着领先技术研究的一批专家，他们将针对人脸识别、卷积神经网络、大规模分布式机器学习系统搭建、推荐系统、自然语言处理及SLAM在机器人领域应用等热点话题进行分享。限时特惠199元即可听6位技术专家的在线分享，欢迎报名参加，加微信csdncxrs入群交流。