无人驾驶学习笔记 - A-LOAM 算法代码解析总结
目录
1、概述
2、scanRegistration.cpp
2.1、代码注释
2.1.1、主函数
2.1.2、removeClosedPointCloud（雷达周边过近点移除）
2.1.3 laserCloudHandler激光处理回调函数
2.2、总结概括
3、LaserOdometry.cpp
3.1、代码注释
3.2、总结概括
4、laserMapping.cpp
4.1、代码注释
4.2、总结概括
5、lidarFactor.cpp
6、参考连接
PS: 在看了loam的论文和代码以后，紧接着看了A-LOAM，核心算法没有太大变换，应用了ceres 替代了手推高斯推导，LOAM代码中大量的基于欧拉角转来转去的推导实在容易劝退众生。A-LOAM相对整体结构也更清楚，避免遗忘，将代码的学习总结于此。
该总结包括代码和文字总共有6万字左右，建议结合目录阅读，首先要熟悉loam Aloam 论文中核心的方法，然后可以先阅读每一部分的总结概括，有了认知以后再去每个节点的详细注释中学习
1、概述
A-LOAM 的运行节点关系图如下
关于 A-LOAM 算法原理可以参考： 无人驾驶学习笔记 - LOAM 算法论文核心关键点总结_ppipp1109的博客-CSDN博客
本文总结主要从扫描匹配、低精度里程计、高精度建图三部分对应的函数讲解
2、scanRegistration.cpp
scanRegistration.cpp用来读取激光雷达数据并且对激光雷达数据进行整理，去除无效点云以及激光雷达周围的点云，并且计算每一个点的曲率，按照提前设定好的曲率范围来分离出不同类型的点。
2.1、代码注释
2.1.1、主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;
// 从配置文件中获取多少线的激光雷达
nh.param("scan_line", N_SCANS, 16);
// 最小有效距离
nh.param("minimum_range", MINIMUM_RANGE, 0.1);
printf("scan line number %d \n", N_SCANS);
// 只有线束是16 32 64的才可以继续
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, laserCloudHandler);
pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100);
pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100);
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100);
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100);
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100);
pubRemovePoints = nh.advertise("/laser_remove_points", 100);
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();
return 0;
} 2.1.2、removeClosedPointCloud（雷达周边过近点移除）
void removeClosedPointCloud(const pcl::PointCloud &cloud_in,
pcl::PointCloud &cloud_out, float thres)
{
// 假如输入输出点云不使用同一个变量，则需要将输出点云的时间戳和容器大小与输入点云同步
if (&cloud_in != &cloud_out)
{
cloud_out.header = cloud_in.header;
cloud_out.points.resize(cloud_in.points.size());
}
size_t j = 0;
// 把点云距离小于给定阈值的去除掉,也就是距离雷达过近的点去除
for (size_t i = 0; i < cloud_in.points.size(); ++i)
{
if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
continue;
cloud_out.points[j] = cloud_in.points[i];
j++;
}
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
// 这里是对每条扫描线上的点云进行直通滤波，因此设置点云的高度为1，宽度为数量，稠密点云
cloud_out.height = 1;
cloud_out.width = static_cast(j);
cloud_out.is_dense = true;
} 2.1.3 laserCloudHandler激光处理回调函数
预处理
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 如果系统没有初始化的话，就等几帧
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//作者自己设计的计时类，以构造函数为起始时间，以toc()函数为终止时间，并返回时间间隔(ms)
TicToc t_whole;
TicToc t_prepare;
//每条扫描线上的可以计算曲率的点云点的起始索引和结束索引
//分别用scanStartInd数组和scanEndInd数组记录
std::vector scanStartInd(N_SCANS, 0);
std::vector scanEndInd(N_SCANS, 0);
pcl::PointCloud laserCloudIn;
// 把点云从ros格式转到pcl的格式
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector indices;
// 去除掉点云中的nan点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
// 去除距离小于阈值的点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); 计算点云角度范围，目的是求解点所在的扫描线，并通过论文中方法，对齐时间戳，根据时间占比求出所有点对应的角度范围。其中根据扫描范围在不同象限做了不同处理，考虑了扫描范围在二三象限时候，对角度进行加2pi将其约束到 -pi/2 - 3/2 * pi范围内。类似地如果扫描范围在一四象限，需要将结束点角度也减2pi。详细解释可参考下文的3.2.2节
A_LOAM源码解析1——scanRegistration - 知乎
// 计算起始点和结束点的角度，由于激光雷达是顺时针旋转，这里取反就相当于转成了逆时针
int cloudSize = laserCloudIn.points.size();
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;
// 通过上面的公式，endOri - startOri理应在pi~3pi之间
// 正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正
// 如果出现了异常，不认为是扫描点本身的问题，而是公式先验地+2pi出了问题
// 总有一些例外，比如这里大于3PI，和小于PI，就需要做一些调整到合理范围
//扫描场在右半部分（一、四象限）的情况
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
//扫描场在左半部分（二、三象限）的情况
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
对于每个点云点计算对应的扫描线，主要掌握其计算方法，实际现在的3d激光雷达驱动中都已经集成了每个点云点的线号、时间戳和角度等信息，不需要再单独计算。
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector> 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;
// 计算他的俯仰角
// 计算垂直视场角，决定这个激光点所在的scanID
// 通过计算垂直视场角确定激光点在哪个扫描线上（N_SCANS线激光雷达）
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
// 计算是第几根scan
if (N_SCANS == 16)
{
// +-15°的垂直视场，垂直角度分辨率2°，-15°时的scanID = 0
/*
* 垂直视场角，可以算作每个点的
* 如果是16线激光雷达，结算出的angle应该在-15~15之间
*/
scanID = int((angle + 15) / 2 + 0.5);
// scanID(0~15), 这些是无效的点，cloudSize--
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
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
// 计算水平角
float ori = -atan2(point.y, point.x);
// 根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿
/*
* 如果此时扫描没有过半，则halfPassed为false
*/
if (!halfPassed)
{
// 确保-PI / 2 < ori - startOri < 3 / 2 * PI
//如果ori-startOri小于-0.5pi或大于1.5pi，则调整ori的角度
if (ori < startOri - M_PI / 2)
{
// 在-pi与pi的断点处容易出这种问题
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
// 如果超过180度，就说明过了一半了
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
// 确保-PI * 3 / 2 < ori - endOri < PI / 2
ori += 2 * M_PI; // 先补偿2PI
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
// 角度的计算是为了计算相对的起始时刻的时间
/*
* relTime 是一个0~1之间的小数，代表占用扫描时间的比例，乘以扫描时间得到真实扫描时刻，
* scanPeriod扫描时间默认为0.1s 100hz
*/
float relTime = (ori - startOri) / (endOri - startOri);
// 整数部分是scan的索引，小数部分是相对起始时刻的时间
point.intensity = scanID + scanPeriod * relTime;
// 根据scan的idx送入各自数组
laserCloudScans[scanID].push_back(point);
}
// cloudSize是有效的点云的数目
cloudSize = count;
printf("points size %d \n", cloudSize);
接下来就是对雷达数据的特征提取，和论文中一致，分别提取角点和面点。主要思想是利用点云点的曲率来提取特征点。曲率是指一定范围内拟合出的圆的半径的倒数所以曲率大的为角点，曲率小的为面点。
代码中的方法是，同一条扫描线上取目标点左右则各5个点，求均值。并与之作差。
// 将每条扫描线上的点输入到laserCloud指向的点云，
// 并记录好每条线上可以计算曲率的初始点和结束点的索引（在laserCloud中的索引）
pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
// 全部集合到一个点云里面去，但是使用两个数组标记起始和结束，这里分别+5和-6是为了计算曲率方便
for (int i = 0; i < N_SCANS; i++)
{
// 前5个点和后5个点都无法计算曲率，因为他们不满足左右两侧各有5个点
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
// 将一帧无序点云转换成有序点云消耗的时间
printf("prepare time %f \n", t_prepare.toc());
// 计算每一个点的曲率，这里的laserCloud是有序的点云，故可以直接这样计算（论文中说对每条线扫scan计算曲率）
// 但是在 每条scan的交界处 计算得到的曲率是不准确的，这可通过scanStartInd[i]、scanEndInd[i]来选取
/*
* 表面上除了前后五个点都应该有曲率，但是由于临近点都在扫描上选取，实际上每条扫描线上的前后五个点也不太准确，
* 应该由scanStartInd[i]、scanEndInd[i]来确定范围
*/
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;
// 存储曲率，索引
// 对应论文中的公式（1），但是没有进行除法
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
/*
* cloudSortInd[i] = i相当于所有点的初始自然序列，从此以后，每个点就有了它自己的序号（索引）
* 对于每个点，我们已经选择了它附近的特征点数量初始化为0，
* 每个点的点类型初始设置为0（次极小平面点）
*/
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
} 提取特征需要注意几条原则：
1、为了提高效率，论文中对每条扫描线分成了4个扇形，而代码中实际是分成了6份，在每份中寻找曲率最大的20个点最为极大角点，对提取数据做了约束，最大点不大于2个，极小面点不大于4个，剩下的编辑未次极小面点；
2、为防止特征点过于集中，每提取一个特征点，就对该点和它附近的点的标志位设置未“已选中”，在循环提取时会跳过已提取的特征点，对于次极小面点采取下采样方式避免特征点扎堆。
pcl::PointCloud cornerPointsSharp; // 极大边线点
pcl::PointCloud cornerPointsLessSharp; // 次极大边线点
pcl::PointCloud surfPointsFlat; // 极小平面点
pcl::PointCloud surfPointsLessFlat; // 次极小平面点（经过降采样）
// 对每条线扫scan进行操作（曲率排序，选取对应特征点）
float t_q_sort = 0; // 用来记录排序花费的总时间
// 遍历每个scan
for (int i = 0; i < N_SCANS; i++)
{
/// 如果最后一个可算曲率的点与第一个的差小于6，说明无法分成6个扇区，跳过
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
// 用来存储不太平整的点
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
// 将每个scan等分成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;
TicToc t_tmp;
// 对点云按照曲率进行排序，小的在前，大的在后
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
// t_q_sort累计每个扇区曲率排序时间总和
t_q_sort += t_tmp.toc();
// 选取极大边线点（2个）和次极大边线点（20个）
int largestPickedNum = 0;
// 从最大曲率往最小曲率遍历，寻找边线点，并要求大于0.1
for (int k = ep; k >= sp; k--)
{
// 排序后顺序就乱了，这个时候索引的作用就体现出来了
// 取出点的索引
int ind = cloudSortInd[k];
// 看看这个点是否是有效点，同时曲率是否大于阈值
// 没被选过 && 曲率 > 0.1
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++;
// 每段选2个曲率大的点
if (largestPickedNum <= 2)
{
// label为2是曲率大的标记
cloudLabel[ind] = 2;
// cornerPointsSharp存放大曲率的点
// 既放入极大边线点容器，也放入次极大边线点容器
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
// 以及20个曲率稍微大一些的点
else if (largestPickedNum <= 20)
{
// label置1表示曲率稍微大
// 超过2个选择点以后，设置为次极大边线点，仅放入次极大边线点容器
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
// 超过20个就算了
else
{
break;
}
// 这个点被选中后 pick标志位置1
cloudNeighborPicked[ind] = 1;
// 为了保证特征点不过度集中，将选中的点周围5个点都置1,避免后续会选到
// ID为ind的特征点的相邻scan点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
// 右侧
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;
}
}
}
// 下面开始挑选面点（4个）
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
// 确保这个点没有被pick且曲率小于阈值
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
// -1认为是平坦的点
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
// 这里不区分平坦和比较平坦，因为剩下的点label默认是0,就是比较平坦
if (smallestPickedNum >= 4)
{
break;
}
// 同样距离 < 0.05的点全设为已经选择过
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]);
}
}
}
// 对每一条scan线上的次极小平面点进行一次降采样
pcl::PointCloud surfPointsLessFlatScanDS;
pcl::VoxelGrid downSizeFilter;
// 一般平坦的点比较多，所以这里做一个体素滤波
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
接下来就是对当前点云、四种特征点云进行 打包发布，不再赘述，目的是给laserodometry 通信，将预处理的数据传递给里程计节点。
2.2、总结概括
该部分代码主要工作就是处理无效点云，计算点云线束和对齐时间戳，计算曲率、并提取特征点。
总体处理流程，下面博主流程图总结的很好，不再单独整理了，借鉴于此
SLAM前端入门框架-A_LOAM源码解析 - 知乎
上述流程图应该已经很清楚的表明这个scanRegistration文件的执行流程。可能会存在几个关键问题，在下面具体展开一下。
关于步骤2和步骤3中去除无效点云和自身点云应该是很正常的流程
步骤4计算这一帧起始角度和终止角度是为了构建一个“整体”，下面就可以通过这个点和初始角度的偏差来计算，这个点到底在这一帧内部进行了多久的运动，并且将这个时间存储起来，从而进行雷达去畸变。（具体什么是雷达去畸变可以参考LOAM中的论文）
步骤5，计算出每一个点对应的线束，并且重新组织，是为了下面计算曲率使用。
计算方式是通过同一条线上的角度相同。利用atan(z/(sqr(x*x+y*x))来计算。（具体的线束分布需要按照不同的激光雷达给出的说明手册）
步骤6，利用论文中给出的公式来计算（只能说是大体上是），代码中做了一些改变。
步骤7，人为给定一个阈值（0.1），将每一条扫描线分为六段，目的是不能让特征过于集中，每一段计算曲率大于阈值的点（少量 2个），大于阈值的点（多量 20个），小于阈值的点（4个），其他所有没有被选中的点。在这个过程中，如果一个个点被选中了，就立刻标记周围五个距离小于0.05的点，目的也是防止特征过于集中。
3、LaserOdometry.cpp
laserOdometry.cpp主要是通过读取scanRegistration.cpp中的信息来计算帧与帧之间的变化，最终得到里程计坐标系下的激光雷达的位姿。也就是前端激光里程计和位姿粗估计，帧间匹配应用scan-scan 思想
3.1、代码注释
主函数 定义订阅点云数据的话题与发布里程计的角点面点，以及里程计等
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param("mapping_skip_frame", skipFrameNum, 2);
printf("Mapping %d Hz \n", 10 / skipFrameNum);
// 订阅提取出来的点云
// 极大边线点 次极大边线点 极小平面点 次极小平面点
ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
//全部点点云
ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler);
// 发布上一帧的边线点
ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100);
// 发布上一帧的平面点
ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100);
// 发布全部有序点云，就是从scanRegistration订阅来的点云，未经其他处理
ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100);
// 发布帧间的位姿变换
ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init", 100);
// 发布帧间的平移运动
ros::Publisher pubLaserPath = nh.advertise("/laser_odom_path", 100);
nav_msgs::Path laserPath;
int frameCount = 0;
ros::Rate rate(100);
while (ros::ok())
{
ros::spinOnce();
//........
rate.sleep();
}
return 0;
} 主函数中的主循环未10HZ的高频周期，主要是帧间匹配。
预处理 取出需要的点云数据转化为pcl格式
while (ros::ok())
{
ros::spinOnce(); // 触发一次回调，参考https://www.cnblogs.com/liu-fa/p/5925381.html
// 首先确保订阅的五个消息都有，有一个队列为空都不行
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
// 分别求出队列第一个时间
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
// 因为同一帧的时间戳都是相同的，因此这里比较是否是同一帧
//比较时间是否同步
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
printf("unsync messeage!");
ROS_BREAK();
}
// 分别将五个点云消息取出来，同时转成pcl的点云格式
mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
cornerSharpBuf.pop();
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();
TicToc t_whole;
// initializing
// 一个什么也不干的初始化
if (!systemInited)
{
// 主要用来跳过第一帧数据
// 仅仅将cornerPointsLessSharp保存至laserCloudCornerLast
// 将surfPointsLessFlat保存至laserCloudSurfLast，以及更新对应的kdtree
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{
// 极大边线点的数量
int cornerPointsSharpNum = cornerPointsSharp->points.size();
// 极小平面点的数量
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
// 点到线以及点到面的非线性优化，迭代2次（选择当前优化的位姿的特征点匹配，并优化位姿（4次迭代），
//然后重新选择特征点匹配并优化）
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0;
plane_correspondence = 0;
//ceres::LossFunction *loss_function = NULL;
// 使用Huber核函数来减少外点的影响，Algorithm 1: Lidar Odometry中有
// 定义一下ceres的核函数
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
// 由于旋转不满足一般意义的加法，因此这里使用ceres自带的local param
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// 待优化的变量是帧间位姿，平移和旋转，这里旋转使用四元数来表示
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
pcl::PointXYZI pointSel;
std::vector pointSearchInd;
std::vector pointSearchSqDis;
TicToc t_data; 分别通过两次迭代计算点线和面线之间的关系，并创建KD-tree。其中点线基于最近邻原理建立corner特征点（边线点）之间的关联，每一个极大边线点去上一帧的次极大边线点中找匹配；采用边线点匹配方法:假如在第k+1帧中发现了边线点i，通过KD-tree查询在第k帧中的最近邻点j，查询j的附近扫描线上的最近邻点l，j与l相连形成一条直线l-j，让点i与这条直线的距离最短。
构建一个非线性优化问题：以点i与直线lj的距离为代价函数，以位姿变换T(四元数表示旋转+位移t)为优化变量。下面是优化的程序。
// find correspondence for corner features
// 寻找角点的约束
// 基于最近邻原理建立corner特征点之间关联，每一个极大边线点去上一帧的次极大边线点集中找匹配
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 运动补偿
// 这个函数类似论文中公式（5）的功能
// 将当前帧的极大边线点（记为点O_cur），变换到上一帧Lidar坐标系（记为点O），以利于寻找极大边线点的correspondence
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
// 在上一帧所有角点构成的kdtree中寻找距离当前帧最近的一个点
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
// 只有小于给定门限才认为是有效约束
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0]; // 对应的最近距离的索引取出来
// 找到其所在线束id，线束信息藏在intensity的整数部分
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
// 寻找角点，在刚刚角点id上下分别继续寻找，目的是找到最近的角点，由于其按照线束进行排序，所以就是向上找
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// if in the same scan line, continue
// 不找同一根线束的
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
// if not in nearby scans, end the loop
// 要求找到的线束距离当前线束不能太远
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
// 计算和当前找到的角点之间的距离
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
// 寻找距离最小的角点及其索引
if (pointSqDis < minPointSqDis2)
{
// find nearer point
// 记录其索引
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// search in the direction of decreasing scan line
// 同样另一个方向寻找对应角点
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
// 如果这个角点是有效的角点
// 即特征点i的两个最近邻点j和m都有效，构建非线性优化问题
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{
// 取出当前点和上一帧的两个角点
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
下面采用平面点匹配方法：假如在第k+1帧中发现了平面点i，通过KD-tree查询在第k帧（上一帧）中的最近邻点j，查询j的附近扫描线上的最近邻点l和同一条扫描线的最近邻点m，这三点确定一个平面，让点i与这个平面的距离最短；
构建一个非线性优化问题：以点i与平面lmj的距离为代价函数，以位姿变换T(四元数表示旋转+t)为优化变量。
// find correspondence for plane features
// 与上述类似，寻找平面点的最近邻点j,l,m
for (int i = 0; i < surfPointsFlatNum; ++i)
{
// 这个函数类似论文中公式（5）的功能
// 将当前帧的极大边线点（记为点O_cur），变换到上一帧Lidar坐标系（记为点O），以利于寻找极大边线点的correspondence
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
// 先寻找上一帧距离这个面点最近的面点
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
// 距离必须小于给定阈值
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
// 取出找到的上一帧面点的索引
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
// 取出最近的面点在上一帧的第几根scan上面
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// 额外在寻找两个点，要求，一个点和最近点同一个scan，另一个是不同scan
// search in the direction of increasing scan line
// 按照增量方向寻找其他面点
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// if not in nearby scans, end the loop
// 不能和当前找到的上一帧面点线束距离太远
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
// 计算和当前帧该点距离
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or lower scan line
// 如果是同一根scan且距离最近
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// if in the higher scan line
// 如果是其他线束点
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// search in the direction of decreasing scan line
// 同样的方式，去按照降序方向寻找这两个点
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or higher scan line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 如果另外找到的两个点是有效点，就取出他们的3d坐标
if (minPointInd2 >= 0 && minPointInd3 >= 0)
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 构建点到面的约束
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}应用ceres 构建求解器：
printf("data association time %f ms \n", t_data.toc());
// 如果总的约束太少，就打印一下
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
// 调用ceres求解器求解
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
// 这里的w_curr 实际上是 w_last
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
}里程计数据打包与发布比较简单不再赘述，其中值得注意的是需要降频发给后端。
在求点到线和点到面函数中需要对点云数据就行运动补偿以去除运动畸变带来的估计误差，
运动补偿畸变函数LOAM假设激光雷达的运动为匀速模型，虽然实际中并不完全匹配，但是实际使用效果也基本没有问题。
常用的做法是补偿到起始时刻，如果有IMU，我们通过IMU得到的雷达高频位姿，可以求出每个点相对起始点的位姿，就可以补偿回去。
如果没有IMU，我们可以使用匀速模型假设，使⽤上⼀个帧间⾥程记的结果作为当前两帧之间的运动，假设当前帧也是匀速运动，可以估计出每个点相对起始时刻的位姿。
最后，当前点云中的点相对第一个点去除因运动产生的畸变，效果相当于静止扫描得到的点云。下面是去除运动畸变的函数。
/ undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s;
// 由于kitti数据集上的lidar已经做过了运动补偿，因此这里就不做具体补偿了
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0; // s = 1s说明全部补偿到点云结束的时刻
//s = 1;
// 所有点的操作方式都是一致的，相当于从结束时刻补偿到起始时刻
// 这里相当于是一个匀速模型的假设
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;
Eigen::Vector3d point(pi->x, pi->y, pi->z);
Eigen::Vector3d un_point = q_point_last * point + t_point_last;
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
// transform all lidar points to the start of the next frame
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp);
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//Remove distortion time info
po->intensity = int(pi->intensity);
}3.2、总结概括
该部分主要实现前端高频低精度的激光里程计。首先搞清楚几个坐标系之间的转换，然后对点云匹配前先去畸变。采用两部LM的方法分别求解点到线的距离，点到面的距离，应用ceres 构建求解器。构建损失函数，构建优化项代入ceres 求解器。该部分在LOAM 中，求解是相当麻烦
流程如下图
代码流程图，同样参考知乎大佬
SLAM前端入门框架-A_LOAM源码解析 - 知乎
上图已经将laserOdometry的流程描述的比较清楚了。下面就具体的说明一下可能会存在的实现上或者原理上的问题。
步骤1，主要就是配置订阅和发布节点，以及一些提前使用的变量的初始化。
步骤2，步骤3主要判断是否存在数据异常。
步骤4，用来判断是否有第一帧数据，如果没有第一帧数据，没有办法进行后续的匹配。所以需要进行一个初始化判断。
步骤5，是最关键的一个步骤，内部流程是将获取的每一个角点和面点（小集合）利用估计的映射，映射到上一时刻（这一帧雷达的启始时刻）（代码中有实现关于畸变去除的部分），通过利用历史帧构建的面KDTree和线KDTree（大集合）来匹配相似的点来构建点线和点面残差，利用ceres来求解较为精准的里程计。
线残差的计算方式，面残差的计算方式具体看下文，在lidarFactor中有具体的原理和涉及
步骤6，将这一帧的角点和面点（大集合）信息加入面KDTree和线KDTree中，以供下一帧匹配使用。
步骤7，发布激光里程计，点云以及角点和面点（大集合）。
4、laserMapping.cpp
laserMapping.cpp主要是通过已经获得的激光里程计信息来消除激光里程计和地图之间的误差（也就是累计的误差），使得最终的姿态都是关于世界坐标的。
前端里程计会周期向后端发送位姿T_odom,但是Mapping模块中，我们需要得到位姿是T_map_cur,因此mapping就需要估计出odom坐标系与map坐标系之间的相对位姿变换T_map_odm，这部分需要仔细推导一下。 T_map_cur = T_map_odom × T_odom_cur
4.1、代码注释
主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
float lineRes = 0; // 次极大边线点集体素滤波分辨率
float planeRes = 0; // 次极小平面点集体素滤波分辨率
nh.param("mapping_line_resolution", lineRes, 0.4);
nh.param("mapping_plane_resolution", planeRes, 0.8);
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
// 从laserOdometry节点接收次极大边线点
ros::Subscriber subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
// 从laserOdometry节点接收次极小平面点
ros::Subscriber subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
// 从laserOdometry节点接收到的最新帧的位姿T_cur^w
ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_to_init", 100, laserOdometryHandler);
// 从laserOdometry节点接收到的原始点云（只经过一次降采样）
ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_3", 100, laserCloudFullResHandler);
// submap所在cube中的点云 / 发布周围5帧的点云集合（降采样以后的）
pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 100);
// 所欲cube中的点云
pubLaserCloudMap = nh.advertise("/laser_cloud_map", 100);
// 原始点云
pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_registered", 100);
// 经过Scan to Map精估计优化后的当前帧位姿
pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 100);
// 将里程计坐标系位姿转化到世界坐标系位姿（地图坐标系），相当于位姿优化初值
pubOdomAftMappedHighFrec = nh.advertise("/aft_mapped_to_init_high_frec", 100);
// 经过Scan to Map精估计优化后的当前帧平移
pubLaserAfterMappedPath = nh.advertise("/aft_mapped_path", 100);
/*
* 重置这两个数组，这两数组用于存储所有边线点cube和平面点cube
*/
for (int i = 0; i < laserCloudNum; i++)
{
laserCloudCornerArray[i].reset(new pcl::PointCloud());
laserCloudSurfArray[i].reset(new pcl::PointCloud());
}
std::thread mapping_process{process};
ros::spin();
return 0;
} 关于坐标转换的几个函数：
这里涉及到几个坐标系的相互转换，需要理清关系：雷达坐标系（每帧扫描到的点云点的坐标point_curr都在雷达坐标系中），里程计坐标系（由laseOdometry节点粗估计得到的LiDAR位姿wodom_curr对应的坐标系），地图坐标系（真实的世界坐标系）
// set initial guess，里程计位姿转化为地图位姿，作为后端初始估计
void transformAssociateToMap()
{
// T_w_curr = T_w_last * T_last_curr(from lidar odom)
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
// 更新odom到map之间的位姿变换
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
//雷达坐标系点转化为地图点
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
//po->intensity = 1.0;
}
//地图点转化到雷达坐标系点
void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
po->x = point_curr.x();
po->y = point_curr.y();
po->z = point_curr.z();
po->intensity = pi->intensity;
}
回调函数处理
// 回调函数中将消息都是送入各自队列，进行线程加锁和解锁
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
//receive odomtry
//里程计坐标系位姿转化为地图坐标系位姿
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
// high frequence publish
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
//里程计坐标系位姿转换为地图坐标系位姿
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMappedHighFrec.publish(odomAftMapped);
}接下来就是核心函数，scan to map 位姿精匹配的实现。但是，如果完全使用所有区域的点云进行匹配，这样的效率会很低，而且内存空间可能会爆掉。LOAM论文中提到采用的是栅格（cube）地图的方法，将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆掉，同时保证效率。
process 主线程中，首先对数据就行预处理，同步时间戳，转换为pcl格式，点云数据坐标转换，建立cube,并通过六个循环处理当前栅格，使其始终保持单钱栅格在中间位置，目的是为了保证当前帧不在局部地图的边缘，方便从地图中获取足够的特征点
// 主处理线程
void process()
{
while(1)
{
// 假如四个队列非空，四个队列分别存放边线点、平面点、全部点、和里程计位姿
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
// laserOdometry模块对本节点的执行频率进行了控制，laserOdometry模块publish的位姿是10Hz，点云的publish频率则可以没这么高
// 保证其他容器的最新消息与cornerLastBuf.front()最新消息时间戳同步
mBuf.lock();
// 以cornerLastBuf为基准，把时间戳小于其的全部pop出去
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
// 原则上取出来的时间戳都是一样的，如果不一定说明有问题了
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
// 点云全部转成pcl的数据格式
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
// lidar odom的结果转成eigen数据格式
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
// 考虑到实时性，就把队列里其他的都pop出去，不然可能出现处理延时的情况
// 其余的边线点清空
// 为了保证LOAM算法整体的实时性，因Mapping线程耗时>100ms导致的历史缓存都会被清空
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
TicToc t_whole;
// 根据前端结果，得到后端的一个初始估计值
transformAssociateToMap();
TicToc t_shift;
// 根据初始估计值计算寻找当前位姿在地图中的索引，一个各自边长是50m
// 后端的地图本质上是一个以当前点为中心，一个珊格地图
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
// 如果小于25就向下取整，相当于四舍五入的一个过程
// 由于int(-2.1)=-2是向0取整，当被求余数为负数时求余结果统一向左偏移一个单位
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
// 如果当前珊格索引小于3,就说明当前点快接近地图边界了，需要进行调整，相当于地图整体往x正方向移动
// 调整取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 18, 3 < centerCubeK < 8
// 如果cube处于边界，则将cube向中心靠拢一些，方便后续拓展cube
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
// 从x最大值开始
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整体右移
for (; i >= 1; i--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
// 此时i = 0,也就是最左边的格子赋值了之前最右边的格子
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
// 该点云清零，由于是指针操作，相当于最左边的格子清空了
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
// 索引右移
centerCubeI++;
laserCloudCenWidth++;
}
// 同理x如果抵达右边界，就整体左移
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整体左移
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
// y和z的操作同理
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}
// 以上操作相当于维护了一个局部地图，保证当前帧不在这个局部地图的边缘，这样才可以从地图中获取足够的约束
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// 从当前格子为中心，选出地图中一定范围的点云
// 向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube
// 在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（前后250米范围内，总共500米范围），三个维度总共125个cube
// 在这125个cube里面进一步筛选在视域范围内的cube
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
// 把各自的索引记录下来
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
// 将有效index的cube中的点云叠加到一起组成submap的特征点云
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
// 开始构建用来这一帧优化的小的局部地图
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
// 为了减少运算量，对点云进行下采样
pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum); scan to map 在submap的cube 与全部地图的cube匹配时，帧间匹配的方法不再适用，cube上的方法如下：
1. 取当前帧的角点与面点
2. 找到全部地图特征点中，当前特征点的5个最近邻点。
3. 如果是角点，则以这五个点的均值点为中心，以5个点的主方向向量（类似于PCA方法）为方向，作一条直线，令该边线点与直线距离最短，构建非线性优化问题。
4. 如果是平面点，则寻找五个点的法方向（反向的PCA方法），令这个平面点在法方向上与五个近邻点的距离最小，构建非线性优化问题。
5. 优化变量是雷达位姿，求解能够让以上非线性问题代价函数最小的雷达位姿。
// 最终的有效点云数目进行判断
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
// 送入kdtree便于最近邻搜索
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
// 建立对应关系的迭代次数不超2次 两次非线性优化
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//ceres::LossFunction *loss_function = NULL;
// 建立ceres问题
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
// 构建角点相关的约束
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
// 把当前点根据初值投到地图坐标系下去
// submap中的点云都是在(map)world坐标系下，而接收到的当前帧点云都是Lidar坐标系下
// 所以在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到(map)world坐标系下
pointAssociateToMap(&pointOri, &pointSel);
// 地图中寻找和该点最近的5个点
// 特征点中，寻找距离当前帧corner特征点最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 判断最远的点距离不能超过1m，否则就是无效约束
if (pointSearchSqDis[4] < 1.0)
{
// 计算这个5个最近邻点的中心
std::vector nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
// 计算这五个点的均值
center = center / 5.0;
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
// 构建5个最近邻点的协方差矩阵
for (int j = 0; j < 5; j++)
{
Eigen::Matrix tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 进行特征值分解
Eigen::SelfAdjointEigenSolver saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
// 根据特征值分解情况看看是不是真正的线特征
// 特征向量就是线特征的方向
// 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向矢量
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 最大特征值大于次大特征值的3倍认为是线特征
// 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 根据拟合出来的线特征方向，以平均点为中心构建两个虚拟点
// 从中心点沿着方向向量向两端移动0.1m，使用两个点代替一条直线，这样计算点到直线的距离的形式就跟laserOdometry相似
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 构建约束，和lidar odom约束一致
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
int surf_num = 0;
// 构建面点约束
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix matA0;
Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones();
// 构建平面方程Ax + By +Cz + 1 = 0
// 通过构建一个超定方程来求解这个平面方程
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
// 调用eigen接口求解该方程，解就是这个平面的法向量
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
double negative_OA_dot_norm = 1 / norm.norm(); // 法向量长度的倒数
// 法向量归一化
norm.normalize(); // 法向量归一化，相当于直线Ax + By + Cz + 1 = 0两边都乘negative_OA_dot_norm
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
// 根据求出来的平面方程进行校验，看看是不是符合平面约束
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
// 这里相当于求解点到平面的距离
// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false; // 点如果距离平面太远，就认为这是一个拟合的不好的平面
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 如果平面有效就构建平面约束
if (planeValid)
{
// 利用平面方程构建约束，和前端构建形式稍有不同
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
// 调用ceres求解
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}
// 完成（迭代2次）特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，让下一次的Mapping初值更准确
transformUpdate(); 接下来就是一些后处理，即将当前帧的特征点加入到全部地图栅格中，对全部地图栅格中的点进行降采样，刷新附近点云地图，刷新全部点云地图，发布当前帧的精确位姿和平移估计。
TicToc t_add;
// 将优化后的当前帧角点加到局部地图中去
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// 该点根据位姿投到地图坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 算出这个点所在的格子的索引
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
// 同样四舍五入一下
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
// 如果超过边界的话就算了
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
// 根据xyz的索引计算在一位数组中的索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
// 面点也做同样的处理
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
// 把当前帧涉及到的局部地图的珊格做一个下采样
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];
pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
TicToc t_pub;
//publish surround map for every 5 frame
// 每隔5帧对外发布一下
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
// 把该当前帧相关的局部地图发布出去
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}
// 每隔20帧发布全量的局部地图
if (frameCount % 20 == 0)
{
pcl::PointCloud laserCloudMap;
// 21 × 21 × 11 = 4851
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
// 把当前帧发布出去
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
// 发布当前位姿
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
// 发布当前轨迹
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
// 发布tf
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
} 4.2、总结概括
该部分主要实现低频率高精度的地图构建。该部分的算法流程如论文中可参考下图理解
关于坐标之间的转换是理解代码的关键，关于三个坐标系之间的转换关系，可以参考知乎大佬的图
SLAM前端入门框架-A_LOAM源码解析 - 知乎
下面皆引自上述链接
我们再来描述一下这个过程，首先每一帧的里程计坐标从上一个节点不断传来，起初里程计坐标和世界坐标下是没有偏差的，利用里程计坐标和两者之间的变化来求解世界坐标。优化后得到准确的世界坐标。利用这个准确的坐标和这一点的里程计坐标来求解，里程计坐标和世界坐标之间的变化，来给下一帧的激光里程计作为估计参考。
接下来是代码流程部分：
具体看下图：
上述图已经描绘的很清楚了，我是按照代码片段和个人的一些判断分成了15个步骤，每一步骤都有具体解释。比较复杂的可能也就是两部分，一部分是关于大cube的构建和移动，另一部分就是利用特征值和特征向量来计算线和面了
首先是cube的构建：
A_LOAM在实现的过程中构建了一个21*21*11的大立方体。
在流程图的第1步初始化时给其分配了对应的空间。
在流程图第5步中计算当前世界坐标系下的位置，就是计算上图中红色五角星的位置。对应的蓝色cube也就是此刻世界坐标下的点云。
其次是调整cube和此刻的相对位置：
流程图第6步所做的事情就是将这个这个大cube（21*21*11）的相对中间的位置为红色五角星（此刻点云在世界坐标系的位置），也就是通过6个while循环，移动到(3-18,3-18,3-11)的位置。具体移动一小格的示意图见下图
5、lidarFactor.cpp
在laserOdometry和laserMapping中利用特征来计算位姿变化时，需要计算残差，这里就是来计算残差的。
下面介绍下ceres优化问题如何求解：定义一个模板类，重写优化问题，在这个类中定义残差，重载（）运算符，（）运算符函数前⼏个参数是参数块的起始指针，最后⼀个参数是残差块的指针，（）运算符负责计算残差。下面是优化边线点的模板类程序。
线残差计算方式（引自 SLAM前端入门框架-A_LOAM源码解析 - 知乎）
点线特征模板程序
struct LidarEdgeFactor
{
LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
template
bool operator()(const T *q, const T *t, T *residual) const
{
// 将double数组转成eigen的数据结构，注意这里必须都写成模板
Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};
// 计算的是上一帧到当前帧的位姿变换，因此根据匀速模型，计算该点对应的位姿
// 这里暂时不考虑畸变，因此这里不做任何变换
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix lp;
// 把当前点根据当前计算的帧间位姿变换到上一帧
lp = q_last_curr * cp + t_last_curr;
Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); // 模是三角形的面积
Eigen::Matrix de = lpa - lpb;
// 残差的模是该点到底边的垂线长度
// 这里感觉不需要定义三维
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarEdgeFactor, 3, 4, 3>(
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
面参数计算方法：
面点残差模板
struct LidarPlaneFactor
{
LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
last_point_m(last_point_m_), s(s_)
{
// 求出平面单位法向量
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
ljm_norm.normalize();
}
template
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
//Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
//Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};
// 根据时间戳进行插值
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix lp;
lp = q_last_curr * cp + t_last_curr;
// 点到平面的距离
residual[0] = (lp - lpj).dot(ljm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneFactor, 1, 4, 3>(
new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};
关于ceres 优化器的使用，可参考 高博的《视觉slam十四讲》熟悉熟悉。推荐ceres官网（http://www.ceres-solver.org/）
6、参考连接
SLAM前端入门框架-A_LOAM源码解析 - 知乎
从A_LOAM开始入门3D激光SLAM - 知乎
二十七-VIO-SLAM开源框架Vin-mono跑EuRoC数据集 - 知乎
LOAM细节分析 - 知乎
loam代码解析 - 知乎