A-LOAM简介
最近在学习激光slam，站在前辈的肩膀，花了一周时间看了一遍代码; 现在尝试着能不能把框架讲清楚，如果有错误的地方，欢迎大佬指正！
aloam将主要分为高频率 低精度的激光里程计估计算法，和 低频率高精度的 借助激光里程计作为初始估计的 将当前帧到地图的优化算法。本质就是构建点到线，点到面的残差来估计机器人的位姿状态。
aloam框架主要是
- kittiHelper.cpp 将kitti数据集转为bag
- scanRegistration.cpp 处理bag中的/velodyne_points 点云话题
- laserOdometry.cpp 激光里程计估计
- laserMapping 将当前帧到地图的优化算法
- lidarFactor.hpp 使用ceres残差优化的结构体定义
scanRegistration.cpp
scanRegistration.cpp的任务是特征提取，为后面帧间匹配做准备，提取出角点和平面点：
- 输入：订阅bag中的velodyne_points
- 输出：处理后的 所有点云 角点特征 弱焦点特征 平面特征 弱平面特征 界外点
一：先看main函数
int main(int argc, char **argv)
{
//节点 名称:scanRegistration
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;//ros 句柄
//从配置参数中 读取 scan_line 参数, 多少线的激光雷达 在launch文件中配置的
nh.param<int>("scan_line", N_SCANS, 16);
//从配置参数中 读取 minimum_range 参数, 最小有效距离 在launch文件中配置的 踢出雷达上的载体出现在视野里的影响
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1); // 0.1只是默认值，launch中是0.3
printf("scan line number %d \n", N_SCANS);
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
//订阅 velodyne 的 lidar消息 收到一个消息包 则进入 laserCloudHandler 回调函数一次
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
// 定义：发布处理后的 所有点云 角点特征 弱焦点特征 平面特征 弱平面特征 界外点
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
// 发布每条线束 默认不发送
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();
return 0;
}main函数比较简单就是订阅订阅bag中的velodyne_points话题，和定义一些需要发布的信息；下面重点是看订阅velodyne_points话题后，回调函数laserCloudHandler 如何处理订阅的点云消息。
二：回调函数：laserCloudHandler
1. 首先判断是否初始化，systemDelay可以自己设置，用来丢掉前几帧：
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
if(!systemInited)
{
systemInitCount++;
if(systemInitCount >=systemDelay)
{
systemInited = true;
}
else
return;
}2. 下一步将接收到的ros中的点云信息laserCloudMsg做简单的处理：
TicToc t_prepare; // 预处理时间
TicToc t_whole; // 总时间
// 后面将按scan线束遍历，下面vector保存遍历index
std::vector<int> scanStartInd(N_SCANS,0);
std::vector<int> scanEndInd(N_SCANS,0);
// 将订阅得到的ros点云 转为pcl格式的点云
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg,laserCloudIn);
std::vector<int> indices;
// 去除NaN值
pcl::removeNaNFromPointCloud(laserCloudIn,laserCloudIn,indices);
// 去除太近的点
removeClosedPointCloud(laserCloudIn,laserCloudIn,MINIMUM_RANGE);其中removeClosedPointCloud函数的代码风格是仿照pcl中removeNaNFromPointCloud的，用来去除离激光雷达很近的点（这样的点很可能是机器人自身的，没有匹配意义）。
3. 然后就是根据激光雷达工作特性求出每个点的方向角，和点所在的线束（根据俯仰角度）
// 求水平方向的起始角度 结束角度 负号是为了将雷达旋转方向与atan计算方向保持一致
int cloudSize = laserCloudIn.points.size();
/**
* @brief atan2添加负号。因为atan2[-pi,pi]按逆时针坐标系计算，激光雷达顺时针扫描
* atan2得到的角度是[-pi, pi], 所以， 如果都用标准化的角度， 那么endOri可能小于或者接近startOri， 这不利于后续的运动补偿
* 因为运动补偿需要从每个点的水平角度确定其在一帧中的相对时间
* 结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描，正常情况下在这个范围内：π< endOri - startOri < 3π
*
* atan
* y pi/2
* ^
* |
* pi |
* -------------------->x0(-0)
* -pi |
* |
* -pi/2
* +++++++++++++++++++++++++++
* +++++++++++++++++++++++++++
* -atan
* y -pi/2
* ^
* |
* -pi |
* -------------------->x0(-0)
* pi |
* |
* pi/2
*/
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//atan2的范围是 [-Pi,Pi] ,这里加上2Pi是为了保证起始到结束相差2PI,符合实际
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;
}
bool halfPassed = false; // 判断旋转是否过半
int count = cloudSize; // 记录求scanID后的有效点的数量
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
// 求点在哪条激光线束 并按线束保存到laserCloudScans中
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;
// 判断输入的激光雷达的线束 只支持 16 32 64
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);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
std::cout << "wrong scan number" << std::endl;
ROS_BREAK();
}
/**
* 0 < ori - startOri < M_PI --> halfPassed =false 此时if语句中要做的是确保 -pi/2 < ori - startOri < 3*pi/2
* ori - startOri > M_PI --> halfPassed =true 此时else语句中要做的是确保 -3*pi/2 < ori - endOri < pi/2 此时ori - endOri本应属于[-pi,0]
*/
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);
}// 求点线束for结束4. 按线束增加方向重新组织点云 并且保存好遍历每一束点云的起始 和结束的index（每一束去除前后五个点）
cloudSize = count;
std::cout << "寻找scanID后的点云个数: " << cloudSize << std::endl;
// TODO 按线束增加方向重新组织点云 并且保存好遍历每一束点云的起始 和结束的index（每一束去除前后五个点） 这段代码组织的很优雅
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;
}
std::cout << "prepare time: " << t_prepare.toc() << std::endl;5. 前面都是预热赛，做一些准备工作 下面开始提取初始特征点了
/* 前面都是预热赛，做一些准备工作 下面开始提取初始特征点了 */
// 得到点的曲率和初始化操作
for(int i = 5; i < cloudSize; ++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 *diffX + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
// 分类点云起始时间
TicToc t_pts;
// 用来存放四种特征点云
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;6.下面的代码为了提取角点特征 和 平面特征嵌套三个for循环。分清每个for循环的功能就不会读到一半不知道自己在哪个for循环里了；第一个大的for循环 为了遍历线束（支持16 32 64 线），第二个中的for循环 为了将每个线束均分为6份，为了均匀提取特征；第三：就是两个平行的小的for循环就是筛选 角点 和 平面点
float t_q_sort = 0;
// 大的for循环 遍历线束
for(int i = 0; i <N_SCANS; ++i)
{
if(scanEndInd[i] - scanStartInd[i] < 6)
continue;
// TODO 为什么需要这么多的弱平面特征 存放一个线束中的弱平面特征 包含强平面特征 非角点特征都是弱平面特征
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
// 小的for循环 每条线束均分6份 均匀提取特征
for(int j = 0; j < 6; ++j)
{
int sp = scanStartInd[i] + (scanEndInd[i]-scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i]-scanStartInd[i]) * (j+1) / 6 - 1;
// 将每段索引按照索引的曲率值大小重新排序 放入cloudSortInd
TicToc t_tmp;
std::sort(cloudSortInd + sp, cloudSortInd + ep, comp);
t_q_sort += t_tmp.toc();
// 角点 2, 弱角点20
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;
}
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;
}
}
}
// 平面点
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)
{
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]);
}
}
}// 小的for循环 每条线束均分6份 结束
// 弱平面点降采样 测试了一下点数大概减少了一半
// 降采样前弱平面点个数：772
// 降采样后弱平面点个数：352
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2,0.2,0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}// 大的for循环 遍历线束结束7. 最后发布 处理过的全部点云 角点点云 弱角点点云 平面点点云 弱平面特征，为后面的laserOdometry做准备
std::cout << "sort q time: " << t_q_sort << std::endl;
std::cout << "sepertate points time: " << t_pts.toc() << std::endl;
// 发布 处理过的全部点云 角点点云 弱角点点云 平面点点云 弱平面特征
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);
// 发布每条线束 默认不发布
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);
}
}
std::cout << "scan registration time: " << t_whole.toc() << std::endl;
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");三：总结
因为这是我在看完aloam后，对着源码手敲的一遍，所以可能注释的不是很详细，但是逻辑还挺清晰的，因为scanResgistration代码也不难，但是得知道激光雷达是如何工作的。
在学习aloam中，自己也将aloam整个代码框架注释了一遍，放在了github，参考了
- 博客园用户WellP.C的博客：LOAM笔记及A-LOAM源码阅读
- csdn用户月照银海似蛟龙的多篇博客
2550
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
