根据输入的图像,计算相机运动T和特征点位置3D坐标。
两两帧之间,只关心运动,不关心结构,把估计位姿串起来得到运动轨迹。
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;
//枚举表征VO状态,初始化、正常、丢失
enum VOState {
INITIALIZING=-1,
OK=0,
LOST
};
//VO状态、地图(关键帧和特征点)、参考帧、当前帧
VOState state_;
Map::Ptr map_;
Frame::Ptr ref_;
Frame::Ptr curr_;
//orb、当前帧特征点KeyPoint、当前帧描述子、参考帧3D坐标Point3f、参考帧描述子、匹配关系
cv::Ptr<cv::ORB> orb_;
vector<cv::Point3f> pts_3d_ref_;
vector<cv::KeyPoint> keypoints_curr_;
Mat descriptors_curr_;
Mat descriptors_ref_;
vector<cv::DMatch> feature_matches_;
//位姿估计T、内点数、丢失数
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
//参数,特征数量、尺度、好匹配比率、最大持续丢失时间、最小内点数
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;
double key_frame_min_trans;
public:
//构造函数析构函数
VisualOdometry();
~VisualOdometry();
//核心,添加新关键帧
bool addFrame( Frame::Ptr frame ); // add a new frame
protected:
//内部处理函数,特征匹配
void extractKeyPoints();
void computeDescriptors();
void featureMatching();
void poseEstimationPnP();
void setRef3DPoints();
//关键帧的功能函数
void addKeyFrame();
bool checkEstimatedPose();
bool checkKeyFrame();
};
}
#endif // VISUALODOMETRY_H
visual_odometry.cpp
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
namespace myslam
{
//构造函数,相机位姿初始化VOstate、当前帧、参考帧、地图、丢失点、内联点
VisualOdometry::VisualOdometry() :
state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 )
{
//特征数量、尺度、好匹配比率、最大持续丢失时间、最小内点数、关键帧旋转平移,orb初始化
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" );
orb_ = cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ );
}
VisualOdometry::~VisualOdometry(){}
//核心!!!添加关键帧
bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
switch ( state_ )
{
//1. 初始化状态,第一帧
case INITIALIZING:
{
state_ = OK;
//当前帧和参考帧都是第一帧
curr_ = ref_ = frame;
//将此帧插入地图中
map_->insertKeyFrame ( frame );
//提取关键点和描述子
extractKeyPoints();
computeDescriptors();
//参考帧特征点的3D坐标,补全depth数据
setRef3DPoints();
break;
}
//2. 匹配成功状态
case OK:
{
curr_ = frame;
extractKeyPoints();
computeDescriptors();
//特征匹配并计算相机位姿 T
featureMatching();
poseEstimationPnP();
//位姿估计好
if ( checkEstimatedPose() == true )
{
//当前位姿 Tcw = Tcr*Trw
curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;
//当前帧为参考帧
ref_ = curr_;
//参考帧特征点的3D坐标,补全depth数据
setRef3DPoints();
num_lost_ = 0;
//检查是否为关键帧
if ( checkKeyFrame() == true ) // is a key-frame
{
addKeyFrame();
}
}
//位姿估计坏,丢失点计数+1,判断是否大于最大丢失数,是则VO状态切换为lost
else
{
num_lost_++;
if ( num_lost_ > max_num_lost_ )
{
state_ = LOST;
}
return false;
}
break;
}
//3. 匹配失败状态
case LOST:
{
cout<<"vo has lost."<<endl;
break;
}
}
return true;
}
//提取特征点
void VisualOdometry::extractKeyPoints()
{
orb_->detect ( curr_->color_, keypoints_curr_ );
}
//计算描述子
void VisualOdometry::computeDescriptors()
{
orb_->compute ( curr_->color_, keypoints_curr_, descriptors_curr_ );
}
//特征匹配
void VisualOdometry::featureMatching()
{
vector<cv::DMatch> matches;
cv::BFMatcher matcher ( cv::NORM_HAMMING );
matcher.match ( descriptors_ref_, descriptors_curr_, matches );
//找到matches数组中最小距离,赋值给min_dist
float min_dis = std::min_element (
matches.begin(), matches.end(),
[] ( const cv::DMatch& m1, const cv::DMatch& m2 )
{
return m1.distance < m2.distance;
} )->distance;
//根据距离筛选特征点
feature_matches_.clear();
for ( cv::DMatch& m : matches )
{
if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
{
feature_matches_.push_back(m);
}
}
cout<<"good matches: "<<feature_matches_.size()<<endl;
}
//pnp需要参考帧3D,当前帧2D,所以当前帧迭代为参考帧时,需要加上depth数据
void VisualOdometry::setRef3DPoints()
{
// select the features with depth measurements
pts_3d_ref_.clear();
descriptors_ref_ = Mat();
//遍历当前关键点数组遍历
for ( size_t i=0; i<keypoints_curr_.size(); i++ )
{
//找到depth数据
double d = ref_->findDepth(keypoints_curr_[i]);
if ( d > 0)
{
//像素坐标到相机坐标系3D坐标
Vector3d p_cam = ref_->camera_->pixel2camera(
Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d
);
//构造Point3f,3D坐标
pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) ));
//参考帧描述子,将当前帧描述子按行放进去。
descriptors_ref_.push_back(descriptors_curr_.row(i));
}
}
}
//PnP估计相机位姿T
void VisualOdometry::poseEstimationPnP()
{
vector<cv::Point3f> pts3d;
vector<cv::Point2f> pts2d;
//参考帧3D坐标和当前帧2D坐标(需要.pt像素坐标)
for ( cv::DMatch m:feature_matches_ )
{
pts3d.push_back( pts_3d_ref_[m.queryIdx] );
pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
}
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;
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;
//由旋转和平移构造出当前帧相对于参考帧的T
T_c_r_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))
);
}
//位姿检验模块,匹配点不能太少,运动不能太大
bool VisualOdometry::checkEstimatedPose()
{
//匹配点太少,false
if ( num_inliers_ < min_inliers_ )
{
cout<<"reject because inlier is too small: "<<num_inliers_<<endl;
return false;
}
//T的模太大,false
Sophus::Vector6d d = T_c_r_estimated_.log();
if ( d.norm() > 5.0 )
{
cout<<"reject because motion is too large: "<<d.norm()<<endl;
return false;
}
return true;
}
//检查关键帧,T中R或t比较大都是关键帧
bool VisualOdometry::checkKeyFrame()
{
Sophus::Vector6d d = T_c_r_estimated_.log();
Vector3d trans = d.head<3>();
Vector3d rot = d.tail<3>();
if ( rot.norm() >key_frame_min_rot || trans.norm() >key_frame_min_trans )
return true;
return false;
}
//添加关键帧
void VisualOdometry::addKeyFrame()
{
cout<<"adding a key-frame"<<endl;
map_->insertKeyFrame ( curr_ );
}
}