stvo-pl融合点线段特征的立体视觉里程计代码解读

摘要

基于点和线段的SLAM系统在点特征不充足的场景下具有一定的优势。本文对文章Robust Stereo Visual Odometry through a Probabilistic Combination of Points and Line Segments对应的源码进行记录。

主函数入口在imagesStVO.cpp中
首先进入初始化部分

if( frame_counter == 0 ) // initialize
            StVO->initialize(img_l,img_r,0);

初始化主要完成系统主要参数的设置
包括，fast角点的阈值，线段的最小长度阈值，初始化一个双目帧，然后提取视觉帧上的特征，包括点特征和线段特征，然后初始化位姿变换，位姿估计的协防差，DT，以及用于关键帧的一些参数。初始化的代码如下：

void StereoFrameHandler::initialize(const Mat img_l_, const Mat img_r_ , const int idx_)
{
    // variables for adaptative thresholds
    orb_fast_th = Config::orbFastTh();
    llength_th  = Config::minLineLength() * std::min( cam->getWidth(), cam->getHeight() ) ;
    // define StereoFrame
    prev_frame = new StereoFrame( img_l_, img_r_, idx_, cam );
    prev_frame->extractStereoFeatures( llength_th, orb_fast_th );
    prev_frame->Tfw     = Matrix4d::Identity();
    prev_frame->Tfw_cov = Matrix6d::Identity();
    prev_frame->DT      = Matrix4d::Identity();
    curr_frame = prev_frame;
    // SLAM variables for KF decision
    T_prevKF         = Matrix4d::Identity();
    cov_prevKF_currF = Matrix6d::Zero();
    prev_f_iskf      = true;
    N_prevKF_currF   = 0;
}

下面介绍特征提取模块

prev_frame->extractStereoFeatures( llength_th, orb_fast_th );

void StereoFrame::extractStereoFeatures( double llength_th, int fast_th )
{

    if( Config::plInParallel() )
    {
        auto detect_p = async(launch::async, &StereoFrame::detectStereoPoints,        this, fast_th );
        auto detect_l = async(launch::async, &StereoFrame::detectStereoLineSegments,  this, llength_th );
        detect_p.wait();
        detect_l.wait();
    }
    else
    {
        detectStereoPoints(fast_th);
        detectStereoLineSegments(llength_th);
    }

}

这里设置了并行计算，因为线段的提取和描述子计算都会提升计算力需求，所以为了提高在线效率，这里进行并行提取线段特征和点特征。
首先介绍立体图像上点特征的提取

void StereoFrame::detectStereoPoints( int fast_th )
{

    if( !Config::hasPoints() )
        return;

    // detect and estimate each descriptor for both the left and right image
    if( Config::lrInParallel() )
    {
        auto detect_l = async(launch::async, &StereoFrame::detectPointFeatures, this, img_l, ref(points_l), ref(pdesc_l), fast_th );
        auto detect_r = async(launch::async, &StereoFrame::detectPointFeatures, this, img_r, ref(points_r), ref(pdesc_r), fast_th );
        detect_l.wait();
        detect_r.wait();
    }
    else
    {
        detectPointFeatures( img_l, points_l, pdesc_l, fast_th );
        detectPointFeatures( img_r, points_r, pdesc_r, fast_th );
    }

    // perform the stereo matching
    matchStereoPoints(points_l, points_r, pdesc_l, pdesc_r, (frame_idx==0) );
}

这里也是设置了并行，对左右目图像进行分别特征点 提取，然后再进行立体视觉下的特征点匹配。提取特征点的代码如下

void StereoFrame::detectPointFeatures( Mat img, vector<KeyPoint> &points, Mat &pdesc, int fast_th )
{
    // Detect point features
    if( Config::hasPoints() )
    {
        int fast_th_ = Config::orbFastTh();
        if( fast_th != 0 )
            fast_th_ = fast_th;
        Ptr<ORB> orb = ORB::create( Config::orbNFeatures(), Config::orbScaleFactor(), Config::orbNLevels(),
                                    Config::orbEdgeTh(), 0, Config::orbWtaK(), Config::orbScore(),
                                    Config::orbPatchSize(), fast_th_ );
        orb->detectAndCompute( img, Mat(), points, pdesc, false);
    }

}

这里主要是提取orb特征点。然后是对特征点进行立体匹配。思路如下，首先对整个图像进行栅格化，计算每个orb对应的栅格的索引。存储在coords中。然后在每个栅格中存储位于其内的特征点。栅格化是为了对于每个特征点，能够快速的在一定范围中找到其潜在的匹配点。这里根据描述子对特征点进行匹配，然后根据第一小距离和第二小距离之比进行筛选。得到的匹配存储在matches_12中。然后就是进行反向的匹配，如果通过正反向的匹配，都是一样的结果，那就保留这个匹配。代码如下

int matchGrid(const std::vector<point_2d> &points1, const cv::Mat &desc1, const GridStructure &grid, const cv::Mat &desc2, const GridWindow &w, std::vector<int> &matches_12) {

    if (points1.size() != desc1.rows)
        throw std::runtime_error("[matchGrid] Each point needs a corresponding descriptor!");

    int matches = 0;
    matches_12.resize(desc1.rows, -1);

    int best_d, best_d2, best_idx;
    std::vector<int> matches_21, distances;

    if (Config::bestLRMatches()) {
        matches_21.resize(desc2.rows, -1);
        distances.resize(desc2.rows, std::numeric_limits<int>::max());
    }

    for (int i1 = 0; i1 < points1.size(); ++i1) {

        best_d = std::numeric_limits<int>::max();
        best_d2 = std::numeric_limits<int>::max();
        best_idx = -1;

        const std::pair<int, int> &coords = points1[i1];
        cv::Mat desc = desc1.row(i1);

        std::unordered_set<int> candidates;
        grid.get(coords.first, coords.second, w, candidates);

        if (candidates.empty()) continue;
        for (const int &i2 : candidates) {
            if (i2 < 0 || i2 >= desc2.rows) continue;

            const int d = distance(desc, desc2.row(i2));

            if (Config::bestLRMatches()) {
                if (d < distances[i2]) {
                    distances[i2] = d;
                    matches_21[i2] = i1;
                } else continue;
            }

            if (d < best_d) {
                best_d2 = best_d;
                best_d = d;
                best_idx = i2;
            } else if (d < best_d2)
                best_d2 = d;
        }

        if (best_d < best_d2 * Config::minRatio12P()) {
            matches_12[i1] = best_idx;
            matches++;
        }
    }

    if (Config::bestLRMatches()) {
        for (int i1 = 0; i1 < matches_12.size(); ++i1) {
            int &i2 = matches_12[i1];
            if (i2 >= 0 && matches_21[i2] != i1) {
                i2 = -1;
                matches--;
            }
        }
    }

    return matches;
}

得到匹配之后，对每对匹配进行立体的极线约束检验。在y方向的偏差应该足够小，在x方向，即视差应该足够大，这样的匹配再进行保存。然后通过重投影，得到三维特征点的坐标。关于立体视觉的深度恢复。

 // bucle around pmatches
    Mat pdesc_l_aux;
    int pt_idx = 0;
    for (int i1 = 0; i1 < matches_12.size(); ++i1) {
        const int i2 = matches_12[i1];
        if (i2 < 0) continue;

        // check stereo epipolar constraint
        if (std::abs(points_l[i1].pt.y - points_r[i2].pt.y) <= Config::maxDistEpip()) {
            // check minimal disparity
            double disp_ = points_l[i1].pt.x - points_r[i2].pt.x;
            if (disp_ >= Config::minDisp()){
                pdesc_l_aux.push_back(pdesc_l_.row(i1));
                Vector2d pl_(points_l[i1].pt.x, points_l[i1].pt.y);
                Vector3d P_ = cam->backProjection(pl_(0), pl_(1), disp_);
                if (initial)
                    stereo_pt.push_back(new PointFeature(pl_, disp_, P_, pt_idx++, points_l[i1].octave));
                else
                    stereo_pt.push_back(new PointFeature(pl_, disp_, P_, -1, points_l[i1].octave));
            }
        }
    }

然后存储到特征点的序列中，包括特征点的图像坐标，视差，空间点坐标，以及octave。
接下来介绍对线段特征的检测

void StereoFrame::detectStereoLineSegments(double llength_th)
{

    if( !Config::hasLines() )
        return;

    // detect and estimate each descriptor for both the left and right image
    if( Config::lrInParallel() )
    {
        auto detect_l = async(launch::async, &StereoFrame::detectLineFeatures, this, img_l, ref(lines_l), ref(ldesc_l), llength_th );
        auto detect_r = async(launch::async, &StereoFrame::detectLineFeatures, this, img_r, ref(lines_r), ref(ldesc_r), llength_th );
        detect_l.wait();
        detect_r.wait();
    }
    else
    {
        detectLineFeatures( img_l, lines_l, ldesc_l, llength_th );
        detectLineFeatures( img_r, lines_r, ldesc_r, llength_th );
    }

    // perform the stereo matching
    matchStereoLines(lines_l,  lines_r,  ldesc_l, ldesc_r, (frame_idx==0));

}

在detectLineFeatures( img_l, lines_l, ldesc_l, llength_th )存储了对直线的端点，方向，直线的位置是以直线段的中点定义的。直线的response是以直线段的长度除以图像长宽中的最大值。
然后对左右目的图像进行立体匹配。
这里存储的是左目起点，左目终点，起点终点的视差，以及对应的空间坐标。

stereo_ls.push_back( new LineFeature(Vector2d(sp_l(0),sp_l(1)),disp_s,sP_,
                                                     Vector2d(ep_l(0),ep_l(1)),disp_e,eP_,
                                                     le_l,angle_l,ls_idx,lines_l[i1].octave) );
                ls_idx++;

这里在预处理的时候要对左右匹配图像的端点的视差进行检查，是否满足相应的阈值。
现在已经知道了这两个特征对应的存储信息了。
后面就是对特征在连续帧上进行跟踪，然后进行位姿的优化。

StVO->insertStereoPair( img_l, img_r, frame_counter );
StVO->optimizePose();