手撕 视觉slam14讲 ch13 代码（5）双目初始化 StereoInit()

上一篇，我们分析了Frontend::AddFrame()函数，将会根据前端状态变量FrontendStatus，运行不同的三个函数，StereoInit()，Track()和Reset()，首先肯定是双目初始化StereoInit()函数：

双目初始化的步骤也很简单：就是根据左右目之间的光流匹配，寻找可以三角化的地图点，成功时建立初始地图：

• 首先用 DetectFeatures() 函数对左目提GFTT特征点
• 然后用FindFeaturesInRight() 函数进行左右目光流来匹配左右目的特征点。根据左目特征点的位置来找右边对应特征点的位置。
• 如果匹配到的特征点数量大于num_features_init_（默认100个），就进行 BuildInitMap() 函数来进行地图初始化。根据双目的相对位置和匹配的特征点把特征点进行三角化，计算出特征点的 3D位置 。
• 地图初始化成功后就改前端状态为TRACKING_GOOD，并把当前帧图像和地图点送到 viewer_ 线程里用做显示。

//双目初始化
 //根据左右目之间的光流匹配，寻找可以三角化的地图点，成功时建立初始地图
bool Frontend::StereoInit(){
    //提取左目的GFTT特征点（数量）
    int num_features_left = DetectFeatures();
    // 右目光流追踪
    int num_coor_features=FindFeaturesInRight();

    //如果匹配到的特征点数量小于num_features_init_，默认100个，就返回false
    if (num_coor_features < num_features_init_)
    {
        return false;
    }

    //初始化地图
    bool build_map_success= BuildInitMap();

    //如果地图初始化成功就改前端状态为TRACKING_GOOD，并把当前帧和地图点传到viewer_线程里
    if (build_map_success)
    {
        status_=FrontendStatus::TRACKING_GOOD;
        //地图在可视化端的操作，添加当前帧并更新整个地图
        if (viewer_)
        {
            viewer_->AddCurrentFrame(current_frame_);
            viewer_->UpdateMap();
        }
        return true; //地图初始化成功，返回ture
    }
    return false; //地图初始化失败，返回false
}

接下来我们具体看一下函数实现：

DetectFeatures()函数：

• 第一步，通过mask的方式，来使得特征提取均匀化，就是在已经存在特征点的地方，画一个20x20的矩形框，在这个范围内不再提取新的特征点；
• 第二步，在第一步的mask的基础上，提取新的特征点；
• 最后，把这些特征点push_back到当前帧的特征点里面去。关联当前帧current_frame_和检测到的新特征点的像素位置kp，同时统计这两次检测到的特征点数量

/*提取左目的GFTT特征点（数量）
    //opencv中mask的作用就是创建感兴趣区域，即待处理的区域。
         通常，mask大小创建分为两步，先创建与原图一致，类型为CV_8UC1或者CV_8UC3的全零图（即黑色图）。如mask = Mat::zeros(image.size(),CV_8UC1); 
        然后用rect类或者fillPoly()函数将原图中待处理的区域（感兴趣区域）置为1。
*/
int Frontend::DetectFeatures(){
    cv::Mat mask(current_frame_->left_img_.size(),CV_8UC1, 255);
    //循环遍历当前帧上的左侧图像特征,并绘制边框
    for(auto feat : current_frame_->features_left_){
        /*
            void cv::rectangle(cv::InputOutputArray img, cv::Point pt1, cv::Point pt2, 
                                const cv::Scalar &color, int thickness = 1, int lineType = 8, int shift = 0)
            绘制一个简单的、厚的或向上填充的矩形。函数cv::rectangle绘制一个矩形轮廓或填充矩形，其两个相对的角分别为pt1和pt2。
            position_.pt————————>关键点坐标
            inline cv::Point2f::Point_(float _x, float _y)------>关键点上下左右10距离的矩形，color为0，就是黑色填充满
        */
       cv::rectangle(mask,feat->position_.pt-cv::Point2f(10,10),feat->position_.pt+cv::Point2f(10,10),0,CV_FILLED);
    }
    //建立一个关键点的vector
    std::vector<cv::KeyPoint>keypoints;

    //提取左图特征点 
    /*
        virtual void cv::Feature2D::detect(cv::InputArray image, std::vector<cv::KeyPoint> &keypoints, cv::InputArray mask = noArray())
        重载：
            检测图像(第一种变体)或图像集(第二种变体)中的关键点。
        参数：
            image – 图像.
            keypoints – The detected keypoints. In the second variant of the method keypoints[i] is a set of keypoints detected in images[i] .
                        检测到的关键点。在该方法的第二种变体中，keypoints[i]是在图像[i]中检测到的一组关键点。
            mask – Mask specifying where to look for keypoints (optional). It must be a 8-bit integer matrix with non-zero values in the region of interest.
                        指定在何处查找关键点的掩码(可选)。它必须是一个8位整数矩阵，在感兴趣的区域中具有非零值。
        GFTT角点
    */        
    gftt_->detect(current_frame_->left_img_,keypoints,mask);

    //关联current_frame_和kp，同时统计这两次检测到的特征点数量
    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;
}

FindFeaturesInRight() 函数：

• 第一步：进行赋初值。如果当前特征点有在地图上有对应的点，那么将根据特征点的3D POSE和当前帧的位姿反求出特征点在当前帧的像素坐标。如果没有对应特征点，右目的特征点初值就是和左目一样。这样可以加快光流的计算速度同时可以一点程度上避免光流找到错误的点
• 第二步：调用OpenCV中的LK光流法来追踪右目特征点的位置
• 第三步：把匹配上的特征点 push_back 到当前帧的右目特征点里面去，没匹配上就放个空指针。
• 最后统计一下匹配上的特征点数目。

// 右目光流追踪
int Frontend::FindFeaturesInRight(){
    //赋初值
    std::vector<cv::Point2f>kps_left,kps_right;
    //  这里把当前帧左目的特征点位置放到 kps_left 这个临时变量里面。
    //  如果当前特征点有在地图上有对应的点，那么将根据特征点的3D POSE和当前帧的位姿反求出特征点在当前帧的像素坐标。如果没有对应特征点，右目的特征点初值就是和左目一样。
    for(auto &kp : current_frame_->features_left_){
        kps_left.push_back(kp->position_.pt);
        auto mp =kp->map_point_.lock();//检查是否上锁
        // 如果当前特征点有在地图上有对应的点
        if (mp)
        {
             // use projected points as initial guess（使用投影点作为初始估计值）//tzy
            auto px=camera_right_->world2pixel(mp->Pos(),current_frame_->Pose());
            //存入右侧图像的关键点
            kps_right.push_back(cv::Point2f(px[0],px[1]));
        }
        //否则，使用左侧相机特征点坐标为初始值
        else{
            kps_right.push_back(kp->position_.pt);
        }
    }
    
    // 开始使用LK流来估计右侧图像中的点
    std::vector<uchar> status;
    Mat error;
        //opencv自带的光流跟踪函数
         /*
        CV_EXPORTS_W void calcOpticalFlowPyrLK( InputArray prevImg, InputArray nextImg,
                                        InputArray prevPts, InputOutputArray nextPts,
                                        OutputArray status, OutputArray err,
                                        Size winSize = Size(21,21), int maxLevel = 3,
                                        TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
                                        int flags = 0, double minEigThreshold = 1e-4 );
            Calculates an optical flow for a sparse feature set using the iterative Lucas-Kanade method with pyramids.
        */
    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); // 最后一个参数flag，使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计

    // 统计匹配上的特征点个数，并存储
    int num_good_pts;
    for (size_t i = 0; i < status.size(); ++i)
    {
        // 为真表示光流追踪成功
        if (status[i])
        {
            cv::KeyPoint kp(kps_right[i], 7); // 右目关键点的直径为7
            Feature::Ptr feat(new Feature(current_frame_, kp));
            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; // 返回成功匹配的点对数量
}

BuildInitMap() 函数：

首先，如果上一步光流追踪中匹配到的特征点数量大于num_features_init_（默认100个），才进行地图初始化：

• 第一步：设置两个相机的位置，通过 VisualOdometry::Init() 函数，系统初始化的时候调用了 Frontend::SetCameras() 函数，然后在 Dataset::Init() 函数里面有对每个相机的pose进行实际的赋值。
• 第二步：计算每个特征点在两个相机的平面的归一化坐标。
• 第三步：使用 algorithm.h 文件中的 triangulation() 函数，来算这个特征点在世界坐标系下的3D 位置，（通过构建线性方程，然后SVD分解来计算特征点的位置）

bool Frontend::BuildInitMap(){
    //设置左右两个相机的位置（获取pose）
    std::vector<SE3>poses{camera_left_->pose(),camera_right_->pose()};
    size_t cnt_init_landmarks= 0;    //设置地图标志初始值
    //循环左侧图像特征点 (获取 特征点归一化坐标)
    for (size_t i = 0; i < current_frame_->features_left_.size(); ++i)
    {
        //判断右侧图像是否有对应特征点，有则继续向下执行，没有就continue
        if (current_frame_->features_right_[i]== nullptr)    continue;
        // create map point from triangulation  (开始获取三角化所需的 特征点归一化坐标)
        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))};
        //新建一个三维0矩阵
        Vec3 pworld =Vec3::Zero();

        // 尝试对每一对匹配点进行三角化
        /*
            inline bool myslam::triangulation(const std::vector<SE3> &poses, std::vector<Vec3> points, Vec3 &pt_world)
            linear triangulation with SVD 线性三角测量
            参数:
                poses – poses,
                points – points in normalized plane
                pt_world – triangulated point in the world
            返回:
                true if success
        */
        if (triangulation(poses,points,pworld) &&pworld[2]>0)
        {
            //创建地图存储数据
            //创建一个新地图，用于信息更新
            auto new_map_point = MapPoint::CreateNewMappoint();
            new_map_point->SetPos(pworld);
        
             //将观测到的特征点加入新地图
             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;

             //初始化地图点makr+1
             cnt_init_landmarks++;
             //在地图中插入当前新的更新点
             map_->InsertMapPoint(new_map_point);
        }
    }
    //把初始化的这一帧设置为关键帧
    current_frame_->SetKeyFrame();
    //在地图中插入关键帧
    map_->InsertKeyFrame(current_frame_);

    //后端更新地图（在后端更新）
    backend_->UpdateMap();

    //输出操作日志
    LOG(INFO) << "Initial map created with " << cnt_init_landmarks
              << " map points";
    //返回：单个图像构建初始地图成功
    return true;
}

其中triangulation() 三角化函数：

实现的推导过程中可以看一下这篇博客里的SVD分解的部分

// 通过构建线性方程，然后SVD分解来计算特征点的位置。
/**
 * linear triangulation with SVD
 * @param poses     poses,
 * @param points    points in normalized plane
 * @param pt_world  triangulated point in the world
 * @return true if success
 */

inline bool triangulation(const std::vector<SE3> &poses,
                   const std::vector<Vec3> points, Vec3 &pt_world) {
    MatXX A(2 * poses.size(), 4);
    VecX b(2 * poses.size());
    b.setZero();
    for (size_t i = 0; i < poses.size(); ++i) {
        Mat34 m = poses[i].matrix3x4();
        A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
        A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
    }
    // Eigen::ComputeThinU和Eigen::ComputeThinV是两个参数，用于指定只计算矩阵的前k个奇异向量，其中k是矩阵的秩。这样可以加快计算速度并减小内存占用
    auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    /*
    取SVD分解得到v矩阵的最有一列作为解：svd.matrixV().col(3)
    深度值是svd.matrixV()(3, 3)
    head<3>() 是取前三个值
    */
    pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();

    if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) {
        return true;
    }
    return false;// 解质量不好，放弃
}