loam代码分析（2）

最新推荐文章于 2024-02-18 20:20:29 发布

jiajiading

最新推荐文章于 2024-02-18 20:20:29 发布

阅读量1.7k

点赞数 2

分类专栏： slam 文章标签： loam

本文链接：https://blog.csdn.net/jiajiading/article/details/102818428

版权

slam 专栏收录该内容

29 篇文章 89 订阅

订阅专栏

loam代码分析（2）

FeatureAssociation

目前最新开源的3d slam算法lego-loam，为loam的改进版。同时另有高人进行了工程优化。
其原作者github
其工程优化的github
个人对其工程优化后的代码和原理进行了基本分析；
包含：

imageProjecion
FeatureAssociation
mapOptmization
transformFusion
这四个文件

FeatureAssociation

FeatureAssociation.cpp同样为独立线程，主要功能是根据带有聚类后地面和非地面的点云数据进行特征值提取，主要提取其平面特征、角点特征；同时采用匹配算法计算前后两帧激光视角下的位姿转换；从而更新里程计；其流程如下：

畸变矫正（未能理解）；
计算每个点平滑性参数；
对每个点进行标记，用于忽略此点特征提取；
平面特征点和角点特征提取；
计算两帧位姿转换；
计算里程计；

畸变矫正（adjustDistortion）

此函数暂时未能理解，后续；

计算平滑参数（calculateSmoothness）

平滑性采用的方法为将当前点的距离与前后各5点计算误差累计和；显然误差累计和较大，则此点属于跳跃点，或边缘点；

void FeatureAssociation::calculateSmoothness() {
  int cloudSize = segmentedCloud->points.size();                         
  for (int i = 5; i < cloudSize - 5; i++) {
    float diffRange = segInfo.segmentedCloudRange[i - 5] +               // 当前点的10倍与水平方向上前后5个点，距离差。差越大，表明当前点为1边缘点
                      segInfo.segmentedCloudRange[i - 4] +               // 连续点云边缘的点最大
                      segInfo.segmentedCloudRange[i - 3] +
                      segInfo.segmentedCloudRange[i - 2] +
                      segInfo.segmentedCloudRange[i - 1] -
                      segInfo.segmentedCloudRange[i] * 10 +
                      segInfo.segmentedCloudRange[i + 1] +
                      segInfo.segmentedCloudRange[i + 2] +
                      segInfo.segmentedCloudRange[i + 3] +
                      segInfo.segmentedCloudRange[i + 4] +
                      segInfo.segmentedCloudRange[i + 5];

    cloudCurvature[i] = diffRange * diffRange;

    cloudNeighborPicked[i] = 0;
    cloudLabel[i] = 0;

    cloudSmoothness[i].value = cloudCurvature[i];
    cloudSmoothness[i].ind = i;
  }
}

特征点屏蔽标记（markOccludedPoints）

在提取特征点时，防止提取同一位置附近的多个点，或者边缘跳跃点，防止提取远处跳跃近处时的点，由于雷达旋转测距方式，近处障碍无遮挡导致远处边缘的点并不固定。
标记的例子如下图，红色点标记为屏蔽点：
在这里插入图片描述

void FeatureAssociation::markOccludedPoints() {
  int cloudSize = segmentedCloud->points.size();

  for (int i = 5; i < cloudSize - 6; ++i) {
    float depth1 = segInfo.segmentedCloudRange[i];
    float depth2 = segInfo.segmentedCloudRange[i + 1];                    // 遍历提取相邻的两个点的距离
    int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] -   // 获取相邻两个点的在水平方向上的索引号差
                                  segInfo.segmentedCloudColInd[i]));

    if (columnDiff < 10) {                                                // 如果水平索引在10个点内，将远处的边缘的5个点标记为1,
      if (depth1 - depth2 > 0.3) {
        cloudNeighborPicked[i - 5] = 1;                                   // 近处的边缘点则为0
        cloudNeighborPicked[i - 4] = 1;
        cloudNeighborPicked[i - 3] = 1;
        cloudNeighborPicked[i - 2] = 1;
        cloudNeighborPicked[i - 1] = 1;
        cloudNeighborPicked[i] = 1;
      } else if (depth2 - depth1 > 0.3) {
        cloudNeighborPicked[i + 1] = 1;
        cloudNeighborPicked[i + 2] = 1;
        cloudNeighborPicked[i + 3] = 1;
        cloudNeighborPicked[i + 4] = 1;
        cloudNeighborPicked[i + 5] = 1;
        cloudNeighborPicked[i + 6] = 1;
      }
    }

    float diff1 = std::abs(segInfo.segmentedCloudRange[i - 1] -
                           segInfo.segmentedCloudRange[i]);
    float diff2 = std::abs(segInfo.segmentedCloudRange[i + 1] -
                           segInfo.segmentedCloudRange[i]);

    if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] &&                  // 如果两个相邻点的距离超过本点的距离0.02倍时，也标记为1，即孤立的点标记为1
        diff2 > 0.02 * segInfo.segmentedCloudRange[i])
      cloudNeighborPicked[i] = 1;
  }
}

平面点和角点提取 extractFeatures（）

平面点及其角点提取时，均是对16线中其中一条激光线点云进行判断是否为连续点还是断开边缘点。因此垂直方向上16条线独立处理。
由于每条激光线均是360度，并将其一圈的所有数据进行平均分成6等份分别进行特征点提取；
取其中1份，将每个点按照平滑参数从小到大进行排序；

角点特征点提取

将平滑参数从大到小进行遍历，即如此可最先遍历到最大的跳跃点，即边缘特征点。
其中边缘特征点条件：

不得是远处跳跃到近处的边缘点；
不得是地面上点；
并且平滑性需大于一定值；
特征点相邻的5个点也不得是特征点，即需要设置屏蔽位
角点示例，其中蓝色点为角点，红色点为屏蔽点：

        int ind = cloudSmoothness[k].ind;                                        // 提取索引
        if (cloudNeighborPicked[ind] == 0 &&                                     // 当前并非远方点变近的边缘的点、并且平滑度大于一定值、非地面数据（结论：就是提取水平方向连续断开的端点，且仅提取断开近处的端点）
            cloudCurvature[ind] > _edge_threshold &&                             // ？？？？？？？这里是提取出连续点中断开的端点，即连续点云断开的边缘点？？？？
            segInfo.segmentedCloudGroundFlag[ind] == false) {
          largestPickedNum++;                                                    // 统计满足上条件的个数
          if (largestPickedNum <= 2) {                                           // 记录最大的两个点，为最陡的两个点，即一圈最多6*2 = 12个点
            cloudLabel[ind] = 2;
            cornerPointsSharp->push_back(segmentedCloud->points[ind]);
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
          } else if (largestPickedNum <= 20) {                                   // 前20个点为稍微陡峭的两个点， 
            cloudLabel[ind] = 1;
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
          } else {                                                               // 后面无需考虑
            break;
          }
                    cloudNeighborPicked[ind] = 1;                                          // 此点近距离边缘端点，已经处理过并将其设置为1
          for (int l = 1; l <= 5; l++) {                                         // 如果当前点在最后5个点内，不处理
            if( ind + l >= segInfo.segmentedCloudColInd.size() ) {
              continue;
            }
            int columnDiff =
                std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
                             segInfo.segmentedCloudColInd[ind + l - 1]));        // 如果相邻有效索引的两点，在水平上索超出10（即10个水平角度分辨率），便跳出，即不平坦
            if (columnDiff > 10) break;
            cloudNeighborPicked[ind + l] = 1;                                    // 即与当前点相对平滑的5个点设置为1，
          }
          for (int l = -1; l >= -5; l--) {                                       // 若当前点在前5个点内，不处理
            if( ind + l < 0 ) {
              continue;
            }
            int columnDiff =
                std::abs(int(segInfo.segmentedCloudColInd[ind + l] -             // 同理 
                             segInfo.segmentedCloudColInd[ind + l + 1]));
            if (columnDiff > 10) break;
            cloudNeighborPicked[ind + l] = 1;
          }

平面特征点提取

将平滑参数从小到大进行遍历，即如此可最先遍历到最平滑点，连续平滑中间的点。
其中平滑特征点条件：

不得是已标记；
必须是地面上点；
并且平滑性需小于一定值；
特征点相邻的5个点也不得是特征点，即需要设置屏蔽位

      for (int k = sp; k <= ep; k++) {                                           // 平滑度从小到大遍历
        int ind = cloudSmoothness[k].ind;
        if (cloudNeighborPicked[ind] == 0 &&
            cloudCurvature[ind] < _surf_threshold &&                             // 平滑的点，且是地面上的点
            segInfo.segmentedCloudGroundFlag[ind] == true) {
          cloudLabel[ind] = -1;                                                  // 标记平滑地面的点为-1
          surfPointsFlat->push_back(segmentedCloud->points[ind]);                // 放入平滑地面点云

          smallestPickedNum++;
          if (smallestPickedNum >= 4) {                                          // 即一圈最多为6*4 = 24个点
            break;
          }

          cloudNeighborPicked[ind] = 1;
          for (int l = 1; l <= 5; l++) {
            if( ind + l >= segInfo.segmentedCloudColInd.size() ) {
              continue;
            }
            int columnDiff =
                std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) -
                             segInfo.segmentedCloudColInd.at(ind + l - 1)));
            if (columnDiff > 10) break;                                           // 每选择一个点，则便使相邻的5个点，若是平滑的，则使其标记为1，

            cloudNeighborPicked[ind + l] = 1;                                     // 此操作可作为降采样功能，使特征点至少间隔5个点
          }
          for (int l = -1; l >= -5; l--) {
            if (ind + l < 0) {
              continue;
            }
            int columnDiff =
                std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) -
                             segInfo.segmentedCloudColInd.at(ind + l + 1)));
            if (columnDiff > 10) break;

            cloudNeighborPicked[ind + l] = 1;
          }
        }
      }

计算两帧位姿转换（updateTransformation）

此模块主要功能是采用当前帧的特征点云，包括平面点云和角点点云；与上一帧点云进行匹配，求去坐标转换；包括两部分：

采用平面点云计算依据激光视角下的tz,poll,pitch转换；
采用角点计算依据激光视角下的yaw， x， y的转换；

根据论文可知，点云匹配采用L-M优化算法，代码暂时未看懂。
未完待续；

里程计获取（integrateTransformation）

由于上一步经过两帧激光点云的匹配，则可计算出当前激光视角与上帧激光视角的位姿转换包括旋转delta_R 和平移delta_T。
上帧激光视角在世界坐标系下的位姿转换为（R,T）。因此可获取当前帧在世界坐标系下的坐标转换:

$P_{curr} = R_{delta}*P_{last} + T_{delta}$
$P_{last} = R * P_{init} + T$
故:
$P_{curr} = R_{delta}*(R * P_{init} + T) + T_{delta}$
$P_{curr} = R_{delta}*R * P_{init} + (R_{delta}*T + T_{delta})$
从而获取里程计

具体代码分析，未完待续；

jiajiading

关注

2
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
loam代码分析（2）

loam代码分析（2）FeatureAssociation畸变矫正（adjustDistortion）计算平滑参数（calculateSmoothness）特征点屏蔽标记（markOccludedPoints）平面点和角点提取 extractFeatures（）角点特征点提取平面特征点提取计算两帧位姿转换（updateTransformation）里程计获取（integrateTransform...
复制链接

扫一扫