作者 | 密斯特李 编辑 | 汽车人
原文链接：https://zhuanlan.zhihu.com/p/515732721
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心技术交流群
后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！
上期文章介绍了LeGO-LOAM，它围绕在轻量级地面无人平台上的应用针对性的进行了特征提取、位姿估计等方面的优化，并在系统中加入闭环检测模块搭建了一套完整的SLAM解决方案。近年来，随着传感器技术的跨越式发展，LiDAR传感器也在极速更迭，更轻便、更廉价、对具体应用针对性更强的激光雷达产品层出不穷，其中最具代表性的就是Dji子公司Livox推出的一系列固态激光雷达产品。无论是F-LOAM还是LeGO-LOAM，都是针对于混合固态激光雷达的应用，混合固态本质上仍是一列通过激光束的水平旋转实现的三维空间扫描，只不过不再是激光发射器的机械旋转，而是利用反射棱镜的旋转。固态激光雷达采用完全不同的扫描模式，LOAM及其各种衍生方法均无法直接套用于固态激光雷达上。
本文介绍的LOAM-Livox是LOAM针对Livox固态激光雷达的应用。该工作Loam_livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV发表于2020年ICRA会议，代码源生于A-LOAM，开源于hku-mars/loam_livox。
文章首先对Livox固态激光雷达的扫描特性进行了介绍，并与以Velodyne VLP-16为代表的混合固态激光雷达进行了对比，并深入分析了LOAM应用于固态激光雷达的难点。针对这些难点问题，作者有针对性地对LOAM的整体架构、特征提取和基于特征点配准的雷达里程计模块进行了优化，具体技术细节和代码见后文。
根据论文内容和代码，笔者将这一工作的主要创新点归纳为以下几点：
第一个优化：针对Livox固态激光雷达扫描特性的有效点筛选和特征点提取，从原始点云中获取线、面特征点。
第二个优化：迭代的位姿优化策略，在迭代过程中不断对畸变的点云进行校正。
最大亮点：LOAM的Livox实现。尤其是对Livox点云的数据预处理部分，对后续其他基于固态激光雷达的SLAM解决方案具有十分重要的启发和借鉴意义。
从学术的角度讲，这篇文章的写作结构十分值得借鉴，能够很好的展现作者解决问题的思路，故事讲的十分圆满。当我们拿到一个新的传感器，首先就是要明确这款传感器的特性，分析它和之前的到底有什么不同；而后，结合这些差异提出针对性的解决办法；最后对改进系统进行全面的测试验证，评估其表现。
Loam-livox的系统架构和ROS的节点关系如下图所示，可以清晰地看出各个模块间的关系。
如下图所示，由于固态激光雷达的FoV有限（scan中能够提取的特征点十分稀疏），且扫描模式为非重复扫描(同一目标点在连续scan中并不一定被扫到)，scan-to-scan的匹配无法实现。因此，LOAM_Livox舍弃了LOAM的laserOdometry线程，将系统整体划分为两个部分，对应两个ROS节点，在两个不同进程中运行。整体架构就变得非常简洁，即由livox_scanRegistration进行特征点的提取和有效点的筛选，由livox_laserMapping实现scan-to-map的匹配。各节点的具体功能如下：
livox_scanRegistration节点负责根据扫描点是否在FoV边缘、点的强度反射率、入射角以及是否属于受障碍物遮挡而产生的边沿等因素从原始点云筛选有效点云，并从中提取线和面特征，高效获取scan中最具代表性的点，同时对点云降维。
livox_laserMapping节点主要负责利用和提取的线面特征点，结合系下的局部点云地图，由scan-to-map的特征点云匹配估计当前LiDAR位姿 ；由于LiDAR时刻在运动，scan存在运动畸变，为提升匹配精度，在匹配迭代过程中不断进行畸变消除处理。
A. 雷达扫描特性分析
固态激光雷达与混合固态激光雷达的差异见下图，主要包含四个方面：
一是小范围的视域。固态雷达通过内部MEMS棱镜的旋转实现三维空间扫描。受制于MEMS的尺寸，棱镜转角不能很大，因此FoV较小(见上图)。狭小的视域范围导致单帧扫描点云中的特征点较少，从而使得基于点云特征的匹配十分困难，且极易受到动态目标的干扰。
二是不规则的扫描。对于混合固态激光雷达而言，扫描点云是规则阵列排布的，即能够直接索引某一点在上下左右方向相邻的点。而固态激光雷达是不规则排布的，扫描点云呈花瓣状(如图3)，相邻点间的空间邻域关系并不明确。
三是不重复的扫描。混合固态激光雷达的点云扫描轨迹是固定的，当LiDAR处于静态时，新的扫描点会重复地打在之前的扫描点上(不能完全重合是由于随机测量噪声导致的)。而固态激光雷达为了最大化覆盖比例，采用非重复扫描的策略。如上图3所示，点的扫描轨迹随时间并不重复。
四是点云运动畸变。两种雷达都同样面临点云运动畸变问题，但对于非重复、不规则扫描的固态激光雷达影响更大，不容忽视。
综上所述，需要结合以上固态激光雷达特性，研究三维点云中的特征点提取方法。
B. 有效点筛选
根据固态激光雷达的激光光斑大小、激光信噪比和扫描的机械特性，从原始点云中筛选质量优质的点作为有效点。这部分的代码实现在livox_feature_extractor.hpp的Livox_laser类的extract_laser_features()函数中：
template < typename T >
std::vector< pcl::PointCloud< pcl::PointXYZI > > extract_laser_features( pcl::PointCloud< T > &laserCloudIn, double time_stamp = -1 )
{
...
...
// 计算三维点的水平角和垂直角的正切值、深度、水平深度等，并对噪声点进行剔除
int clutter_size = projection_scan_3d_2d( laserCloudIn, scan_id_index );
// 计算曲率和入射角，提取特征点
compute_features();
if ( clutter_size == 0 )
{
return laserCloudScans;
}
else
{
split_laser_scan( clutter_size, laserCloudIn, scan_id_index, laserCloudScans );
return laserCloudScans;
}
}主要原则包括以下几点：
代码实现部分：
template < typename T >
int projection_scan_3d_2d( pcl::PointCloud< T > &laserCloudIn, std::vector< float > &scan_id_index )
{
unsigned int pts_size = laserCloudIn.size();
m_pts_info_vec.clear();
m_pts_info_vec.resize( pts_size );
m_raw_pts_vec.resize( pts_size );
std::vector< int > edge_idx;
std::vector< int > split_idx;
scan_id_index.resize( pts_size );
m_map_pt_idx.clear();
m_map_pt_idx.reserve( pts_size );
std::vector< int > zero_idx;
m_input_points_size = 0;
for ( unsigned int idx = 0; idx < pts_size; idx++ )
{
m_raw_pts_vec[ idx ] = laserCloudIn.points[ idx ];
Pt_infos *pt_info = &m_pts_info_vec[ idx ];
m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) );
pt_info->raw_intensity = laserCloudIn.points[ idx ].intensity; // 强度信息
pt_info->idx = idx; // 点序号
pt_info->time_stamp = m_current_time + ( ( float ) idx ) * m_time_internal_pts; // 点的时间戳
m_last_maximum_time_stamp = pt_info->time_stamp;
m_input_points_size++;
if ( !std::isfinite( laserCloudIn.points[ idx ].x ) ||
!std::isfinite( laserCloudIn.points[ idx ].y ) ||
!std::isfinite( laserCloudIn.points[ idx ].z ) )
{
add_mask_of_point( pt_info, e_pt_nan ); // 剔除NaN的点
continue;
}
if ( laserCloudIn.points[ idx ].x == 0 ) // 剔除x为0的点
{
if ( idx == 0 )
{
// TODO: handle this case.
screen_out << "First point should be normal!!!" << std::endl;
pt_info->pt_2d_img << 0.01, 0.01;
pt_info->polar_dis_sq2 = 0.0001;
add_mask_of_point( pt_info, e_pt_000 );
//return 0;
}
else
{
pt_info->pt_2d_img = m_pts_info_vec[ idx - 1 ].pt_2d_img;
pt_info->polar_dis_sq2 = m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
add_mask_of_point( pt_info, e_pt_000 );
continue;
}
}
m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) );
pt_info->depth_sq2 = depth2_xyz( laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].y, laserCloudIn.points[ idx ].z );
pt_info->pt_2d_img << laserCloudIn.points[ idx ].y / laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].z / laserCloudIn.points[ idx ].x;
pt_info->polar_dis_sq2 = dis2_xy( pt_info->pt_2d_img( 0 ), pt_info->pt_2d_img( 1 ) );
eval_point( pt_info ); // 剔除距离过近和强度过小的点
if ( pt_info->polar_dis_sq2 > m_max_edge_polar_pos ) // 剔除靠近FoV边沿的点
{
add_mask_of_point( pt_info, e_pt_circle_edge, 2 );
}
// Split scans
if ( idx >= 1 )
{
float dis_incre = pt_info->polar_dis_sq2 - m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
if ( dis_incre > 0 ) // far away from zero
{
pt_info->polar_direction = 1;
}
if ( dis_incre < 0 ) // move toward zero
{
pt_info->polar_direction = -1;
}
if ( pt_info->polar_direction == -1 && m_pts_info_vec[ idx - 1 ].polar_direction == 1 )
{
if ( edge_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
{
split_idx.push_back( idx );
edge_idx.push_back( idx );
continue;
}
}
if ( pt_info->polar_direction == 1 && m_pts_info_vec[ idx - 1 ].polar_direction == -1 )
{
if ( zero_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
{
split_idx.push_back( idx );
zero_idx.push_back( idx );
continue;
}
}
}
}
split_idx.push_back( pts_size - 1 );
int val_index = 0;
int pt_angle_index = 0;
float scan_angle = 0;
int internal_size = 0;
if( split_idx.size() < 6) // minimum 3 petal of scan.
return 0;
for ( int idx = 0; idx < ( int ) pts_size; idx++ )
{
if ( val_index < split_idx.size() - 2 )
{
if ( idx == 0 || idx > split_idx[ val_index + 1 ] )
{
if ( idx > split_idx[ val_index + 1 ] )
{
val_index++;
}
internal_size = split_idx[ val_index + 1 ] - split_idx[ val_index ];
if ( m_pts_info_vec[ split_idx[ val_index + 1 ] ].polar_dis_sq2 > 10000 )
{
pt_angle_index = split_idx[ val_index + 1 ] - ( int ) ( internal_size * 0.20 );
scan_angle = atan2( m_pts_info_vec[ pt_angle_index ].pt_2d_img( 1 ), m_pts_info_vec[ pt_angle_index ].pt_2d_img( 0 ) ) * 57.3;
scan_angle = scan_angle + 180.0;
}
else
{
pt_angle_index = split_idx[ val_index + 1 ] - ( int ) ( internal_size * 0.80 );
scan_angle = atan2( m_pts_info_vec[ pt_angle_index ].pt_2d_img( 1 ), m_pts_info_vec[ pt_angle_index ].pt_2d_img( 0 ) ) * 57.3;
scan_angle = scan_angle + 180.0;
}
}
}
m_pts_info_vec[ idx ].polar_angle = scan_angle;
scan_id_index[ idx ] = scan_angle;
}
return split_idx.size() - 1;
}C. 特征点提取
LOAM_Livox中沿用LOAM的线、面特征提取策略，即根据各点的几何平滑度提取特征点。主要改进在于线特征的提取增加了强度信息的考虑，即强度平滑度大的点同样作为线特征进行后续的匹配。代码部分与LOAM基本一致，这里不再展示。
A. 线、面特征残差构建
构建方式与LOAM相同，即点到直线和点到平面的距离。
B. 点云运动补偿
由于LiDAR是一种非瞬时观测的传感器，所有LO都逃不开运动补偿，尤其对于Livox这类固态激光雷达，更是不容忽视。运动补偿的思路十分简单，就是假设在当前scan扫描过程中，LiDAR为匀速直线运动，于是就可以根据各个点精确的时间戳和scan扫描过程中LiDAR的相对运动内插得到点的精确位置，这与LOAM中所用方法也是相同的。在LOAM_Livox中，采用了一种工程化更好的解决方案，即采用并行的处理方式。具体而言，将当前scan再分为3个piecewise，对各个piecewise并行处理，如下图所示：
具体代码实现于split_laser_scan()函数中，将点云分为若干piecewise作为节点间传递的msg：
// Split whole point cloud into scans.
template < typename T >
void split_laser_scan( const int clutter_size, const pcl::PointCloud< T > &laserCloudIn,
const std::vector< float > & scan_id_index,
std::vector< pcl::PointCloud< PointType > > &laserCloudScans )
{
std::vector< std::vector< int > > pts_mask;
laserCloudScans.resize( clutter_size );
pts_mask.resize( clutter_size );
PointType point;
int scan_idx = 0;
for ( unsigned int i = 0; i < laserCloudIn.size(); i++ )
{
point = laserCloudIn.points[ i ];
if ( i > 0 && ( ( scan_id_index[ i ] ) != ( scan_id_index[ i - 1 ] ) ) )
{
scan_idx = scan_idx + 1;
pts_mask[ scan_idx ].reserve( 5000 );
}
laserCloudScans[ scan_idx ].push_back( point );
pts_mask[ scan_idx ].push_back( m_pts_info_vec[ i ].pt_type );
}
laserCloudScans.resize(scan_idx);
int remove_point_pt_type = e_pt_000 |
e_pt_too_near |
e_pt_nan
// e_pt_circle_edge
;
int scan_avail_num = 0;
std::vector< pcl::PointCloud< PointType > > res_laser_cloud_scan;
for ( unsigned int i = 0; i < laserCloudScans.size(); i++ )
{
scan_avail_num = 0;
pcl::PointCloud< PointType > laser_clour_per_scan;
for ( unsigned int idx = 0; idx < laserCloudScans[ i ].size(); idx++ )
{
if ( ( pts_mask[ i ][ idx ] & remove_point_pt_type ) == 0 )
{
if ( laserCloudScans[ i ].points[ idx ].x == 0 )
{
screen_printf( "Error!!! Mask = %d\r\n", pts_mask[ i ][ idx ] );
assert( laserCloudScans[ i ].points[ idx ].x != 0 );
continue;
}
auto temp_pt = laserCloudScans[ i ].points[ idx ];
set_intensity( temp_pt, default_return_intensity_type );
laser_clour_per_scan.points.push_back(temp_pt);
scan_avail_num++;
}
}
//printf(" %d|%d number of point in this scan = %d ------------------\r\n ", i, laserCloudScans.size(), scan_avail_num);
if(scan_avail_num)
{
res_laser_cloud_scan.push_back(laser_clour_per_scan);
}
}
laserCloudScans= res_laser_cloud_scan;
}位姿估计部分的代码与A-LOAM类似，同样采用ceres构建非线性优化问题实现，代码不再重复展示。
采用自测数据集对系统性能进行了评估，包括定性的建图结果对比、定量的定位定姿精度以及系统运行效率。通过建图结果对比验证了在手持数据集上piecewise运动畸变消除更为有效。定位定姿测试在室外环境下进行，两组数据集下定位偏差与轨迹距离比分别为0.41%和0.65%，具体结果请参考原文。
实验证实了LOAM-Livox的有效性，能够实现较好的定位和地图构建结果，是固态激光雷达Livox应用于LOAM的一个非常优秀的工作。
往期回顾
港大重磅SLAM新作！R3LIVE++：一个实时鲁棒的紧耦合激光雷达-惯性-视觉融合框架
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、多传感器融合、SLAM、光流估计、轨迹预测、高精地图、规划控制、AI模型部署落地等方向；
加入我们：自动驾驶之心技术交流群汇总！
自动驾驶之心【知识星球】
想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！
