简介
A-LOAM 的详细注释代码见 A Loam With Chinese Comments
整体思路
scanRegistration 节点的作用包括：
- 接收原始点云，去除无效点以及跟雷达距离过近的点
- 根据激光雷达的性质，计算每个点所属的扫描线以及相对于当前帧点云起始点的时间
- 提取角点和平面点
- 将预处理之后的点云以及提取出来的角点和平面点点云发布，供其余节点使用
以上操作都是在接收点云的回调函数上执行，每接收一次点云对该点云进行一次处理。
点云过滤
去除点云中无效点和距离过近点的操作比较简单，这里不再赘述。
// 对点云进行预处理，去除掉不合要求的点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
计算各点的扫描线以及相对时间
这里需要注意一下，代码中计算激光点扫描线是基于以下假设的：
- 使用的雷达是 velydone 16, 32, 64 线的型号，使用了对应的竖直 FoV 以及分辨率
- 这里通过雷达的水平方向来判断是否经过一圈从而判断是不是进入新的扫描线，这个假设也是基于水平 FoV 360° 的激光雷达，对于水平 FoV 不是完整 360° 的激光雷达这里的逻辑也不能适用
- 在计算相对起始点时间差时，假设雷达每条线同时进行扫描，对于 velodyne 的多线雷达是这样的，但是对于一些固态激光雷达这个假设可能并不成立
另外，a loam 发布的时间还比较早，目前主流使用的激光雷达基本上发布点云的时候对于每个点所属的扫描线以及时间戳都可以直接包含在内，所以这一步可能并不需要。
计算扫描线的流程如下：
通过起始点和结束点的 x, y 确认该帧点云起始点和结束点激光束的方向，理论上大约相差 360° 左右。（这里假定了激光雷达水平视域为 360°）在某些情况下（结束点相对于起始点多转了一点/少转了一点）时需要额外处理：
// 计算起始点和结束点的朝向
// 通常激光雷达扫描方向是顺时针，这里在取 atan 的基础上先取反，这样起始点理论上为 -pi，结束点为 pi，更符合直观
// 理论上起始点和结束点的差值应该是 0，为了显示两者区别，将结束点的方向补偿 2pi
// 表示结束点和起始点实际上逆时针经过一圈
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI;
// 处理几种个别情况，以保持结束点的朝向和起始点的方向差始终在 [pi, 3pi] 之间 （实际上是 2pi 附近）
if (endOri - startOri > 3 * M_PI)
{
// case 1: 起始点在 -179°，结束点在 179°，补偿 2pi 后相差超过一圈，实际上是结束点相对于起始点还没转完整一圈
// 因此将结束点的 2pi 补偿去掉为 179°，与起始点相差 358°，表示结束点和起始点差别是一圈少2°
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
// case 2: 起始点为 179°，结束点为 -179°，补偿后结束点为 181°，此时不能反映真实差别，需要
// 对结束点再补偿上 2pi，表示经过了一圈多 2°
endOri += 2 * M_PI;
}
接下来对点云进行遍历，对其中每个点进行扫描线的计算，计算方法是计算出激光点的俯仰角，并通过激光雷达的垂直视域（FoV）以及分辨率计算得出，这里作者参考的激光雷达是 velodyne 的 16、32、64 线系列，数据手册在参考部分。
// 计算激光点的俯仰角
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
if (N_SCANS == 16)
{
// velodyne 激光雷达的竖直 FoV 是[-15, -15]，分辨率是 2°，这里通过这样的计算可以
// 对改激光点分配一个 [0, 15] 的扫描线 ID
scanID = int((angle + 15) / 2 + 0.5);
// 如果点的距离值不准有可能会计算出不在范围内的 ID 此时不考虑这个点
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
// 思路和 16 线的情况一致
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 64)
{
// 和 16 线的情况一致
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
// 不考虑扫描线 id 在50以上的点
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
接下来计算激光点相对于起始点的时间差，通过计算该点方向和水平点的方向差即可，同样这里假设了激光雷达的视域是完整 360°，同时作者也进行一些 corner cases 的处理，如下所示：
// 计算激光点水平方向角，通过取反操作可以讲雷达扫描方向为逆时针（-pi 到 pi）
float ori = -atan2(point.y, point.x);
if (!halfPassed)
{
// 对一些 Corner case 处理
if (ori < startOri - M_PI / 2)
{
// case 1：起始点在 179 °，逆时针转过几度后，当前点是 -179°，需要加上 2pi 作为补偿
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
// case 2: 理论上在逆时针转的时候不会出现这种情况，在顺时针的情况下，起始点在 -179°，
// 顺时针转过两度后到 179°，此时差距过大，需要减去 2pi
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
// 角度校正后如果和起始点相差超过 pi，表示已经过半圈
halfPassed = true;
}
}
else
{
// 经过半圈后，部分情况（扫描线从 179°到 -179°时）需要加 2pi，
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
// case 1: 在逆时针下理论上不会出现
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
// case 2: 起始点在 -179°，逆时针经过半圈后当前点在 1°，
// 此时差值是对的，因此将2pi补偿减去
ori -= 2 * M_PI;
}
}
// 估计当前当前点和起始点的时间差
float relTime = (ori - startOri) / (endOri - startOri);
最后将每个点的扫描线以及相对时间存在点的 intensity 这个变量中，因为 aloam 并没有使用这个变量。这里作者为了保证相对时间只占用小数部份（相对时间最大可以是 1.0），所以乘上了一个比例因子 scanPeriod = 0.1
point.intensity = scanID + scanPeriod * relTime;
特征点提取
特征点提取过程的代码流程比较清晰，这里大概整理一下基本思路和逻辑。
首先计算每个点的曲率，曲率表示方法为当前点和其周围 10 个点（左右各 5 个）的距离平方和。这里对应论文里面的 c 值。接下来对每条扫描线将其分为 6 个部分，每个部分进行单独的特征点提取过程，不同部分之间互不干扰。这里对应了论文中将点云按子区域划分的部分。接下来分别进行角点和平面点的提取。
角点提取
角点提取顺序和论文基本一致：
- 在每个区域中按曲率排序激光点
- 从曲率最大的点中开始选择角点，代码中将角点分成了质量较好的角点点云以及相对一般的候选点点云
- 如果选择的点曲率没有超过给定阈值，则不考虑
- 如果已选角点小于 2，则同时加入到两个角点点云中，会主动和上一帧角点特征进行匹配
- 如果已选角点大于 2 但小于 20，则只加入到质量一般的候选角点中，用于和下一帧的较好角点进行匹配
- 如果角点已经 20 个以上，则不继续添加角点
对于每一个被选择的角点，其周围一定数量内（作用相邻 5 个）并且距离比较近的点也不做考虑，这里也是基于论文中的过滤操作，为了使特征点分布尽量均匀。
值得注意的是，论文中还有另外两类点也不要选择，分别是点云排列方向和激光束方向基本平行，以及边缘被遮挡的点。a loam 里面并没有进行过滤。
平面点提取
平面点提取过程大同小异：
- 在每个区域中按曲率排序激光点
- 从曲率最大的点中开始选择平面点，代码中同样将平面点分为了质量较好的和质量一般的平面点两部分，和上面不同的是，这里代码中将所有不属于质量较好的平面点以及角点的点都算做质量一般的平面点，因此实际上预处理后的点云要么被划分成角点，要么被当做平面点。这在城市这类结构化建筑物比较多的地方是比较合理，如果在一些曲面比较多密集的区域可能不能成立。
- 如果选择点的曲率没有小于给定阈值则不处理
- 将选择的点标记为质量较好的平面点
- 如果已经选择了 4 个平面点，则后续不再处理
- 同时对每个已经标记的平面点周围进行标记，使其周围的点不会被考虑为平面点
- 最后将所有没被标记为角点的点都加入到质量一般的平面点点云中，由于这部分点云会比较大，因此使用体素滤波下采样处理
发布预处理后的点云以及特征点
这里比较简单，所以不再赘述。