0.3版本的VO仍受两帧间匹配的局限性影响，一旦视频序列的某帧丢失，导致后面匹配失败。
将VO匹配到的特征点放到地图中，并将当前帧与地图点进行匹配，计算和地图之间位姿。每一帧为地图贡献一些信息，比如添加新的特征点或更新旧特征点的位置估计。地图中的特征点一般都是使用世界坐标，所以当前帧到来时，匹配它和地图之间运动关系可以直接得到Tcw。
这样做的好处就是，维护一个不断更新的地图，只要地图正确，即使中间某帧出了问题，不至于崩溃。
另外地图分为局部和全局。这里我们实时定位的需要的是局部地图，全局地图一般用于回环检测和整体建图。
一、地图点Mappoint
头文件.h
地图点本质上就是空间中3D点，成员函数为地图点的基本信息。
有一个Frame*类型的 list，用于记录观察到此地图点的帧。list<Frame*> observed_frames_;
还有个取得地图点3维坐标的功能函数getPositionCV()直接写成内联了。还有个地图点生成函数createMapPoint()，传入成员变量所需要的信息就好了。
#ifndef MAPPOINT_H
#define MAPPOINT_H
#include "myslam/common_include.h"
namespace myslam
{
class Frame;
class MapPoint
{
public:
//1. 变量：ID、出厂ID、好(坏)点、3D坐标、正常视角、描述子
typedef shared_ptr<MapPoint> Ptr;
unsigned long id_;
static unsigned long factory_id_;
bool good_;
Vector3d pos_;
Vector3d norm_;
Mat descriptor_;
// 记录观察到此地图点的帧
list<Frame*> observed_frames_;
// 内点和当前帧可观测
int matched_times_; // being an inliner in pose estimation
int visible_times_; // being visible in current frame
//2.构造函数
MapPoint();
MapPoint( unsigned long id, const Vector3d& position,const Vector3d& norm,Frame* frame=nullptr,const Mat& descriptor=Mat() );
//3.内联成员函数，取得地图点3D坐标
inline cv::Point3f getPositionCV() const{return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );}
//4.地图点生成函数，参数（3D坐标，正常视角、描述子、关键帧）
static MapPoint::Ptr createMapPoint();
static MapPoint::Ptr createMapPoint( const Vector3d& pos_world,const Vector3d& norm_,const Mat& descriptor,Frame* frame );
};
}
#endif // MAPPOINT_H
.cpp
#include "myslam/common_include.h"
#include "myslam/mappoint.h"
namespace myslam
{
//1. 构造函数，参数(ID、3D坐标、正常视角、好(坏)点、内点、当前帧可观测)
MapPoint::MapPoint(): id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), good_(true), visible_times_(0), matched_times_(0){}
MapPoint::MapPoint ( long unsigned int id, const Vector3d& position, const Vector3d& norm, Frame* frame, const Mat& descriptor ): id_(id), pos_(position), norm_(norm), good_(true), visible_times_(1), matched_times_(1), descriptor_(descriptor)
{
observed_frames_.push_back(frame);
}
//2. 成员函数createMapPoint
MapPoint::Ptr MapPoint::createMapPoint()
{
return MapPoint::Ptr( new MapPoint( factory_id_++, Vector3d(0,0,0), Vector3d(0,0,0) ));
}
MapPoint::Ptr MapPoint::createMapPoint ( const Vector3d& pos_world, const Vector3d& norm, const Mat& descriptor, Frame* frame )
{
return MapPoint::Ptr( new MapPoint( factory_id_++, pos_world, norm, frame, descriptor ) );
}
unsigned long MapPoint::factory_id_ = 0;
}
二、VO
头文件.h
和之前相比变化为，
1、ORB部分去掉了参考帧3D点、描述子 2、匹配变成了地图点3D（match_3dpts_）和帧中关键点2d（match_2dkp_index_）
3、T变成了Tcw而不是之前的Tcr，当前帧直接和地图点区别绝对变化位姿，而非与参考帧之间找相对位姿变化
4、增加的函数：优化地图、添加地图点、取得视角
void optimizeMap();
void addMapPoints();
double getViewAngle( Frame::Ptr frame, MapPoint::Ptr point );#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H
#include "myslam/common_include.h"
#include "myslam/map.h"
#include <opencv2/features2d/features2d.hpp>
namespace myslam
{
class VisualOdometry
{
public:
typedef shared_ptr<VisualOdometry> Ptr;
//1. VO状态、地图、参考帧、当前帧
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 key-frame
Frame::Ptr curr_; // current frame
//2. 当前帧特征点（ORB部分去掉了参考帧3D点、描述子）
cv::Ptr<cv::ORB> orb_; // orb detector and computer
vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
Mat descriptors_curr_; // descriptor in current frame
//3. 匹配变成了地图点3D和帧中关键点2d
cv::FlannBasedMatcher matcher_flann_; // flann matcher
vector<MapPoint::Ptr> match_3dpts_; // matched 3d points
vector<int> match_2dkp_index_; // matched 2d pixels (index of kp_curr)
// 4. T变成了Tcw而不是之前的Tcr
SE3 T_c_w_estimated_; // the estimated pose of current frame
int num_inliers_; // number of inlier features in icp
int num_lost_; // number of lost times
// 5. 参数
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
double map_point_erase_ratio_; // remove map point ratio
public:
//构造函数析构函数,添加关键帧
VisualOdometry();
~VisualOdometry();
bool addFrame( Frame::Ptr frame ); // add a new frame
protected:
void extractKeyPoints();
void computeDescriptors();
void featureMatching();
void poseEstimationPnP();
//增加的优化地图函数optimizeMap()
void optimizeMap();
void addKeyFrame();
//增加的添加地图点函数addMapPoints()
void addMapPoints();
bool checkEstimatedPose();
bool checkKeyFrame();
//增加的取得视角函数getViewAngle(frame,point)
double getViewAngle( Frame::Ptr frame, MapPoint::Ptr point );
};
}
#endif // VISUALODOMETRY_H.cpp
1、addFrame函数，没有依赖参考帧，删除了setRef3DPoint函数，多了和地图之间操作
bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
switch ( state_ )
{
case INITIALIZING:
{
state_ = OK;
curr_ = ref_ = frame;
//提取特征加入地图map
extractKeyPoints();
computeDescriptors();
//第一帧就是关键帧，之前用map中insertKeyFrame函数
addKeyFrame();
break;
}
case OK:
{
curr_ = frame;
//新帧先赋值为参考帧的位姿T
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;
}2、featureMatching特征匹配函数，先从地图中拿出一些候选点（出现在视野内的点），然后将他们与当前帧的特征描述子进行匹配。
void VisualOdometry::featureMatching()
{
boost::timer timer;
vector<cv::DMatch> matches;
//1. 选出视野内候选点
//建立装描述子的目标图，匹配需要的是描述子
Mat desp_map;
//建立候选地图点数组
vector<MapPoint::Ptr> candidate;
//检查地图点是否是匹配需要的（地图点在当前帧可观察到）
for ( auto& allpoints: map_->map_points_ )
{
//把地图点取出，赋值给p
MapPoint::Ptr& p = allpoints.second;
// isInFrame函数检查p是否在当前帧可看到
if ( curr_->isInFrame(p->pos_) )
{
//观察次数加一
p->visible_times_++;
//关键点进candidate,描述子进desp_map
candidate.push_back( p );
desp_map.push_back( p->descriptor_ );
}
}
// 2. 候选点描述子地图与当前帧进行匹配
matcher_flann_.match ( desp_map, descriptors_curr_, matches );
// 3. 找最小距离
float min_dis = std::min_element (
matches.begin(), matches.end(),
[] ( const cv::DMatch& m1, const cv::DMatch& m2 )
{
return m1.distance < m2.distance;
} )->distance;
// 4. candidate只是中间存储，新帧会刷新。
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;
}3、addKeyFrame()函数增加关键帧函数多了一步在第一帧时，将其对应的地图点全部添加进地图中。
void VisualOdometry::addKeyFrame()
{
//1. 在第一帧时，将地图点全部添加进地图中
if ( map_->keyframes_.empty() )
{
for ( size_t i=0; i<keypoints_curr_.size(); i++ )
{
double d = curr_->findDepth ( keypoints_curr_[i] );
if ( d < 0 )
continue;
//先找深度d,像素转3D世界坐标
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();
//构造一个地图点，3D点、模长、描述子、帧
MapPoint::Ptr map_point = MapPoint::createMapPoint(p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
//添加进地图
map_->insertMapPoint( map_point );
}
}
//2. 添加关键帧到地图中
map_->insertKeyFrame ( curr_ );
ref_ = curr_;
}4、addMapPoints（）新增函数
分为两部分，一为匹配matches在match_2dkp_index_和地图匹配中索引为true
二为添加地图点mappoint进入地图中
void VisualOdometry::addMapPoints()
{
//1. bool类型数组matched,值为false
vector<bool> matched(keypoints_curr_.size(), false);
//match_2dkp_index_是当前帧与地图匹配到的关键点在keypoints数组中索引
//设置匹配好的关键点索引为true.
for ( int index:match_2dkp_index_ )
matched[index] = true;
//2. 和addKeyFrame中添加地图点一样
for ( int i=0; i<keypoints_curr_.size(); i++ )
{
if ( matched[i] == true )
continue;
//true,在地图中有这个点匹配上，找深度d,构造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 );
}
}5、optimizeMap()新增函数，维护地图的规模。删除一些地图点，在点过少时增加地图点等操作。
void VisualOdometry::optimizeMap()
{
//1. 删除地图点
for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
{
//1.1 当前帧看不见该点，删除
if ( !curr_->isInFrame(iter->second->pos_) )
{
iter = map_->map_points_.erase(iter);
continue;
}
//1.2 匹配率=匹配次数/可见次数，过低删除
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;
}
// 1.3 角度大于
double angle = getViewAngle( curr_, iter->second );
if ( angle > M_PI/6. )
{
iter = map_->map_points_.erase(iter);
continue;
}
// 1.4 不是好点
if ( iter->second->good_ == false )
{
// TODO try triangulate this map point
}
iter++;
}
// 2. 增加点
// 2.1 当前帧与地图匹配点少于100个，
if ( match_2dkp_index_.size()<100 )
addMapPoints();
// 2.2 点太多于1000，增加释放率，否则维持0.1
if ( map_->map_points_.size() > 1000 )
{
map_point_erase_ratio_ += 0.05;
}
else
map_point_erase_ratio_ = 0.1;
cout<<"map points: "<<map_->map_points_.size()<<endl;
}6、getViewAngle（）取得一个空间点在一个帧下的视角角度
double VisualOdometry::getViewAngle ( Frame::Ptr frame, MapPoint::Ptr point )
{
//空间点坐标减去相机中心坐标，得到从相机中心指向空间点的向量
Vector3d n = point->pos_ - frame->getCamCenter();
//单位化
n.normalize();
//返回角度，反余弦
return acos( n.transpose()*point->norm_ );
}