视觉SLAM从理论到实践：设计前端（2）
点击阅读第一部分：
视觉SLAM从理论到实践：设计前端（1）
基本的VO：特征提取和匹配
下面我们来实现VO，先来考虑特征点法。它的任务是，根据输入的图像，计算相机运动和特征点位置。前面我们讨论的都是在两两帧间的位姿估计，然而我们将发现，仅凭两帧的估计是不够的。我们会把特征点缓存成一个小地图，计算当前帧与地图之间的位置关系。但那样程序会复杂一些，所以，让我们先订个小目标，暂时从两两帧间的运动估计出发。
两两帧的视觉里程计
如果像前面两讲一样，只关心两个帧之间的运动估计，并且不优化特征点的位置。然后把估得的位姿“串”起来，也能得到一条运动轨迹。这种方式可以看成两两帧间的（Pairwise）无结构（Structureless）的VO，实现起来最为简单，但是效果不佳。为什么不佳呢？我们一起来体验一下。记该工程为0.2版本。
两两帧之间的VO工作示意图如 所示。在这种VO里，我们定义了参考帧（Reference）和当前帧（Current）这两个概念。以参考帧为坐标系，我们把当前帧与它进行特征匹配，并估计运动关系。假设参考帧相对世界坐标的变换矩阵为Trw，当前帧与世界坐标系间为Tcw，则待估计的运动与这两个帧的变换矩阵构成左乘关系：
Tcr，s.t.Tcw=TcrTcw.
在t-1到t时刻，我们以t-1为参考，求取t时刻的运动。这可以通过特征点匹配、光流或直接法得到，但这里我们只关心运动，不关心结构。换句话说，只要通过特征点成功求出了运动，我们就不再需要这一帧的特征点了。这种做法当然会有缺陷，但是忽略掉数量庞大的特征点可以节省许多的计算量。然后，在t到t+1时刻，我们又以t时刻为参考帧，考虑t到t+1间的运动关系。如此往复，就得到了一条运动轨迹。
这种VO的工作方式是简单的，不过实现也可以有若干种。我们以传统的匹配特征点——求PnP的方法为例实现一遍。希望读者能够结合之前几讲的知识，自己实现一下光流/直接法或ICP求运动的VO。在匹配特征点的方式中，最重要的是参考帧与当前帧之间的特征匹配关系，它的流程可归纳如下：
- 对新来的当前帧，提取关键点和描述子。
- 如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的3D位置，返回第1步。
- 估计参考帧与当前帧间的运动。
- 判断上述估计是否成功。
- 若成功，把当前帧作为新的参考帧，返回第1步。
- 若失败，计录连续丢失帧数。当连续丢失超过一定帧数时，置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;
}
为了运行这个程序，需要做几件事：
- 因为我们用OpenCV 3的viz模块显示估计位姿，请确保你安装的是OpenCV 3，并且viz模块也已编译安装。
- 准备TUM数据集中的其中一个。简单起见，笔者推荐fr1_xyz那一个。请使用associate.py生成一个配对文件associate.txt。关于TUM数据集格式，在 [sec:LKFlow] 节中已介绍。
- 在config/default.yaml中填写你的数据集所在路径，参照笔者的写法即可。然后，用
bin/run_vo config/default.yaml
执行程序，就可以看到实时的演示了，如图5所示。
在演示程序中，你可以看到当前帧的图像与它的估计位置。我们画出了世界坐标系的坐标轴（大）与当前帧的坐标轴（小），颜色与轴的对应关系为：蓝色—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所示。
在两两帧间比较时，我们只计算参考帧与当前帧之间的特征匹配和运动关系，在计算之后把当前帧设为新的参考帧。而在使用地图的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版没有提供对地图的优化，建议读者自行尝试一下。用到的原理主要是最小二乘和三角化，在前两讲已介绍过，不会太困难。
小结
作为实践，本讲带领读者从零开始实现了一个简单的视觉里程计，为的是让读者对前面两讲介绍的算法有一个经验上的认识。如果没有本讲，你很难亲身体会到例如“特征点的VO大约能够实时处理多少个ORB特征点”这样的问题 。我们看到，视觉里程计能够估算局部时间内的相机运动及特征点的位置，但是这种局部的方式有明显的缺点：
- 容易丢失。一旦丢失，我们要么“等待相机转回来”（保存参考帧并与新的帧比较），要么重置整个VO以跟踪新的图像数据。
- 轨迹漂移。主要原因是每次估计的误差会累积至下一次估计，导致长时间轨迹不准确。大一点的局部地图可以缓解这种现象，但它始终是存在的。
值得一提的是，如果只关心短时间内的运动，或者VO的精度已经满足应用需求，那么有时候你可能需要的仅仅就是一个视觉里程计，而不用完全的SLAM。比如，某些无人机控制或AR游戏的应用中，用户并不需要一个全局一致的地图，那么轻便的VO可能是更好的选择。不过，本书的目标是介绍整个SLAM，所以我们还要走得更远一些，看看后端和回环检测是如何工作的。
作者：高翔，张涛，刘毅，颜沁睿。
编者按：本文节选自图书《视觉SLAM十四讲：从理论到实践》，系统介绍了视觉SLAM（同时定位与地图构建）所需的基本知识与核心算法，既包括数学理论基础，又包括计算机视觉的算法实现。此外，还提供了大量的实例代码供读者学习研究，从而更深入地掌握这些内容。
在线直播 | 人工智能核心技术解析与应用实战峰会由CSDN学院倾力打造，力邀一线公司技术骨干做深度解读。本期直播（5月13日）邀请来自阿里巴巴、思必驰、第四范式、一点资讯、58集团、PercepIn等在AI领域有着领先技术研究的一批专家，他们将针对人脸识别、卷积神经网络、大规模分布式机器学习系统搭建、推荐系统、自然语言处理及SLAM在机器人领域应用等热点话题进行分享。限时特惠199元即可听6位技术专家的在线分享，欢迎报名参加，加微信csdncxrs入群交流。