如何插入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 植入结束