回环检测 Scan Contex 及扩展-优快云博客

本文链接：https://blog.youkuaiyun.com/qq_41451702/article/details/149156922

1.论文

Scan Context 应用于基于3D点云的重定位和场景识别，主要思想是将场景3维信息压缩，将笛卡尔坐标系的信息转换到极坐标系下计算。优势是高效利用场景点云分布特征，引入“旋转不变性”描述子，快速搜索。

首先如图 (a) 所示，我们使用点云的俯视图（来自一次 3D 扫描）并将地面区域划分为若干网格。这些网格根据方位角（在一个 LiDAR 帧中从 0 到 2π）和距离（从中心到最大感知范围）两个方向进行划分。其中，黄色区域称为 Ring（环），青色区域称为 Sector（扇区），黑色填充区域称为 Bin（网格单元）。

最终生成的 Scan Context 如图 (b) 所示，是一个矩阵，它明确地保留了点云的绝对几何结构。图 (a) 中的环和扇区分别对应于图 (b) 中的同色列和行。从每个 bin 中的点提取一个代表性数值，用作图 (b) 中对应像素的值。在本文中，使用的是每个 bin 内点的最大高度值。

Scan-Context描述子分为Ring和Sector，Ring类似于极坐标中的角度，Sector类似于极坐标中的长度。

Sector 的数量为 60，表示角度范围为 0~360°，每个 Sector 的分辨率为 6°；
Ring 的数量为 20，表示距离范围为 0~Lmax。

3D点云被Ring和Sector划分成2D图像（20*60= 1200个bin(格子）），每个格子的值是落在这个格子里面所有点的最大高度（z值）。对照图（b），把图（a）俯视的圆环图剪开，变成矩形，横向对应一个环，纵向对应距离从近到远。不同的颜色，表示这个格子中点的最大高度。

主要流程：

读取单帧3D点云数据，建立Scan-Context，在Mapping过程中生成KeyFrame中查找，使用Ring-key在KD-tree下查找最近邻结果，结果计算统计得分，得到最佳匹配，完成回环检测。

对一帧点云先进行 SC（Scan Context）的计算；
从该帧点云的 SC 中提取出一个 N 维的向量（和环数一致）用于在 KD 树中搜索相近的关键帧；
将搜索得到的参考帧的 SC 和待匹配的当前帧进行比较，如果比较得分高于一定阈值则认为找到回环；

2.具体使用

2.1 单帧点云生成scan context描述子，计算ring key, sector key

void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
    Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 
    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );

    polarcontexts_.push_back( sc ); 
    polarcontext_invkeys_.push_back( ringkey );
    polarcontext_vkeys_.push_back( sectorkey );
    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );

    // cout <<polarcontext_vkeys_.size() << endl;

} // SCManager::makeAndSaveScancontextAndKeys

2.2 回环检测，找到当前帧最相似的回环帧

std::pair<int, float> detectLoopClosureID( void );

总结：使用scan context只需要调用这两个函数即可。以当前帧寻找回环帧举例说明

// save and load  scan context
void saveSCD(std::string fileName, Eigen::MatrixXd matrix, std::string delimiter = " ")
{
    
    int precision = 3;
    const static Eigen::IOFormat the_format(precision, Eigen::DontAlignCols, delimiter, "\n");
 
    std::ofstream file(fileName);
    if (file.is_open())
    {
        file << matrix.format(the_format);
        file.close();
    }
}

std::string padZeros(int val, int num_digits = 6) {
  std::ostringstream out;
  out << std::internal << std::setfill('0') << std::setw(num_digits) << val;
  return out.str();
}

void loadSCD(std::string fileName, Eigen::MatrixXd& matrix, char delimiter = ' ')
{
    std::vector<double> matrixEntries;
    std::ifstream matrixDataFile(fileName);
    if (!matrixDataFile.is_open())
    {
        std::cout << "文件名："<< fileName <<std::endl;
        std::cout << "读入SCD文件失败!!"<< std::endl;
        return;
    }

    std::string matrixRowString;
    std::string matrixEntry;
    int matrixRowNumber = 0;
    while (getline(matrixDataFile, matrixRowString)) 
    {
        std::stringstream matrixRowStringStream(matrixRowString);
 
        while (getline(matrixRowStringStream, matrixEntry,delimiter)) 
        {
            matrixEntries.push_back(stod(matrixEntry)); 
        }
        matrixRowNumber++; 
    }
    
    matrix =  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(matrixEntries.data(), matrixRowNumber, matrixEntries.size() / matrixRowNumber);
    matrixDataFile.close();
}
// step 1 load scds
for (size_t i = 0; i < fileCount; ++i) {
    std::string filename = padZeros(i) + ".scd";
    std::string scd_path = loadSCDDirectory + filename;
    Eigen::MatrixXd load_sc;
    loadSCD(scd_path, load_sc);

    Eigen::MatrixXd ringkey   = scManager.makeRingkeyFromScancontext(load_sc);
    Eigen::MatrixXd sectorkey = scManager.makeSectorkeyFromScancontext(load_sc);
    auto polarcontext_invkey_vec = eig2stdvec(ringkey);

    scManager.polarcontexts_.push_back(load_sc);
    scManager.polarcontext_invkeys_.push_back(ringkey);
    scManager.polarcontext_vkeys_.push_back(sectorkey);
    scManager.polarcontext_invkeys_mat_.push_back(polarcontext_invkey_vec);
}


// step 2 当前帧生成sc 描述子
scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);

// step 3 寻找当前帧的回环帧
auto detectResult = scManager.detectLoopClosureID(); 
int loopKeyPre = detectResult.first;

注：上述代码只是返回一个最相似的回环帧，实际使用的是时候发现最相似的回环帧进行重定位效果不一定是最好的，可以多返回几个候选回环帧。

3.扩展

作者也给出了基于scan context的多个扩展代码，重点关注下面两个：

Integrated with LIO-SAM: SC-LIO-SAM
Integrated with FAST-LIO2: FAST_LIO_SLAM

3.1 SC-LIO-SAM

相比LIO-SAM修改的代码

1.保存关键帧的scan context描述子，saveKeyFramesAndFactor()

// todo：每一帧(关键帧)点云生成scan context 描述子
// ***********************************************************
// Scan Context loop detector - giseop
// - SINGLE_SCAN_FULL: using downsampled original point cloud (/full_cloud_projected + downsampling)
// - SINGLE_SCAN_FEAT: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.)
// - MULTI_SCAN_FEAT: using NearKeyframes (because a MulRan scan does not have beyond region, so to solve this issue ... )
const SCInputType sc_input_type = SCInputType::SINGLE_SCAN_FULL; // change this 

if (sc_input_type == SCInputType::SINGLE_SCAN_FULL) {
    pcl::PointCloud<PointType>::Ptr thisRawCloudKeyFrame(new pcl::PointCloud<PointType>());
    pcl::copyPointCloud(*laserCloudRawDS, *thisRawCloudKeyFrame);
    scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
}  
else if (sc_input_type == SCInputType::SINGLE_SCAN_FEAT) { 
    scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame); 
}
else if (sc_input_type == SCInputType::MULTI_SCAN_FEAT) { 
    pcl::PointCloud<PointType>::Ptr multiKeyFrameFeatureCloud(new pcl::PointCloud<PointType>());
    loopFindNearKeyframes(multiKeyFrameFeatureCloud, cloudKeyPoses6D->size() - 1, historyKeyframeSearchNum);
    scManager.makeAndSaveScancontextAndKeys(*multiKeyFrameFeatureCloud); 
}

// save sc data
// curr_scd当前点云(关键帧)的描述子
const auto& curr_scd = scManager.getConstRefRecentSCD();
std::string curr_scd_node_idx = padZeros(scManager.polarcontexts_.size() - 1);
saveSCD(saveSCDDirectory + curr_scd_node_idx + ".scd", curr_scd);

// save keyframe cloud as file giseop
bool saveRawCloud { true };
pcl::PointCloud<PointType>::Ptr thisKeyFrameCloud(new pcl::PointCloud<PointType>());
if (saveRawCloud) { 
    *thisKeyFrameCloud += *laserCloudRaw;
} else {
    *thisKeyFrameCloud += *thisCornerKeyFrame;
    *thisKeyFrameCloud += *thisSurfKeyFrame;
}
pcl::io::savePCDFileBinary(saveNodePCDDirectory + curr_scd_node_idx + ".pcd", *thisKeyFrameCloud);
pgTimeSaveStream << laserCloudRawTime << std::endl;
// ***********************************************************

2.loopClosureThread线程中新增通过scan context找回环帧的方法performSCLoopClosure()

注：performRSLoopClosure原代码回环帧的条件是 在历史关键帧中查找与当前帧空间距离较近但时间上相隔足够远的关键帧

通过scan context找回环帧的方法封装在 detectLoopClosureID()中，后续当前帧和回环帧通过ICP修正位姿的方法和performRSLoopClosure()函数一样。

void performSCLoopClosure()
{
    if (cloudKeyPoses3D->points.empty())
        return;

    // 1. SC描述子检索回环，获得历史最相似帧索引和相对yaw
    auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff 
    int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;  // 当前关键帧索引
    int loopKeyPre = detectResult.first;                // 历史匹配帧索引
    float yawDiffRad = detectResult.second; // 未使用

    if (loopKeyPre == -1) // 未找到回环
        return;

    std::cout << "SC loop found! between " << loopKeyCur << " and " << loopKeyPre << "." << std::endl;

    // 2. 提取当前帧与回环帧的局部点云
    pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
    {
        int base_key = 0;
        loopFindNearKeyframesWithRespectTo(cureKeyframeCloud, loopKeyCur, 0, base_key);
        loopFindNearKeyframesWithRespectTo(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum, base_key);

        if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
            return;
        if (pubHistoryKeyFrames.getNumSubscribers() != 0)
            publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
    }

    // 3. ICP 配准
    static pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setMaxCorrespondenceDistance(150); // 建议设为2倍历史帧窗口距离
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-6);
    icp.setEuclideanFitnessEpsilon(1e-6);
    icp.setRANSACIterations(0);

    icp.setInputSource(cureKeyframeCloud);
    icp.setInputTarget(prevKeyframeCloud);
    pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    icp.align(*unused_result);

    if (!icp.hasConverged() || icp.getFitnessScore() > historyKeyframeFitnessScore) {
        std::cout << "ICP fitness test failed (" << icp.getFitnessScore() << " > " << historyKeyframeFitnessScore << "). Reject this SC loop." << std::endl;
        return;
    } else {
        std::cout << "ICP fitness test passed (" << icp.getFitnessScore() << " < " << historyKeyframeFitnessScore << "). Add this SC loop." << std::endl;
    }

    // 4. 可视化配准结果
    if (pubIcpKeyFrames.getNumSubscribers() != 0) {
        pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
        publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
    }

    // 5. 获取两帧之间的配准位姿（T_cur->pre）
    float x, y, z, roll, pitch, yaw;
    Eigen::Affine3f correctionLidarFrame = icp.getFinalTransformation();
    pcl::getTranslationAndEulerAngles(correctionLidarFrame, x, y, z, roll, pitch, yaw);
    gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
    gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));

    // 6. 构建鲁棒回环因子（Cauchy核）
    float robustNoiseScore = 0.5;
    gtsam::Vector robustNoiseVector6(6); 
    robustNoiseVector6 << robustNoiseScore, robustNoiseScore, robustNoiseScore,
                          robustNoiseScore, robustNoiseScore, robustNoiseScore;
    noiseModel::Base::shared_ptr robustConstraintNoise; 
    robustConstraintNoise = gtsam::noiseModel::Robust::Create(
        gtsam::noiseModel::mEstimator::Cauchy::Create(1),
        gtsam::noiseModel::Diagonal::Variances(robustNoiseVector6)
    );

    // 7. 存入因子队列
    mtx.lock();
    loopIndexQueue.push_back(std::make_pair(loopKeyCur, loopKeyPre));
    loopPoseQueue.push_back(poseFrom.between(poseTo));
    loopNoiseQueue.push_back(robustConstraintNoise);
    mtx.unlock();

    // 8. 可选，存储回环关系
    loopIndexContainer.insert(std::pair<int, int>(loopKeyCur, loopKeyPre));
}

3.回环索引记录不同

// 原代码
map<int, int> loopIndexContainer;
loopIndexContainer[loopKeyCur] = loopKeyPre;

// SC_LIO-SAM
multimap<int, int> loopIndexContainer;
loopIndexContainer.insert(std::pair<int, int>(loopKeyCur, loopKeyPre));

map (不允许重复 key)，若同一个 loopKeyCur 再次发生回环（新的 loopKeyPre），原有记录会被覆盖，每个关键帧最多只会保留一条回环记录；

multimap(允许重复 key)，同一个关键帧 loopKeyCur 添加多个回环记录（即多个 loopKeyPre 值）

4.performRSLoopClosure() 函数中ICP的源点云和目标点云之间匹配点对的最大距离setMaxCorrespondenceDistance修改为150m，原代码是historyKeyframeSearchRadius*2 （30m）

// 原代码
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);        
// SC_LIO-SAM
icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter