激光閉環檢測Scancontext閱讀筆記

本文轉載自查看原文 2020-07-26 15:28 1140 論文源碼閱讀

一、構建Scan Context
二、計算Scan Context的每一行的均值作為RingKey
三、將所有歷史幀的RingKey構建Kd-tree查找候選關鍵幀
四、遍歷候選關鍵幀，選擇距離最近的幀作為匹配幀。
- 計算距離的步驟
五、再用icp匹配
優缺點
- 優點
- 缺點
打賞
- 支付寶
- 微信

Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map(IROS2018)
在這里插入圖片描述

一、構建Scan Context

在這里插入圖片描述 \(N_r\)：ring分成多少份
\(N_s\): sectors分成多少份

// 制作Scancontext
MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
    TicToc t_making_desc;

    int num_pts_scan_down = _scan_down.points.size();

    // main
    // 初始化desc=Scancontext
    const int NO_POINT = -1000;
    MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);

    SCPointType pt;
    float azim_angle, azim_range; // 極坐標wihtin 2d plane
    int ring_idx, sctor_idx; //Scancontext中的索引，注意是從1開始的
    for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
    {
        pt.x = _scan_down.points[pt_idx].x; 
        pt.y = _scan_down.points[pt_idx].y;
        pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).

        // xyz to ring, sector
        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
        azim_angle = xy2theta(pt.x, pt.y);

        // if range is out of roi, pass
        // 超出范圍的去掉
        if( azim_range > PC_MAX_RADIUS )
            continue;

        ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
        sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );

        // taking maximum z 
        // 獲取每個bin中的z最大值
        if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
            desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
    }

    // reset no points to zero (for cosine dist later)
    // 沒有點的bin=0
    for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
        for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
            if( desc(row_idx, col_idx) == NO_POINT )
                desc(row_idx, col_idx) = 0;

    t_making_desc.toc("PolarContext making");

    return desc;
} // SCManager::makeScancontext

二、計算Scan Context的每一行的均值作為RingKey

//Scancontext每一行的均值作為Ringkey
MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    /* 
     * summary: rowwise mean vector
    */
    Eigen::MatrixXd invariant_key(_desc.rows(), 1);
    for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
    {
        Eigen::MatrixXd curr_row = _desc.row(row_idx);
        invariant_key(row_idx, 0) = curr_row.mean();
    }

    return invariant_key;
} // SCManager::makeRingkeyFromScancontext