slam14讲之设计slam系统（第二版ch13）代码注释一（前端）

阅读高博slam14讲工程项目那章对前端代码的一些注释，主要是frontend.cpp，其实typedef g2o::BlockSolver_6_3 BlockSolverType中这个<p,l>(本代码中是6,3)一直不是很理解，感觉怎么解释都解释不通，有没有好心人看到能解释一下～～～（感激）

//
// Created by gaoxiang on 19-5-2.
//

#include <opencv2/opencv.hpp>

#include "myslam/algorithm.h"
#include "myslam/backend.h"
#include "myslam/config.h"
#include "myslam/feature.h"
#include "myslam/frontend.h"
#include "myslam/g2o_types.h"
#include "myslam/map.h"
#include "myslam/viewer.h"

namespace myslam {

Frontend::Frontend() {
    /*
    最大特征点数量 num_features 
    角点可以接受的最小特征值 检测到的角点的质量等级，角点特征值小于qualityLevel*最大特征值的点将被舍弃 0.01 
    角点最小距离 20  
    */
    gftt_ =
        cv::GFTTDetector::create(Config::Get<int>("num_features"), 0.01, 20);
    num_features_init_ = Config::Get<int>("num_features_init");
    num_features_ = Config::Get<int>("num_features");
}

//在增加某一帧时，根据目前的状况选择不同的处理函数
bool Frontend::AddFrame(myslam::Frame::Ptr frame) {
    current_frame_ = frame;

    switch (status_) {
        case FrontendStatus::INITING:
            StereoInit();
            break;
        case FrontendStatus::TRACKING_GOOD:
        case FrontendStatus::TRACKING_BAD:
            Track();
            break;
        case FrontendStatus::LOST:
            Reset();
            break;
    }

    last_frame_ = current_frame_;
    return true;
}

//在执行Track之前，需要明白，Track究竟在做一件什么事情
//Track是当前帧和上一帧之间进行的匹配
//而初始化是某一帧左右目（双目）之间进行的匹配
bool Frontend::Track() {
    //先看last_frame_是不是正常存在的
    if (last_frame_) {
        //给current_frame_当前帧的位姿设置一个初值
        current_frame_->SetPose(relative_motion_ * last_frame_->Pose());
    }

    //使用光流法得到前后两帧之间匹配特征点并返回匹配数
    int num_track_last = TrackLastFrame();

    //接下来根据跟踪到的内点的匹配数目，可以分类进行后续操作，估计当前帧的位姿
    tracking_inliers_ = EstimateCurrentPose();

    if (tracking_inliers_ > num_features_tracking_) {
        // tracking good
        status_ = FrontendStatus::TRACKING_GOOD;
    } else if (tracking_inliers_ > num_features_tracking_bad_) {
        // 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;
}

bool Frontend::InsertKeyframe() {
    //需要前后帧追踪的匹配点大于一定数量才可以成为匹配点
    //这样设置是否合理？？ 若是短时间内的前后帧 一定可以满足啊 ？ 
    //这样的关键帧的意义在哪里呢？  搞不懂0.0
    if (tracking_inliers_ >= num_features_needed_for_keyframe_) {
        // still have enough features, don't insert keyframe
        return false;
    }
    // current frame is a new keyframe
    current_frame_->SetKeyFrame();
    map_->InsertKeyFrame(current_frame_);

    LOG(INFO) << "Set frame " << current_frame_->id_ << " as keyframe "
              << current_frame_->keyframe_id_;
    //添加关键帧的路标点
    SetObservationsForKeyFrame();
    //检测当前关键帧的左目特征点
    DetectFeatures();  // detect new features

    // track in right image
    //寻找左右目匹配点
    FindFeaturesInRight();
    // triangulate map points
    TriangulateNewPoints();
    // update backend because we have a new keyframe
    backend_->UpdateMap();

    if (viewer_) viewer_->UpdateMap();

    return true;
}

void Frontend::SetObservationsForKeyFrame() {
    for (auto &feat : current_frame_->features_left_) {
        auto mp = feat->map_point_.lock();
        if (mp) mp->AddObservation(feat);
    }
}

//在InsertKeyFrame函数中出现了一个三角化步骤，
//这是因为当一个新的关键帧到来后，我们势必需要补充一系列新的特征点，
// 此时则需要像建立初始地图一样，对这些新加入的特征点进行三角化，求其3D位置
int Frontend::TriangulateNewPoints() {
    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()表示指针存在
        if (current_frame_->features_left_[i]->map_point_.expired() &&
            current_frame_->features_right_[i] != nullptr) {
            // 左图的特征点未关联地图点且存在右图匹配点，尝试三角化
            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))};
            Vec3 pworld = Vec3::Zero();

            if (triangulation(poses, points, pworld) && pworld[2] > 0) {
                auto new_map_point = MapPoint::CreateNewMappoint();
                //注意这里与初始化地图不同 triangulation计算出来的点pworld，
                //实际上是相机坐标系下的点，所以需要乘以一个TWC
                //但是初始化地图时，是第一帧，一般以第一帧为世界坐标系
                pworld = current_pose_Twc * pworld;
                //设置mapoint类中的坐标
                new_map_point->SetPos(pworld);
                //增加mappoint类中的对应的那个feature（左右目）
                new_map_point->AddObservation(
                    current_frame_->features_left_[i]);
                new_map_point->AddObservation(
                    current_frame_->features_right_[i]);
                //为特征类添加地图点成员变量
                current_frame_->features_left_[i]->map_point_ = new_map_point;
                current_frame_->features_right_[i]->map_point_ = new_map_point;
                //既然有了一个新的地图点mappoint，那就应当更新一下地图（类），向地图类的对象中添加地图点。
                map_->InsertMapPoint(new_map_point);
                cnt_triangulated_pts++;
            }
        }
    }
    LOG(INFO) << "new landmarks: " << cnt_triangulated_pts;
    return cnt_triangulated_pts;
}

//利用g2o来进行优化求解，先进行优化求解器配置
//此处代码可参考视觉slam14讲 重投影误差 p197
int Frontend::EstimateCurrentPose() {   
    // setup g2o
    //双边一个顶点为6维T，一个为3维point
    typedef g2o::BlockSolver_6_3 BlockSolverType; 
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>
        LinearSolverType;
    //定义总solver求解器，选择优化方法
    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) {
        //这里就涉及到前面在TrackLastFrame()函数里面提到的，
        //有些特征虽然被跟踪到了，
        //但是并没有受到三角化，即没有map_point_
        //这里便对feature有没有map_point_进行判断，有则可以往下进行重投影，
        //没有则不行，因为重投影需要点的3D位置
        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
    const double chi2_th = 5.991;
    int cnt_outlier = 0;
    for (int iteration = 0; iteration < 4; ++iteration) {
        //总共优化了40遍，以10遍为一个优化周期，对outlier进行一次判断
        //舍弃掉outlier的边，随后再进行下一个10步优化
        vertex_pose->setEstimate(current_frame_->Pose());
        // 但每次涉及的特征都不一样，所以每次的重投影误差都不一样，
        // 就有可能发现新的outlier，这是一个不断筛查,删除,精化的过程
        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) {
                //这里每个边都有一个level的概念，
                //默认情况下，g2o只处理level=0的边，在orbslam中，
                // 如果确定某个边的重投影误差过大，则把level设置为1，
                //也就是舍弃这个边对于整个优化的影响
                features[i]->is_outlier_ = true;
                e->setLevel(1);
                cnt_outlier++;
            } else {
                features[i]->is_outlier_ = false;
                e->setLevel(0);
            };

            //后20次不设置鲁棒核函数了，意味着此时不太可能出现大的异常点
            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();

    //清除异常点 但是只在feature中清除了
    //mappoint中仍然存在，仍然有使用的可能
    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;
}

//该函数的实现其实非常像FindFeaturesInRight(),
//不同的是一个在左右目之间找，另一个在前后帧之间找
int Frontend::TrackLastFrame() {
    // use LK flow to estimate points in the right image
    std::vector<cv::Point2f> kps_last, kps_current;
    //遍历上一帧中的所有左目特征
    for (auto &kp : last_frame_->features_left_) {
        //判断该特征有没有构建出相应的地图点
        //对于左目图像来说，我们可以将其用于估计相机pose，但是不一定左目图像中的每一个点都有mappoint
        //MapPoint的形成是需要左目和同一帧的右目中构成对应关系才可以，有些左目中的feature在右目中没有配对，就没有Mappoint
        //但是没有Mappoint却不代表这个点是一个outlier
        if (kp->map_point_.lock()) {
            // use project point
            //对于建立了Mappoint的特征点而言 
            //kps_current的初值是通过world2pixel转换得到的
            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 {
            //对于建立了Mappoint的特征点而言 
            //kps_current的初值是kps_last
            kps_last.push_back(kp->position_.pt);
            kps_current.push_back(kp->position_.pt);
        }
    }

    std::vector<uchar> status;
    Mat error;
    cv::calcOpticalFlowPyrLK(
        last_frame_->left_img_, current_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);

    int num_good_pts = 0;

    for (size_t i = 0; i < status.size(); ++i) {
        if (status[i]) {
            //status[i]=true则说明跟踪成功有对应点，false则跟踪失败没找到对应点
            cv::KeyPoint kp(kps_current[i], 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;
}

bool Frontend::StereoInit() {
    //一个frame其实就是一个时间点，
    //里面同时含有左，右目的图像，以及对应的feature的vector
    //这一步在提取左目特征，通常在左目当中提取特征时特征点数量是一定能保证的。
    int num_features_left = DetectFeatures();
    //根据左目特征在右目中找对应，
    //虽然左目提取时特征点数量能够保证，但匹配过程则无法确保
    //能够在右目图像中为所有的左目特征都找到对应，
    //所以这一步最后找到的对应特征数目不一定满足初始化条件
    int num_coor_features = FindFeaturesInRight();
    if (num_coor_features < num_features_init_) {
        return false;
    }

    bool build_map_success = BuildInitMap();
    if (build_map_success) {
        status_ = FrontendStatus::TRACKING_GOOD;
        if (viewer_) {
            viewer_->AddCurrentFrame(current_frame_);
            viewer_->UpdateMap();
        }
        return true;
    }
    return false;
}

//检测当前帧的做图的特征点，并放入feature的vector容器中
int Frontend::DetectFeatures() {
    //掩膜，灰度图，同时可以看出，DetectFeatures是对左目图像的操作
    cv::Mat mask(current_frame_->left_img_.size(), CV_8UC1, 255);
    for (auto &feat : current_frame_->features_left_) {
        //在已有的特征附近一个矩形区域内将掩膜值设为0
        //即在这个矩形区域中不提取特征了，保持均匀性，并避免重复
        cv::rectangle(mask, feat->position_.pt - cv::Point2f(10, 10),
                      feat->position_.pt + cv::Point2f(10, 10), 0, CV_FILLED);
    }

    std::vector<cv::KeyPoint> keypoints;
    //detect函数，第三个参数是用来指定特征点选取区域的，一个和原图像同尺寸的掩膜，其中非0区域代表detect函数感兴趣的提取区域，相当于为
    //detect函数明确了提取的大致位置
    gftt_->detect(current_frame_->left_img_, keypoints, mask);
    int cnt_detected = 0;
    for (auto &kp : keypoints) {
        current_frame_->features_left_.push_back(
            Feature::Ptr(new Feature(current_frame_, kp)));
        cnt_detected++;
    }

    LOG(INFO) << "Detect " << cnt_detected << " new features";
    return cnt_detected;
}

//找到左目图像的feature之后，就在右目里面找特征点
int Frontend::FindFeaturesInRight() {
    // use LK flow to estimate points in the right image
    std::vector<cv::Point2f> kps_left, kps_right;
    //遍历左目特征的特征点（feature）
    for (auto &kp : current_frame_->features_left_) {
        //feature类中的keypoint对应的point2f
        kps_left.push_back(kp->position_.pt);
        //feature类中的mappoint
        auto mp = kp->map_point_.lock();
        //mappoint若非空 ，右侧相机匹配点坐标的初始值为mappoint转换
        //否则初始值直接为左侧相机特征点坐标
        if (mp) {
            // use projected points as initial guess
            auto px =
                camera_right_->world2pixel(mp->pos_, current_frame_->Pose());
            kps_right.push_back(cv::Point2f(px[0], px[1]));
        } else {
            // use same pixel in left iamge
            kps_right.push_back(kp->position_.pt);
        }
    }

    std::vector<uchar> status;//光流跟踪成功与否的状态向量（无符号字符），成功则为1,否则为0
    Mat error;
     //进行光流跟踪，从这条opencv光流跟踪语句我们就可以知道，
     //前面遍历左目特征关键点是为了给光流跟踪提供一个右目初始值
     //OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;
     //如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。
    cv::calcOpticalFlowPyrLK(
        current_frame_->left_img_, current_frame_->right_img_, kps_left,
        kps_right, status, error, cv::Size(11, 11), 3,
        cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30,
                         0.01),
        cv::OPTFLOW_USE_INITIAL_FLOW);

    //右目中光流跟踪成功的点
    int num_good_pts = 0;
    for (size_t i = 0; i < status.size(); ++i) {
        if (status[i]) {
            //KeyPoint构造函数中7代表着关键点直径
            cv::KeyPoint kp(kps_right[i], 7);
            Feature::Ptr feat(new Feature(current_frame_, kp));
            //指明是右侧相机feature
            feat->is_on_left_image_ = false;
            current_frame_->features_right_.push_back(feat);
            num_good_pts++;
        } else {
            //左右目匹配失败
            current_frame_->features_right_.push_back(nullptr);
        }
    }
    LOG(INFO) << "Find " << num_good_pts << " in the right image.";
    return num_good_pts;
}

//现在左目图像的特征提取出来了，并根据左目图像的特征对右目图像做了特征的光流跟踪，
//找到了对应值，当对应数目满足阈值条件时，我们可以开始建立
//初始地图
bool Frontend::BuildInitMap() {
    //构造一个存储SE3的vector，里面初始化就放两个pose，一个左目pose，一个右目pose，
    //看到这里应该记得，对Frame也有一个pose，Frame里面的
    //pose描述了固定坐标系（世界坐标系）和某一帧间的位姿变化，
    std::vector<SE3> poses{camera_left_->pose(), camera_right_->pose()};
    //初始化的路标数目
    size_t cnt_init_landmarks = 0;
    
    //便利左目的feature
    for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) {
        //右目没有对应点
        if (current_frame_->features_right_[i] == nullptr) continue;
        // create map point from triangulation
        //对于左右目配对成功的点，三角化它
        //points中保存了双目像素坐标转换到相机（归一化）坐标
        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))};
        Vec3 pworld = Vec3::Zero();

        //triangulation（）函数 相机位姿 某个feature左右目的坐标 三角化后的坐标保存
        if (triangulation(poses, points, pworld) && pworld[2] > 0) {
            //根据前面存放的左右目相机pose和对应点相机坐标points进行三角化，得到对应地图点的深度，构造出地图点pworld
            //需要对pworld进行判断，看其深度是否大于0, pworld[2]即是其深度。
            auto new_map_point = MapPoint::CreateNewMappoint();//工厂模式创建一个新的地图点
            //mappoint类主要的数据成员  pos 以及 观测到的feature的vector
            new_map_point->SetPos(pworld);
            //为这个地图点添加观测量，这个地图点对应到了当前帧（应有帧ID）
            //左目图像特征中的第i个以及右目图像特征中的第i个
            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;
            cnt_init_landmarks++;
            //对Map类对象来说，地图里面应当多了一个地图点，所以要将这个地图点加到地图中去
            map_->InsertMapPoint(new_map_point);
        }
    }
    //当前帧能够进入初始化说明已经满足了初始化所需的帧特征数量，
    //作为初始化帧，可看做开始的第一帧，所以应当是一个关键帧
    current_frame_->SetKeyFrame();
     //对Map类对象来说，地图里面应当多了一个关键帧，所以要将这个关键帧加到地图中去
    map_->InsertKeyFrame(current_frame_);
    //关键帧插入，后端需要对新纳入的关键帧进行优化处理
    backend_->UpdateMap();

    LOG(INFO) << "Initial map created with " << cnt_init_landmarks
              << " map points";

    return true;
}

bool Frontend::Reset() {
    LOG(INFO) << "Reset is not implemented. ";
    return true;
}

}  // namespace myslam