Hand torn visual slam14 talks about ch13 code (6) normal tracking mode Track()

Previous article During the completion of binocular initialization, the tracking status FrontendStatus changes to TRACKING_GOOD, so after that, the normal tracking Track() function will be entered:

The process of the Track function is also very clear:

First, use the uniform velocity model to set an initial value for the pose of the current frame. Here, use the pose of the previous frame to obtain the initial value of the current pose estimate.
Then, use the optical flow method to match the feature points of the two frames before and after (only the left eye image is used for the two frames before and after), and return the number of points matched by the optical flow method, corresponding to the TrackLastFrame() function
Then, the position of the current frame is optimized based on the matching feature points, corresponding to the EstimateCurrentPose() function.
After that, the current tracking status is judged based on the tracked inliers. If the number of inliers is greater than 50, it is TRACKING_GOOD. If the number of inliers is less than 50 and greater than 20, it is TRACKING_BAD. If it is less than or equal to 20, it is lost.
Then, determine whether to insert a keyframe currently, corresponding to the InsertKeyframe() function
After that, update the position difference between the current frame and the previous frame
Finally, send the current frame to the viewer_thread.

//正常状态下的跟踪
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;
    }
}

Next, let’s take a closer look at the function implementation:

TrackLastFrame() function:

Use the optical flow method to match the previous frame and the current frame, and return the number of points matched by the optical flow method. This is basically the same as the previous left and right eye flow tracking FindFeaturesInRight() function. The difference is that here is the previous frame and the current frame. Use the left eye image to perform optical flow tracking. The process is as follows:

The first step: assign an initial value. If the current feature point has a corresponding point on the map, then the pixel position is inferred based on the 3D position of the feature point and an initial value POSE calculated by the uniform velocity model. If there is no corresponding feature point, the initial value of the feature point of the current frame is the same as the previous frame.
Step 2: Call the LK optical flow method in OpenCV to track the position of the right eye feature points
Step 3: Push_back the matched feature points to the left feature point of the current frame. If there is no match, put a null pointer.
Finally, count the number of matching feature points.

//用光流法匹配上一帧与当前帧，并返回光流法匹配到的点的数量
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() function

Estimate the current frame pose; similar to the g2o optimization in Optimize in backend.cpp, the difference is that there is no need to optimize here, just estimate it and create some new information. A large part of the content here is derived from " For the implementation process of P191g2o in the second edition of "Visual SLAM Lecture 14", refer to the implementation process of g2o in Hand-shred ch7(2) that I wrote before.

Step 1: Create a linear equation solver and determine the decomposition method
Step 2: Construct the matrix block of the linear equation and initialize it with the linear solver defined above
Step 3: Create a total solver, select one from GN, LM, DogLeg, and then initialize it with the above block solver BlockSolver
Step 4: Create a sparse optimizer (SparseOptimizer)
Step 5: Customize the vertices and edges of the graph and add them to the SparseOptimizer optimizer
Step 6: Set optimization parameters and start optimization
Step 7: Use chi-square check to distinguish which feature points are interior points and which feature points are exterior points, for reference,Little Grape: [ORB-SLAM2] Chi-square distribution (Chi-squared) outlier elimination, the main idea is to first define the error (the error must be normalized, that is, dedimensionalized), and then select the confidence level, usually 95 %, if the error of this feature point is within the interval, we consider it as an inside or outside point, otherwise it is an outside point.
Step 8: After the iterative optimization is completed, we give the optimized POSE to the current frame, delete the location map corresponding to the feature point that is the outer point, and finally return the number of inner points.

Original code:

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;
}

Among them we focus on: custom vertices and edges

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;
};

At this point, the EstimateCurrentPose() function is completed. It returns the number of feature point inliers tracked by the current frame (tracking_inliers_), and then determines the current tracking status to prepare for the next added frame. If the number of inliers is greater than 50, it means tracking good. If the number of points is less than 50 and greater than 20, it means tracking bad. If it is less than 20, it means tracking lost.

After updating the tracking status, enter the InsertKeyframe() function to determine whether to insert a keyframe.

InsertKeyframe() function:

First, if the number of internal points currently tracked is greater than 80, it means that the difference between the two frames is not big, and there is no need to insert a key frame. Return false. If it is less than 80, set the current frame as a key frame, corresponding to Frame::SetKeyFrame() function
After that, insert the current frame into the local map, corresponding to the Map::InsertKeyFrame() function. If the currently active keyframes are greater than num_active_keyframes_ (the default is 7), the old or too recent keyframes will be deleted, corresponding to the Map::RemoveOldKeyframe() function.
Then extract new GFTT feature points from the current frame, corresponding to the DetectFeatures() function.
Then match the feature points of the right eye, corresponding to the FindFeaturesInRight() function.
Then triangulate the new feature points and add them to the map, corresponding to the TriangulateNewPoints() function.
Because new keyframes have been added, run Backend::UpdateMap() in the backend to update the local map and start BA optimization of the local map.
Finally, update the map points in the display thread, corresponding to the Viewer::UpdateMap() function.

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();
    }
}

The triangulation TriangulateNewPoints() function process andPrevious article (5) Triangulation() triangulation in StereoInit() The function is basically the same:

// 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;
}

After the InsertKeyframe() function is completed, we will finally give the difference between the current frame pose and the previous frame pose to the relative_motion_ variable (relative pose transformation = current frame pose * inverse of the previous frame pose ), and finally add the current frame in the display thread, corresponding to the Viewer::AddCurrentFrame() function