1.特征提取和匹配
前面我们所说的,仅凭俩帧的估计是不够的。我们会将特征点缓存成一个小地图,计算当前帧和地图之间的位置关系。
忽略掉很多关键点,只关心当前帧与上一时刻帧(即参考帧),即我们只考虑如下图所示的运动:
以传统的匹配特征点(ORB-slam)——求PnP的方法(接下来可以使用光流法/直接法——ICP求解ICP),流程如下图:
1.对新来的当前帧,提取关键点和描述子。
2.如果系统未初始化,以该帧为参考帧,根据深度图计算关键点的位置(为什么要深度图?其他不可以吗),返回第一步。(该步是防止没有参考帧)
3.估计参考帧与当前帧间的运动。
4.判断上述估计是否成功。
5.若成功,把当前帧作为新的参考帧,返回第一步。
6.若失败,记录连续丢失帧数。当丢失超过一定帧数时,置VO状态为丢失,算法结束。若未超过,返回第一步。
visual_odometry.h
#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;//定义了一个智能指针
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();
};
}
#endif // VISUALODOMETRY_H