手撕 视觉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<cv::Point2f>  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<uchar> 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<BlockSolverType::PoseMatrixType>  LinearSolverType;    //线性求解器类型

    // 块求解器BlockSolver
    auto solver = new g2o::OptimizationAlgorithmLevenberg (
        g2o::make_unique<BlockSolverType>( g2o::make_unique<LinearSolverType>()));      // 选择梯度下降法
    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::vector<EdgeProjectionPoseOnly*>edges;
    std::vector<Feature::Ptr>features;

    //遍历每一对点
    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<VertexPose*>(_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<VertexPose *>(_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::vector<SE3>poses{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::vector<Vec3>points{
                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( )函数

至此,前端的完整流程就全部写完(除了重定位没写)