如何插入LinK3D、CSF、BALM来直接插入各个SLAM框架中

0. 简介

LinK3D、CSF、BALM这几个都是非常方便去插入到激光SLAM框架的。这里我们会分别从多个角度来介绍如何将每个框架插入到SLAM框架中

1. LinK3D:三维LiDAR点云的线性关键点表示

LinK3D的核心思想和基于我们的LinK3D的两个LiDAR扫描的匹配结果。绿色线是有效匹配。当前关键点（黑色，CK）的描述符用其相邻关键点来表示。描述符的每个维度对应于扇区区域。第一维度对应于当前关键点的最近关键点所在的扇区区域（蓝色和红色、CK的最近关键点），并且其他维度对应于以逆时针顺序布置的区域。如果在扇区区域中存在关键点，则搜索扇区区域中最近的关键点（紫色和橙色，扇区中CK的最近关键点）并将其用于表示描述符的对应维度。

2. Link3D数据植入

这里的Link3D数据植入其实在外围调用就这些内容，当中AggregationKeypoints_LinK3D存储的是存当前点云中的聚类后的关键点。而pCurrentFrame_LinK3D对应的则是点云帧。该函数中利用LinK3D仿函数执行了提取边缘点，聚类，计算描述子的操作。其实主要实现的都是LinK3D提取器。

        //在这里植入LinK3D，把接收到的点云数据用LinK3D提取边缘点和描述子，发布关键点数据，打印输出描述子
        //LinK3D提取器
        BoW3D::LinK3D_Extractor* pLinK3dExtractor(new BoW3D::LinK3D_Extractor(nScans, scanPeriod_LinK3D, minimumRange, distanceTh, matchTh)); 
        //创建点云帧,该函数中利用LinK3D仿函数执行了提取边缘点，聚类，计算描述子的操作
        Frame* pCurrentFrame_LinK3D(new Frame(pLinK3dExtractor, plaserCloudIn_LinK3D));
        //此时pCurrentFrame_LinK3D这个类指针中包含了边缘点，聚类，描述子的信息
//测试 输出关键点数量和第一个关键点信息 正常输出 
// cout << "------------------------" << endl << "关键点数量:" << pCurrentFrame_LinK3D->mvAggregationKeypoints.size();
// cout << "第一个关键点信息x坐标" << pCurrentFrame_LinK3D->mvAggregationKeypoints[0].x;
        //存当前点云中的聚类后的关键点
        AggregationKeypoints_LinK3D->points.insert(AggregationKeypoints_LinK3D->points.end(), pCurrentFrame_LinK3D->mvAggregationKeypoints.begin(), pCurrentFrame_LinK3D->mvAggregationKeypoints.end());
//测试 输出点云中信息 也能正常输出
// cout << "------------------------" << endl << "关键点数量:" << AggregationKeypoints_LinK3D->points.size();
// cout << "第一个关键点信息x坐标" << AggregationKeypoints_LinK3D->points[0].x;
        // 2.对描述子进行匹配 3.使用匹配对进行帧间icp配准 pPreviousFrame是上一个link3d Frame帧 pCurrentFrame_LinK3D是当前link3d Frame帧
        // 获取上一帧和当前帧之间的匹配索引
         vector<pair<int, int>> vMatchedIndex;  
        pLinK3dExtractor->match(pCurrentFrame_LinK3D->mvAggregationKeypoints, pPreviousFrame->mvAggregationKeypoints, pCurrentFrame_LinK3D->mDescriptors, pPreviousFrame->mDescriptors, vMatchedIndex);
        //仿照BoW3D函数写一个帧间ICP匹配函数求出R,t
        int returnValue = 0;
        // 进行帧间ICP匹配 求当前帧到上一帧的位姿变换
        // 这里求的R t是当前帧点云到上一帧点云的位姿变换
        returnValue = pose_estimation_3d3d(pCurrentFrame_LinK3D, pPreviousFrame, vMatchedIndex, RelativeR, Relativet, pLinK3dExtractor);
        //至此获得了当前帧点云到上一帧点云的位姿变换

        //当前帧Frame用完以后，赋值给上一帧Frame,赋值前先把要丢掉的帧内存释放
        //这里Frame里有成员指针，析构函数里delete成员指针
        delete pPreviousFrame;
        pPreviousFrame = pCurrentFrame_LinK3D;
        //LinK3D 植入结束

点云帧配置如下，其实可以看到这里面没有太多的内容，主要还是调用Link3D中的void LinK3D_Extractor::operator()(pcl::PointCloud::Ptr pLaserCloudIn, vector &keyPoints, cv::Mat &descriptors, ScanEdgePoints &validCluster)函数。并获取关键点、描述子还有聚类信息。具体的实现与具体论文保持一致，可以看Github内容

    //静态（全局？）变量要在这里初始化
    long unsigned int Frame::nNextId = 0;

    Frame::Frame(LinK3D_Extractor* pLink3dExtractor, pcl::PointCloud<pcl::PointXYZ>::Ptr pLaserCloudIn):mpLink3dExtractor(pLink3dExtractor)
    {
        mnId = nNextId++; 

        (*mpLink3dExtractor)(pLaserCloudIn, mvAggregationKeypoints, mDescriptors, mClusterEdgeKeypoints);
    }

2. CSF“布料”滤波算法

然后下面就是CSF的处理，这里其实可以将内容加在BALM当中，Github。因为CSF其实作用是区分地面点的作用

  // 
  CSF csf;
  csf.params.iterations = 600;
  csf.params.time_step = 0.95;
  csf.params.cloth_resolution = 3;
  csf.params.bSloopSmooth = false;

  csf.setPointCloud(*laserCloudSurfLast);
  // pcl::io::savePCDFileBinary(map_save_directory, *SurfFrame);

  std::vector<int>  groundIndexes, offGroundIndexes;
  // 输出的是vector<int>类型的地面点和非地面点索引
  pcl::PointCloud<pcl::PointXYZI>::Ptr groundFrame(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr offGroundFrame(new pcl::PointCloud<pcl::PointXYZI>);
  csf.do_filtering(groundIndexes, offGroundIndexes);
  pcl::copyPointCloud(*laserCloudSurfLast, groundIndexes, *groundFrame);
  pcl::copyPointCloud(*laserCloudSurfLast, offGroundIndexes, *offGroundFrame);

实际用一句话：把点云翻过来，罩上一块不同材质的布料，就可以得到地面了。参考实际物理的布，布料上的点之间存在不同的作用力，详细可以参考这篇文章：给地面盖一层布！——CSF算法提取地面点 - 古月居

3. BALM

对于BALM其实主要分为三块，balm_front、scan2map、balm_back这三个流程。我们一开始使用的是ALOAM的前端来提取激光里程计信息，然后scan2map中加入CSF来进一步区分地面点。最后分成三类传入到BA约束当中经典文献阅读之--BALM(用于雷达建图的集束调整BA)_slam balm-CSDN博客。

点击如何插入LinK3D、CSF、BALM来直接插入各个SLAM框架中——古月居可查看全文