手撕 视觉slam14讲 ch13 代码（6）正常跟踪模式 Track()
上一篇 双目初始化完成过程中，跟踪状态FrontendStatus变成了TRACKING_GOOD，因此之后，将进入 正常跟踪的Track() 函数：
Track函数的流程也很清晰：
- 首先，用匀速模型给当前帧的pose设置一个初值，这里用上一帧位姿获得当前位姿估计的初始值
- 然后，用光流法匹配前后两帧的特征点（前后两帧都只用左目图像），并返回光流法匹配到的点的数量，对应TrackLastFrame()函数
- 然后，根据匹配的特征点来优化当前帧的位置，对应EstimateCurrentPose() 函数。
- 之后，根据追踪到的内点来判断现在的追踪状态，内点数大于50为TRACKING_GOOD，小于50大于20为TRACKING_BAD，如果小于等于20，就是跟丢了。
- 然后，判断当前要不要插入关键帧，对应 InsertKeyframe() 函数
- 之后，更新当前帧和上一帧的位置差
- 最后，把当前帧送到viewer_线程里面。
//正常状态下的跟踪
bool Frontend::Track(){
// 用上一帧位姿获得当前位姿估计的初始值
// 假设匀速运动，即每两帧之间的位姿变换差不多，
// 以【上一帧位姿】乘以【上上帧与上一帧之间的相对运动】作为这次位姿的估计初值
if (last_frame_)
{
current_frame_->SetPose(relative_motion_ * last_frame_->Pose());
}
//用光流法匹配上一帧与当前帧，并返回光流法匹配到的点的数量
int num_track_last= TrackLastFrame();
// 修正当前帧的位姿估计，并返回追踪成功点数量（在光流匹配成功基础上，更信任的点）
tracking_inliers_=EstimateCurrentPose();
// 改变状态，为下次的添加帧做准备
// tracking good
if (tracking_inliers_>num_features_tracking_)
{
status_=FrontendStatus::TRACKING_GOOD;
}
// tracking bad
else if (tracking_inliers_>num_features_tracking_bad_)
{
status_=FrontendStatus::TRACKING_BAD;
}else // lost
{
status_=FrontendStatus::LOST;
}
//将当前帧插入关键帧
InsertKeyframe();
//当前帧位姿 = 上一帧的位姿 左乘 当前帧与上一帧的相对变换
//相对位姿变换 = 当前帧的位姿 * 上一帧位姿的逆
relative_motion_=current_frame_->Pose() * last_frame_->Pose().inverse();
if (viewer_)
{
viewer_->AddCurrentFrame(current_frame_);
return true;
}
}接下来我们具体看一下函数实现：
TrackLastFrame() 函数：
用光流法匹配上一帧与当前帧，并返回光流法匹配到的点的数量，这和前面左右目光流追踪FindFeaturesInRight() 函数基本一样，不同的是这里是上一帧和当前帧的左目图像来进行光流追踪，流程如下：
- 第一步：进行赋初值。如果当前特征点有在地图上有对应的点，那么根据特征点的3D位置和匀速模型算的一个初值POSE来反推像素位置。如果没有对应特征点，当前帧的特征点初值就是和上一帧一样。
- 第二步：调用OpenCV中的LK光流法来追踪右目特征点的位置
- 第三步：把匹配上的特征点 push_back 到当前帧的左目特征点里面去，没匹配上就放个空指针。
- 最后统计一下匹配上的特征点数目。
//用光流法匹配上一帧与当前帧，并返回光流法匹配到的点的数量
int Frontend::TrackLastFrame(){
// 赋初值
std::vector kps_last, kps_current;
for(auto &kp : last_frame_->features_left_){
// 如果这个点有对应的路标点，则当前帧的光流法初值为路标点对当前帧位姿（初值）的投影
if (kp->map_point_.lock())
{
auto mp = kp->map_point_.lock();//检查是否上锁
// 将世界坐标系中的坐标 投影 到像素坐标系上
auto px = camera_left_->world2pixel(mp->Pos(),current_frame_->Pose());
//关键帧的上一个容器中推入匹配到的关键点
kps_last.push_back(kp->position_.pt);
//当前帧中推入对应的像素坐标系上的坐标
kps_current.push_back(cv::Point2f(px[0],px[1]));
}
//没有上锁，直接推入对应的地图点
else{
kps_last.push_back(kp->position_.pt);
kps_current.push_back(kp->position_.pt);
}
}
// 光流追踪
std::vector status;// 设置一个状态的容器
Mat error; //设置一个error矩阵
cv::calcOpticalFlowPyrLK(last_frame_->left_img_,
last_frame_->left_img_, kps_last, kps_current,
status, error,
cv::Size(11, 11), 3,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01),
cv::OPTFLOW_USE_INITIAL_FLOW); // 最后一个参数flag，使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计
//统计匹配上的特征点个数，并存储
int num_good_pts = 0;
for (size_t i = 0; i < status.size(); ++i)
{
//如果匹配到对应的流则个数加一；并指针继续向后指
if (status[i])
{
cv::KeyPoint kp(kps_current[i],7); // 关键点代表的区域直径大小为7
Feature::Ptr feature(new Feature(current_frame_,kp)); // 有一个关键点，就要有一个特征类
feature->map_point_=last_frame_->features_left_[i]->map_point_;// 关键点对应的地图点就是上一帧的点对应的地图点
current_frame_->features_left_.push_back(feature);// 填充当前帧的左图特征点容器
num_good_pts++;// 匹配成功点计数
}
}
// 匹配成功的点数量记录到日志，并返回
LOG(INFO) << "Find " << num_good_pts << " in the last image.";
return num_good_pts;
} EstimateCurrentPose() 函数
估计当前帧位姿；与backend.cpp中的Optimize中的g2o优化相似，区别在于此处无需优化，只是估计出来，新建一些信息，这里很大一部分内容都是源自于《视觉SLAM14讲》第二版中的 P191g2o的实现过程,参考我之前写的手撕ch7(2)中的g2o的实现流程,
- 步骤一： 创建线性方程求解器，确定分解方法
- 步骤二： 构造线性方程的矩阵块，并用上面定义的线性求解器初始化
- 步骤三： 创建总求解器 solver，并从 GN, LM, DogLeg 中选一个，再用上述块求解器 BlockSolver 初始化
- 步骤四： 创建稀疏优化器（SparseOptimizer）
- 步骤五： 自定义图的顶点和边，并添加到 SparseOptimizer 优化器中
- 步骤六： 设置优化参数，开始执行优化
- 步骤七： 使用卡方检查来区分哪些特征点是内点，哪些特征点是外点，参考，小葡萄：[ORB-SLAM2]卡方分布(Chi-squared)外点(outlier)剔除 ，主要思路是首先定义误差（误差要归一化，也就是去量纲化），然后选择置信度，一般为95%，如果这个特征点的误差在区间内我们认为是内外，否则是外点。
- 步骤八：迭代优化完成之后，我们把优化估计的POSE给当前帧，把是外点的特征点对应的地点图删除，最后返回内点个点。
原文代码:
int Frontend::EstimateCurrentPose(){
// setup g2o（g2o过程） 图优化：单节点+多条一元边
// 设定g2o
typedef ::g2o::BlockSolver_6_3 BlockSolverType;//pose 是 6 ，mappoint是 3
typedef g2o::LinearSolverDense LinearSolverType; //线性求解器类型
// 块求解器BlockSolver
auto solver = new g2o::OptimizationAlgorithmLevenberg (
g2o::make_unique( g2o::make_unique())); // 选择梯度下降法
g2o::SparseOptimizer optimizer; //稀疏求解
optimizer.setAlgorithm(solver); //设置求解器
// vertex（优化量 顶点）
VertexPose *vertex_pose= new VertexPose();// camera vertex_pose
vertex_pose->setId(0);// 定义节点编号
vertex_pose->setEstimate(current_frame_->Pose());//设置初值
optimizer.addVertex(vertex_pose);//把节点添加到图中
// K
Mat33 K = camera_left_->K();
// edges（约束 边）（每对匹配点就是一个边）
int index =1;
std::vectoredges;
std::vectorfeatures;
//遍历每一对点
for (size_t i = 0; i < current_frame_->features_left_.size(); ++i)
{
auto mp = current_frame_->features_left_[i]->map_point_.lock();
if (mp)
{
features.push_back(current_frame_->features_left_[i]);
EdgeProjectionPoseOnly *edge = new EdgeProjectionPoseOnly(mp->pos_,K);
edge->setId(index);
edge->setVertex(0,vertex_pose);// 设置连接的顶点
edge->setMeasurement(toVec2(current_frame_->features_left_[i]->position_.pt));//传入观测值
edge->setInformation(Eigen::Matrix2d::Identity());// 信息矩阵
edge->setRobustKernel(new g2o::RobustKernelHuber); //鲁棒核函数
edges.push_back(edge);
optimizer.addEdge(edge);
index++;
}
}
// estimate the Pose the determine the outliers
// 要思路是首先定义误差（误差要归一化，也就是去量纲化），然后选择置信度，一般为95%，如果这个特征点的误差在区间内我们认为是内外，否则是外点
const double chi2_th = 5.991;
int cnt_outlier = 0;
for (int iteration = 0; iteration < 4; ++iteration) {
vertex_pose->setEstimate(current_frame_->Pose());
optimizer.initializeOptimization();// 设置优化初始值
optimizer.optimize(10);// 设置优化次数
cnt_outlier = 0;
// count the outliers
for (size_t i = 0; i < edges.size(); ++i) {
auto e = edges[i];
if (features[i]->is_outlier_) {
e->computeError();
}
if (e->chi2() > chi2_th) {
//设置等级 一般情况下g2o只处理level = 0的边，设置等级为1，下次循环g2o不再优化这个边
features[i]->is_outlier_ = true;
e->setLevel(1);
cnt_outlier++;
} else {
features[i]->is_outlier_ = false;
e->setLevel(0);
};
// 在第三次迭代之后，它会将所有边的鲁棒核函数设置为nullptr，以便在后续的迭代中不再考虑outlier
if (iteration == 2) {
e->setRobustKernel(nullptr);
}
}
}
LOG(INFO) << "Outlier/Inlier in pose estimating: " << cnt_outlier << "/"
<< features.size() - cnt_outlier;
// Set pose and outlier
current_frame_->SetPose(vertex_pose->estimate());
LOG(INFO) << "Current Pose = \n" << current_frame_->Pose().matrix();
for (auto &feat : features) {
if (feat->is_outlier_) {
feat->map_point_.reset();
feat->is_outlier_ = false; // maybe we can still use it in future
}
}
return features.size() - cnt_outlier;
} 其中我们关注其中:自定义的顶点和边
顶点（Vertex）
// 位姿顶点
class VertexPose : public g2o::BaseVertex<6,SE3>{//优化量的参数模板
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
// 重置，设定被优化变量的原始值
virtual void setToOriginImpl() override
{
_estimate =SE3();
}
// 更新
virtual void oplusImpl(const double * update) override
{
Vec6 update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate= SE3::exp(update_eigen) * _estimate; // 左乘更新 SE3 - 旋转矩阵R
}
// 存盘、读盘
virtual bool read(std::istream &in) override { return true; } //存盘
virtual bool write(std::ostream &out) const override { return true; } //读盘
};边（edge）
// 仅估计位姿的一元边
class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2,Vec2,VertexPose>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
// 构造函数
EdgeProjectionPoseOnly(const Vec3 &pos, const Mat33 &K ): _pos3d(pos), _K(K) {}
// 误差计算函数
virtual void computeError() override{
const VertexPose *v = static_cast(_vertices[0]);
SE3 T = v->estimate();
Vec3 pos_pixel= _K * (T*_pos3d); //将3D世界坐标转为相机像素坐标
pos_pixel /= pos_pixel[2];
_error=_measurement-pos_pixel.head<2>();
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast(_vertices[0]);
SE3 T = v->estimate();
Vec3 pos_cam = T * _pos3d; //相机坐标系下空间点的坐标=相机位姿 * 空间点的坐标
double fx = _K(0, 0);
double fy = _K(1, 1);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Zinv = 1.0 / (Z + 1e-18);
double Zinv2 = Zinv * Zinv;
//2*6的雅克比矩阵
_jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
-fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
-fy * X * Zinv;
}
// 读操作
virtual bool read(std::istream &in) override { return true; }
// 写操作
virtual bool write(std::ostream &out) const override { return true; }
private:
Vec3 _pos3d;
Mat33 _K;
}; 至此EstimateCurrentPose() 函数完成,返回的当前帧追踪到的特征点内点数量(tracking_inliers_)，然后判断当前的跟踪状态，为下次的添加帧做准备.如果内点数大于50 ，就是 tracking good ，内点数小于50大于20， 就是tracking bad，如果小于20，就 tracking lost .
更新完跟踪状态后，进入 InsertKeyframe() 函数判断要不要插入关键帧
InsertKeyframe() 函数:
- 首先,如果目前追踪到的内点数大于80个，说明两帧差异不大,则不需要插入关键帧,return false，如果小于80个就把当前帧设置为关键帧，对应 Frame::SetKeyFrame()函数
- 之后,把当前帧插入到局部地图中去，对应Map::InsertKeyFrame() 函数。其中如果当前激活的关键帧大于 num_active_keyframes_ （默认是7），就删除掉老的或太近的关键帧，对应 Map::RemoveOldKeyframe() 函数。
- 然后对当前帧提取新的GFTT特征点，对应DetectFeatures()函数。
- 接着匹配右目特征点，对应FindFeaturesInRight()函数。
- 然后三角化新特征点并加入到地图中去，对应TriangulateNewPoints() 函数。
- 因为添加了新的关键帧，所以在后端里面 运行 Backend::UpdateMap() 更新一下局部地图，启动一次局部地图的BA优化。
- 最后把显示线程中的地图点也更新一下，对应Viewer::UpdateMap()函数。
bool Frontend::InsertKeyframe(){
// 如果追踪到的点很多（说明两帧差异不大），则不需要插入关键帧
if (tracking_inliers_>num_features_needed_for_keyframe_)
{
return false;
}
// 如果追踪到的点很少（说明两帧差异较大），判定该帧为关键帧
// 当前帧为关键帧并分配关键帧id
current_frame_->SetKeyFrame();
// 地图中插入当前帧
map_->InsertKeyFrame(current_frame_);
LOG(INFO) << "Set frame " << current_frame_->id_ << " as keyframe "
<< current_frame_->keyframe_id_;
// 将关键帧中特征点对应的地图点加入到观测容器中
SetObservationsForKeyFrame();
// 关键帧重新提取特征点
DetectFeatures();
// track in right image 在关键帧的右目图像中找与左目对应的特征点
FindFeaturesInRight();
// triangulate map points 三角化新的地图点
TriangulateNewPoints();
// update backend because we have a new keyframe更新地图,触发后端优化
backend_->UpdateMap();
// 可视化模块中更新视图
if (viewer_)
{
viewer_->UpdateMap();
}
}其中三角化TriangulateNewPoints()函数 流程和 上一篇(5) 双目初始化 StereoInit()中的triangulation() 三角化函数基本一样:
// triangulate map points 三角化新的地图点
int Frontend::TriangulateNewPoints(){
// 1.左右目位姿（pose）
std::vectorposes{camera_left_->pose(),camera_right_->pose()};
SE3 current_pose_Twc=current_frame_->Pose().inverse();
int cnt_triangulated_pts = 0;
for (size_t i = 0; i < current_frame_->features_left_.size(); ++i)
{
//expired()函数用于检查智能指针指向的对象是否为空 ,expired()==true 存储空指针
if (current_frame_->features_left_[i]->map_point_.expired() && current_frame_->features_right_[i] !=nullptr)
{
// 左图的特征点未关联地图点且存在右图匹配点
// 2.左右目匹配点（point）
std::vectorpoints{
camera_left_->pixel2camera(
Vec2(current_frame_->features_left_[i]->position_.pt.x, current_frame_->features_left_[i]->position_.pt.y)),
camera_right_->pixel2camera(
Vec2(current_frame_->features_right_[i]->position_.pt.x,current_frame_->features_right_[i]->position_.pt.y))
};
// 3 .pworld
Vec3 pworld = Vec3::Zero();
// 三角化成功并且深度为正
if (triangulation(poses,points,pworld) && pworld[2]>0)
{
//创建一个新地图，用于信息更新
auto new_map_point = MapPoint::CreateNewMappoint();
//三角化得到的坐标是左目相机系坐标,这里和初始化中的triangulation区分：
// 这里计算出的pworld是相机系坐标，左乘Twc才是世界系坐标, 而初始化是第一帧，一般以第一帧为世界系，因此得到的pworld直接当世界系坐标使用
pworld = current_pose_Twc * pworld;
new_map_point->SetPos(pworld);
//将观测到的特征点加入新地图
new_map_point->AddObservation(current_frame_->features_left_[i]);
new_map_point->AddObservation(current_frame_->features_right_[i]);
//为特征类Feature对象填写地图点成员
current_frame_->features_left_[i]->map_point_=new_map_point;
current_frame_->features_right_[i]->map_point_=new_map_point;
//对Map类对象来说，地图里面应当多了一个地图点，所以要将这个地图点加到地图中去
map_->InsertMapPoint(new_map_point);
cnt_triangulated_pts++;
}
}
}
LOG(INFO) << "new landmarks: " << cnt_triangulated_pts;
return cnt_triangulated_pts;
} 在InsertKeyframe()函数走完后，最后我们会把当前帧位姿和上一帧的位姿的差给到relative_motion_变量(相对位姿变换 = 当前帧的位姿 * 上一帧位姿的逆)，最后在显示线程中添加当前帧，对应Viewer::AddCurrentFrame( )函数