3D激光SLAM核心：为什么要进行点云特征提取

1.为什么要进行点云特征提取

首先要弄明白为什么要进行特征提取。搞过2D激光SLAM的人都晓得，在进行扫描匹配的时候，经常是直接使用原始点云数据。这是因为一般2D激光SLAM所用的单线激光雷达每一帧点云数非常的少，一般几千个点都算多的了。因此点云中的所有点都可以参与运算，性能不会有大的问题。

而3D激光SLAM中所用的一般是多线激光雷达，每一个数据帧中的点云数量非常的多。像KITTI数据集中所用的Velodyne 16线激光雷达，一般点云数在几万。而Velodyne 64线激光雷达点云数量更是多达十几万。这个数量太过庞大，如果每一个点都参与运算，那么对计算资源的需求将是不能接受的。

这里感兴趣的同学可以使用PCL库的ICP方法测试一下，测试代码可以见于：

_icp_test at ddf · softdream/3d_lidar_slam (github.com) github.com/softdream/3d_lidar_slam/tree/ddf/pcl_icp_test

点云特征提取就是为了尽可能降低参与运算的点的数量。在3D激光中，虽然特征提取不像视觉中那么方便，但是点云的空间结构化信息是可以利用起来的。最常见的有利用法向量提取特征(典型的PFH)，利用曲率提取特征(loam及其一系列衍生方法所用的方法)。

2. 本项目所用的特征提取方法说明

在本项目中，将参考loam方法中使用的特征提取方法。loam长盛不衰，与其简洁又高效的的特征提取方式脱不开关系，因此极具借鉴意义。当然，本人也将探索新型的特征提取方法，特别是基于深度图(Range Image)的特征提取方法非常有吸引力，这个后续会另开分支，单独介绍这种方法。

3. 特征提取理论与代码编写

特征提取这块原理比较简单，主要麻烦点在代码编写中，这个后面会叙述。loam及其衍生型的特征提取方法都是对原始点云提取Corner Point和Plane Point。而提取特征点的依据就是曲率。

那么重点就在于如何计算曲率了。

根据loam论文中所述，在计算某一点的曲率时，需要找到该点前后五个点（包括该点，一共11个点）一起来计算，计算公式为：

其中 c 是当前点 i 的曲率， S 是当前点周围前后11个点的集合，  是当前点 i 的坐标，那么   就是集合中其它点的坐标。

实际在编程的过程中要注意的地方在于如何确定集合 S 。这个后面会详细说。这里列出特征提取的主要步骤：

3.1 确定激光点所在的扫描线并建立点的索引

在loam系列方法中，会根据激光点的垂直方向的出射角确定出它在哪个扫描线上。第 i 个点所在的角度为：

 

此外我们还需要查阅手册确定出激光雷达在垂直方向的视场角以及垂直方向的角度分辨率来确定角度和线数的关系。以KITTI所用的Velodyne为例，其垂直视场角范围是30°(+15°~-15°)，角度分辨率为2°。

代码如下：

 

 

   int scan_idx = -1;
 typename CloudType::ValueType angle = ::atan2( pt.z, ::sqrt( pt.x * pt.x + pt.y * pt.y ) ) * 180 / M_PI;
       
 if constexpr ( Config::N_SCANS == 16 ) {
  if ( angle >= -15 && angle <= 15 ) {
   scan_idx = static_cast( ( angle + 15 ) / 2 + 0.5 );
  }
 } 

 

 

这里可以根据激光点所在线数对其进行一个分类，即每个激光扫描线上的点放在一个容器里，一共16个。还需要对每一个点建立一个索引，方便后续可以直接根据该索引找到这个点，对应代码为：

 

 

if ( scan_idx > -1 && scan_idx < Config::N_SCANS ) {
  scans_row_data_vec[scan_idx].push_back( pt );   
  point_index_vec[scan_idx].push_back( i );
 }

 

 

3.2 计算每个点的曲率

上面说了要计算曲率必须先找到集合 S ，我们在3.1步骤中确定每个点所在的线数并将其加入到对应的容器中就是为了给这个做准备的。因为在loam系列的方法中，集合 S 是在每个点所在扫描线上去找该点前后各5个点的。这很大程度上简化了算法过程，因为不用考虑激光点上下两个方向上的点了。

 

 

for ( size_t i = Config::Row_Index_Start; i < Config::N_SCANS - Config::Row_Index_End; i ++ ) {
 scans_row_curv_vec[i].resize( scans_row_data_vec[i].size() );
 for( size_t j = 5; j < scans_row_data_vec[i].size() - 5; j ++ ) {
  typename CloudType::ValueType diff_x = scans_row_data_vec[i][j - 5].x + scans_row_data_vec[i][j - 4].x + scans_row_data_vec[i][j - 3].x + scans_row_data_vec[i][j - 2].x + scans_row_data_vec[i][j - 1].x - 10 * scans_row_data_vec[i][j].x + scans_row_data_vec[i][j + 1].x + scans_row_data_vec[i][j + 2].x + scans_row_data_vec[i][j + 3].x + scans_row_data_vec[i][j + 4].x + scans_row_data_vec[i][j + 5].x;

  typename CloudType::ValueType diff_y = scans_row_data_vec[i][j - 5].y + scans_row_data_vec[i][j - 4].y + scans_row_data_vec[i][j - 3].y + scans_row_data_vec[i][j - 2].y + scans_row_data_vec[i][j - 1].y - 10 * scans_row_data_vec[i][j].y + scans_row_data_vec[i][j + 1].y + scans_row_data_vec[i][j + 2].y + scans_row_data_vec[i][j + 3].y + scans_row_data_vec[i][j + 4].y + scans_row_data_vec[i][j + 5].y;

  typename CloudType::ValueType diff_z = scans_row_data_vec[i][j - 5].z + scans_row_data_vec[i][j - 4].z + scans_row_data_vec[i][j - 3].z + scans_row_data_vec[i][j - 2].z + scans_row_data_vec[i][j - 1].z - 10 * scans_row_data_vec[i][j].z + scans_row_data_vec[i][j + 1].z + scans_row_data_vec[i][j + 2].z + scans_row_data_vec[i][j + 3].z + scans_row_data_vec[i][j + 4].z + scans_row_data_vec[i][j + 5].z;
   
  scans_row_curv_vec[i][j] = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z;
 }   
}

 

 

每个点的曲率放在容器里，供后续使用。

3.3 根据曲率来筛选特征点

在步骤3.2中已经将每个点的曲率保存在了容器scans_row_curv_vec中了。这时候只需要遍历所有的点，并对每一个点的曲率根据阈值进行判断即可。

 

 

for ( size_t i = Config::Row_Index_Start; i < Config::N_SCANS - Config::Row_Index_End; i ++ ) {
 int j_start_index = 0;
 for ( size_t j = 0; j < scans_row_data_vec[i].size(); j ++ ) {
  if ( j >= j_start_index ) {
   // plane feature
   if ( scans_row_curv_vec[i][j] < 0.05 ) {
    output_cloud1_refer->points.push_back( input_cloud.points[ point_index_vec[i][j] ] ); 
   }
   // corner feature
   else if( scans_row_curv_vec[i][j] > 2.0 ) {
    output_cloud2_refer->points.push_back( input_cloud.points[ point_index_vec[i][j] ] );
   }

   //j_start_index = j + window_interval;
   j_start_index = j + 10;
  }
 }
}

 

 

注意这里为了降低特征点的数量，每隔若干点才判断一下，即语句j_start_index = j + 10的作用。

另外在loam系列方法中根据曲率判断特征点的方法比这里复杂一些，它要对点的曲率进行排序，选取曲率最高的若干个点作为角点，曲率最低的若干个点作为平面点。我这里简化了操作。有兴趣的小伙伴可以随便修改试试。

3.4 分别将平面点和角点放在两个点云数据结构里

将从点云中提出出来的平面点和角点分别放在之前定义好的两个点云数据结构里，供后续使用。

注意我在程序编写时使用了可变参数模板作为形参，同时为了支持多态使用了CRTP机制，增加了复杂性，特别是在递归解析可变参数的时候，使用了std::any来存储参数类型，这样写不一定是最好的，有更好方法的小伙伴们可以给出新的建议。

4. 测试

a. 源点云

图1. 原始点云

b. 提取出的Corner Point 点云

图2. Corner Point点云

c. 提取出的Plane Point点云

图3. Plane Point点云

编辑：黄飞