A-LOAM代码笔记（一）
A-LOAM完整laserCloudHandler函数解释
函数整体概述
该函数是 A-LOAM（Advanced LOAM）中scanRegistration节点的核心激光点云回调函数，是算法前端预处理的核心模块。它负责接收原始激光点云数据，完成系统初始化判断、点云格式转换、无效点过滤、点云按线束分割、曲率计算、特征点（角点、平面点）提取，最终将处理后的点云和特征点发布为 ROS 话题，供后续的里程计模块使用。
逐部分代码解释
一、系统初始化判断
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
这部分是系统初始化逻辑，用于确保 IMU 数据就绪后再处理激光点云：
-
systemInited：全局标志位，标记系统是否完成初始化。 -
systemInitCount：初始化计数器，统计接收到的点云帧数。 -
systemDelay：预设的延时帧数，作用是等待 IMU（惯性测量单元）数据完成初始化 ——A-LOAM 需要 IMU 数据辅助消除点云的运动畸变，必须保证 IMU 数据先于激光点云处理。 -
如果系统未初始化，计数器累加；当达到预设帧数后，将
systemInited设为true，后续点云会被完整处理；否则直接返回，不处理当前帧点云。
二、计时模块初始化
TicToc t_whole;
TicToc t_prepare;
TicToc是 A-LOAM 自定义的计时工具类，通过构造函数记录起始时间，调用toc()方法可以获取从构造到调用时的耗时（毫秒级）：
-
t_whole：统计整个函数的总耗时。 -
t_prepare：统计点云预处理步骤的耗时。
三、点云容器与格式转换
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
-
scanStartInd和scanEndInd：用于存储每个线束点云在合并后点云中的起始和结束索引，方便后续按线束处理点云。 -
laserCloudIn：PCL 格式的点云对象，用于存储转换后的点云数据。 -
pcl::fromROSMsg：将 ROS 的sensor_msgs::PointCloud2格式的点云消息转换为 PCL 的pcl::PointCloud格式，以便使用 PCL 的算法处理点云。
四、点云过滤
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
-
pcl::removeNaNFromPointCloud：去除点云中的无效点（坐标为 NaN 的点），这些是激光雷达采集到的无效数据；第一个参数是输入点云，第二个是输出点云（此处直接覆盖输入），第三个参数是保留点的索引。 -
removeClosedPointCloud：A-LOAM 自定义的函数，用于去除距离雷达过近的点（距离小于MINIMUM_RANGE的点），这些点通常是雷达自身或近距离障碍物的噪声点，会影响后续的特征提取。 -
MINIMUM_RANGE：预设的最小距离阈值，通常在 launch 文件中配置。
五、点云角度计算与修正
int cloudSize = laserCloudIn.points.size();
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;
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
-
cloudSize：获取过滤后点云的点数量。 -
startOri和endOri：计算点云第一个点和最后一个点的水平方位角；atan2(y, x)计算点在 xy 平面的极角（范围 [-π, π]），加负号是为了适配 A-LOAM 的坐标系，endOri加 2π 是为了将角度范围调整到 [0, 2π]。 -
角度修正：处理点云扫描跨越 0 度（360 度）的情况，确保
endOri - startOri的范围在 π 到 3π 之间，避免后续相对时间计算出错。
六、点云按线束分割
点击查看代码
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++)
{
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
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)
{
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);
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
这部分是将点云按激光雷达的线束进行分割：
-
angle：计算点的俯仰角（单位：度），通过atan(z / sqrt(x²+y²))计算点相对于 xy 平面的角度，转换为度。 -
scanID：根据俯仰角计算点所属的线束 ID，适配 16/32/64 线激光雷达：-
16 线雷达：俯仰角范围是 - 15° 到 15°，每 2° 为一个线束。
-
32 线雷达：根据其俯仰角范围计算对应的线束 ID。
-
64 线雷达：分为上下两部分计算线束 ID，同时过滤掉俯仰角超出范围的点。
-
-
如果计算出的
scanID无效（超出线束范围），则跳过该点，count用于统计有效点的数量。
七、点的角度修正与相对时间计算
float ori = -atan2(point.y, point.x);
if (!halfPassed)
{
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
laserCloudScans[scanID].push_back(point);
这部分是修正点的水平角度，并计算点在扫描帧中的相对时间：
-
ori：计算点的水平方位角，修正为与起始角度一致的坐标系。 -
halfPassed：标记是否扫描过半，用于处理跨越 0 度的情况，确保角度计算连续。 -
relTime：计算点在当前扫描帧中的相对时间（0 到 1 之间），用于后续的运动畸变补偿。 -
point.intensity：这里复用了intensity字段，存储的是scanID + 扫描周期*相对时间，而不是激光的强度信息，后续用于运动畸变补偿时的时间同步。 -
将点添加到对应线束的点云容器
laserCloudScans中。
八、合并点云并记录线束索引
cloudSize = count;
printf("points size %d \n", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
-
cloudSize：更新为有效点的数量。 -
laserCloud：合并所有线束的点云为一个整体点云。 -
scanStartInd和scanEndInd：记录每个线束在合并点云中的起始和结束索引，+5和-6是为了后续曲率计算时，跳过每个线束的前 5 个和后 6 个点（这些点无法计算曲率，因为需要前后各 5 个点）。
九、曲率计算
printf("prepare time %f \n", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++)
{
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
这部分计算每个点的曲率，用于特征点提取：
-
曲率计算方式：使用点的前后各 5 个点的坐标差值之和的平方和来表示曲率，公式为：
curvature = (sum_{k=-5,k≠0}^5 x_{i+k} -10x_i)² + (sum_{k=-5,k≠0}^5 y_{i+k} -10y_i)² + (sum_{k=-5,k≠0}^5 z_{i+k} -10z_i)²这个值越大，说明点的周围变化越大，越可能是角点；值越小，说明点越平滑，越可能是平面点。 -
cloudCurvature：存储每个点的曲率值。 -
cloudSortInd：存储点的原始索引，用于后续按曲率排序时保留原始位置。 -
cloudNeighborPicked：标记点是否被选为特征点的邻居，避免特征点过于集中。 -
cloudLabel：标记点的类型，0 为未标记，2 为 sharp 角点，1 为 less sharp 角点，-1 为 flat 平面点。
十、特征点提取
TicToc t_pts;
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
for (int i = 0; i < N_SCANS; i++)
{
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++)
{
// 将有效范围均匀分成 6 段，sp 和 ep 分别是第 j 段的起始和结束索引。这样每段内的点数大致相等，保证特征点在全场分布均匀。
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
scanStartInd[i]和scanEndInd[i]是第i条扫描线在总点云中的有效范围- 将有效范围均匀分成六份，
sp和ep分别是第j段的起始和结束索引，这样每段内的点数大致相同，保证特征点在全场分布均匀。
TicToc t_tmp;
// 对子段内的点按曲率排序
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); // comp 是比较函数，按曲率从小到大排序。排序后，cloudSortInd[sp] 对应曲率最小的点，cloudSortInd[ep] 对应曲率最大的点。
cloudSortInd是一个索引数组，初始时cloudSortInd[i]=i
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)// 从大到小遍历
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++;
// 按选取顺序分类
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2; // 尖锐角点
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1; // 次尖锐角点
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
break; // 超过20个则停止
}
cloudNeighborPicked[ind] = 1; // 标记为已选
// 将前后各5个距离较近的邻点也标记为已选，避免特征点过于集中
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) // 检查相邻点间距，若太大则停止
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) // 从小到大遍历
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
cloudLabel[ind] = -1; // 平面点
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4)// 每段最多选4个
{
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
这部分是提取特征点，将点分为角点和平面点：
-
每个线束被分为 6 个扇区，每个扇区独立处理，避免特征点集中。
-
角点提取：
2.1 按曲率从大到小排序，选取曲率最大的前 2 个点作为
cornerPointsSharp（sharp 角点，用于帧间匹配），前 2-20 个点作为cornerPointsLessSharp（less sharp 角点，用于地图匹配）。2.2 标记选中点的前后 5 个点为已选中，避免特征点过于集中。
-
平面点提取：
3.1 按曲率从小到大排序，选取曲率最小的前 4 个点作为
surfPointsFlat（flat 平面点，用于帧间匹配）。3.2 剩余未标记的点作为
surfPointsLessFlat（less flat 平面点，用于地图匹配），并使用体素滤波进行下采样，减少点数量。
// 定义一个空的点云对象，用于存储滤波后的结果
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
// 创建一个体素滤波器对象
pcl::VoxelGrid<PointType> downSizeFilter;
// 设置输入点云：surfPointsLessFlatScan 是当前子段内所有非角点的点（包括平面点和普通点）
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
// 设置体素滤波的叶子尺寸：0.2米 × 0.2米 × 0.2米
// 这意味着将空间划分为边长为0.2米的小立方体（体素），每个体素内只保留一个点（通常取体素内所有点的重心）
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
// 执行滤波，结果存入 surfPointsLessFlatScanDS 中
downSizeFilter.filter(surfPointsLessFlatScanDS);
// 将滤波后的点云追加到总的次平坦点云 surfPointsLessFlat 中
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
体素滤波原因：
-
surfPointsLessFlatScan中的点数量通常很大（因为大多数点都属于品面或者普通区域），如果全部保留，会导致后续的匹配和建图计算量过大 -
通过体素滤波，可以在保持点云整体形状的前提下，大幅减少点的数量（每个0.2立方体内只保留一个点），从而降低计算开销，同时避免特征过于密集，使优化更稳定。
-
最终得到的
surfPointsLessFlat将用于后续的帧间匹配或建图，作为平面约束的输入。
十一、话题发布
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// pub each scam
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++)
{
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "/camera_init";
pubEachScan[i].publish(scanMsg);
}
}
这部分将处理后的点云和特征点发布为 ROS 话题：
-
laserCloudOutMsg：发布合并后的完整点云。 -
cornerPointsSharpMsg：发布 sharp 角点。 -
cornerPointsLessSharpMsg：发布 less sharp 角点。 -
surfPointsFlat2：发布 flat 平面点。 -
surfPointsLessFlat2：发布 less flat 平面点。 -
如果
PUB_EACH_LINE为 true，还会发布每个线束的点云，用于调试。
十二、性能统计
printf("scan registration time %f ms *************\n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");
输出整个函数的总耗时，如果耗时超过 100ms，会发出 ROS 警告，提示处理时间过长，方便调试和性能优化。
相关背景补充
A-LOAM 是经典激光 SLAM 算法 LOAM 的改进版本，scanRegistration节点是其前端处理模块，负责将原始激光点云转化为 SLAM 系统可用的特征点，为后续的帧间里程计和地图构建提供基础数据。该函数的处理流程是激光 SLAM 前端特征提取的典型实现，通过曲率计算和扇区分割的方式，保证了特征点的均匀性和鲁棒性。
浙公网安备 33010602011771号