视觉SLAM十四讲第九章0.4代码注释
文中的主要代码引用自高翔博士的《视觉slam十四讲》，注释部分转载自链接视觉SLAM十四讲第九章0.4代码注释
- run_vo.cpp
// -------------- test the visual odometry -------------
#include - visual_odometry.cpp
#include
cout<<"descriptor computation cost time: "<<timer.elapsed() <<endl;
}
void VisualOdometry::featureMatching()
{
boost::timer timer;
vector<cv::DMatch> matches;
//建立一个保存描述子的map矩阵，保存匹配需要地图点的描述子，
//因为描述子是一个行向量
Mat desp_map;
// select the candidates in map
//candidate的生命周期只限于这个函数，暂时存放在当前帧视野以内的mappoint
vector<MapPoint::Ptr> candidate;
//遍历所有地图点，将符合条件的mappoint放入candidate，将描述子信息放入desp_map
for ( auto& allpoints: map_->map_points_ )
{
MapPoint::Ptr& p = allpoints.second;
// 检测这个点（世界坐标系中）是否在当前帧的视野之内
if ( curr_->isInFrame(p->pos_) )
{
// add to candidate
p->visible_times_++;
candidate.push_back( p );
//Mat也可以利用push_back来插入一行，和vector类似
desp_map.push_back( p->descriptor_ );
}
}
//采用新的匹配方法FlannBasedMatcher(最近邻近似匹配)，而不是暴力匹配BFMatcher；
//这步匹配中，第一个参数由原来的参考帧，变成了上面定义的desp_map地图，进行匹配。
//也就是当前帧直接和地图进行匹配
matcher_flann_.match ( desp_map, descriptors_curr_, matches );
// select the best matches
//找出容器matches中最小的元素的指针或迭代器
//distance变量返回两个描述子之间的距离
//这里用到了lambda表达式，也就是这里面的第三个参数
//可以隐藏返回值的类型，会根据return自动确定，这里又加上了
float min_dis = std::min_element (
matches.begin(), matches.end(),
[] ( const cv::DMatch& m1, const cv::DMatch& m2 )->bool
{
return m1.distance < m2.distance;
} )->distance;
//保存已经匹配的MapPoint（MapPoint的指针类型）
match_3dpts_.clear();
//保存已经匹配的2d关键点的索引
match_2dkp_index_.clear();
for ( cv::DMatch& m : matches )
{
//如果描述子之间的距离小于一个值（30和min_dis*match_ratio_中较大的），则表示匹配成功
if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
{
//会匹配成功很多，但只有一部分符合条件，将符合条件的这些特征点放入容器
//queryIdx表示参考帧的匹配成功的索引；trainIdx表示当前帧的匹配成功的索引
match_3dpts_.push_back( candidate[m.queryIdx] );
match_2dkp_index_.push_back( m.trainIdx );
}
}
//平均每帧的好的匹配点大概100~200个左右
cout<<"good matches: "<<match_3dpts_.size() <<endl;
cout<<"match cost time: "<<timer.elapsed() <<endl;
}
//这里删除了setRef3DPoints()函数，用不到了，因为我们当前帧不需要和参考帧对比，而是直接和地图对比
//利用和地图匹配得到的信息（match_3dpts_）来进行pnp估计位姿
void VisualOdometry::poseEstimationPnP()
{
// construct the 3d 2d observations
vector<cv::Point3f> pts3d;
vector<cv::Point2f> pts2d;
//取出当前帧匹配成功的2d点
for ( int index:match_2dkp_index_ )
{
pts2d.push_back ( keypoints_curr_[index].pt );
}
//取出参考帧（实际上是map里面）的匹配成功的点
for ( MapPoint::Ptr pt:match_3dpts_ )
{
//getPositionCV为inline函数，因为短小且经常被调用
//getPositionCV函数得到3d点的坐标，因为MapPoint类里面包含了很多信息，世界坐标系的坐标也在其中
pts3d.push_back( pt->getPositionCV() );
}
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;
//得到rvec就是旋转矢量，输出的inlier就是局内点的索引，也就是符合模型的数据的索引，是一个列向量
//一般的pnp我们用的是最小二乘法，而这里用的是ransac
/*参数定义
* objectPoints，要匹配的3d空间点数组
* objectPoints，要匹配的2d图像点数组
* cameraMatrix，相机内参矩阵
* distCoeffs，相机畸变矩阵
* rvec，旋转向量输出承接矩阵
* tvec，平移向量输出承接矩阵
* 后面的参数跟ransac算法有关。倒是都有默认值
* useExtrinsicGuess，迭代初始值是否定为提供的rvec和tvec值，这里没有提供，所以用false
* iterationsCount，迭代次数，ransac算法所必须的迭代次数
* reprojectionError，重投影误差。ransac算法迭代时也必须要规定的误差阀值，来确定是否为内点。
* confidence，置信度。ransac算法每次用于更新迭代次数的参数。一般固定选为0.995
* 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;
//看一下
//cout<
//将旋转矢量和平移转换成变换矩阵，初始值，之后还要优化
T_c_w_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 ) )
);
//上面ransac会输出符合模型的点的索引，下面对这些符合模型的点进行BA
// 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_w_estimated_.rotation_matrix(), T_c_w_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();
//index表示局内点的索引，也就是符合模型的点的索引
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 );
// set the inlier map points
//这个参数用于计算匹配率
match_3dpts_[index]->matched_times_++;
}
optimizer.initializeOptimization();
optimizer.optimize ( 10 );
//经过BA优化之后的TCW
T_c_w_estimated_ = SE3 (
pose->estimate().rotation(),
pose->estimate().translation());
//为啥要.matrix(),？？如果不加就变成了2*3的了，第一行表示旋转向量，第二行表示平移向量
//cout<<"T_c_w_estimated_: "<
cout<<"T_c_w_estimated_.matrix: "<<endl<<T_c_w_estimated_.matrix()<<endl;
}
bool VisualOdometry::checkEstimatedPose()
{
// check if the estimated pose is good
if ( num_inliers_ < min_inliers_ )
{
cout<<"reject because inlier is too small: "<<num_inliers_<<endl;
return false;
}
// if the motion is too large, it is probably wrong
//ref表示上一帧，即Trw*Twc=Trc，即上一帧到当前帧的T
SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
//求se3李代数
Sophus::Vector6d d = T_r_c.log();
if ( d.norm() > 5.0 )
{
cout<<"reject because motion is too large: "<<d.norm() <<endl;
return false;
}
return true;
}
bool VisualOdometry::checkKeyFrame()
{
//ref表示上一帧，即Trw*Twc=Trc，即上一帧到当前帧的T
SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
//求se3李代数
Sophus::Vector6d d = T_r_c.log();
Vector3d trans = d.head<3>();
Vector3d rot = d.tail<3>();
//key_frame_min_rot和key_frame_min_trans在vo构造函数中被赋值
//即只要旋转或者平移超过一定距离就可以被认为是关键帧
if ( rot.norm() >key_frame_min_rot || trans.norm() >key_frame_min_trans )
return true;
return false;
}
//新增，用于增加关键帧，第一帧就是关键帧
//其中的map_成员变量在构造函数中被复制，一开始应该是空的
void VisualOdometry::addKeyFrame()
{
//keyframes_为map容器类对象，empty（）方法返回容器是否为空
//第一帧的时候肯定是空的，在提取第一帧的特征点之后，将第一帧的所有特征点放入地图中
if ( map_->keyframes_.empty() )
{
// first key-frame, add all 3d points into map
//在使用之前，已经检测出来了keypoints_curr_
for ( size_t i=0; i<keypoints_curr_.size(); i++ )
{
//curr帧对象已经包含了depth_，即深度图像矩阵
//算出当前帧的关键点的深度
double d = curr_->findDepth ( keypoints_curr_[i] );
if ( d < 0 )
continue;
//在第一帧的时候ref=curr=pframe，其中pframe包含了相机参数
//一开始的Tcw会自动变成单位矩阵
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();
//createMapPoint函数返回托管Mappoint类的对象的指针
//这个用于将第一帧的点坐标，描述子，frame等信息传给MapPoint类，保存在这里
// curr_.get()，智能指针的get() 返回curr托管的Frame类的指针
MapPoint::Ptr map_point = MapPoint::createMapPoint(
p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
);
//将包含信息的map_point插入容器中
//在insertMapPoint这里会输出关键帧的个数
map_->insertMapPoint( map_point );
}
}
//map有两个功能，管理地图中的点，和关键帧
//如果不是第一帧直接执行这里
map_->insertKeyFrame ( curr_ );
ref_ = curr_;
}
//新增函数，增加地图中的点。局部地图类似于slidewindow一样，随时的增删地图中的点，来跟随运动
//后面如果发现地图中的点的数量不够，会调用它
void VisualOdometry::addMapPoints()
{
// add the new map points into map
//创建一个bool型的数组matched，大小为当前keypoints数组大小，值全为false
vector<bool> matched(keypoints_curr_.size(), false);
//首先这个match_2dkp_index_是新来的当前帧跟地图匹配时，好的匹配到的关键点在keypoins数组中的索引
//在这里将已经匹配的keypoint索引置为true，因为之后增加的肯定是之前没匹配的
for ( int index:match_2dkp_index_ )
matched[index] = true;
//遍历当前keypoints数组，然后将深度大于0的关键点都加入地图中
for ( int i=0; i<keypoints_curr_.size(); i++ )
{
//如果为true，说明在地图中找到了匹配，也就意味着地图中已经有这个点了。直接continue
if ( matched[i] == true )
continue;
//如果没有continue的话，说明这个点在地图中没有找到匹配，认定为新的点，
//下一步就是找到depth数据，构造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 );
}
}
//新增函数：优化地图。主要是为了维护地图的规模。删除一些地图点，在点过少时增加地图点等操作。
void VisualOdometry::optimizeMap()
{
// remove the hardly seen and no visible points
//利用auto自动为变量选取类型，在for循环的时候适合使用auto
for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
{
//iter->second表示map容器中元素的值vlaue，由于值为mappoint类指针，因此还可以指向类的成员变量pos_
//判断世界坐标系的点是否在当前good_画面的视野中
//如果点在当前帧都不可见了，说明跑的比较远了，删掉
if ( !curr_->isInFrame(iter->second->pos_) )
{
//删除掉iter指向的这个元素，返回下一个元素的迭代器
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++;
}
//下面说一些增加点的情况，首先当前帧去地图中匹配时，点少于100个了，
// 一般情况是运动幅度过大了，跟之前的帧没多少交集了，所以增加一下。
if ( match_2dkp_index_.size()<100 )
addMapPoints();
//如果点过多了，多于1000个，适当增加释放率，让地图处于释放点的趋势。
if ( map_->map_points_.size() > 1000 )
{
// TODO map is too large, remove some one
map_point_erase_ratio_ += 0.05;
}
//如果没有多于1000个，保持释放率在0.1，维持地图的体量为平衡态
else
map_point_erase_ratio_ = 0.1;
cout<<"map points after optimized: "<<map_->map_points_.size()<<endl;
}
取得一个空间点在一个帧下的视角角度。返回值是double类型的角度值。
double VisualOdometry::getViewAngle ( Frame::Ptr frame, MapPoint::Ptr point )
{
//构造发方法是空间点坐标减去相机中心坐标。得到从相机中心指向指向空间点的向量。
Vector3d n = point->pos_ - frame->getCamCenter();
//单位化
n.normalize();
//返回一个角度，acos()为反余弦，
//向量*乘为：a*b=|a||b|cos