[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇
[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇
- [笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇
- 一些细节
- 为什么要有建图节点？
- 建图节点起到了什么作用？
- 节点概述
- main函数
- 坐标转换
- 优化处理
[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇
一些细节
本节点使用实现了一个较为完整的SLAM过程,也就是同时建图和定位.主要工作：
- 通过将多帧的激光特征点云基于上一节点提供的pose拼接,形成特征点云地图。（包含corner和surface两种特征点云地图）
- 将新入的帧与地图作配准,得到更精准的pose.我们又将基于这个pose执行1过程进行建图。
由于单帧与地图配准计算量较大,本节点没有将所有帧与地图进行配准,而是间隔几帧,比如每5帧配准一次（那么频率就是2hz）。
如果你的运算性能完全够, 你可以一帧一匹配。如果这样,下一个节点transformMaintenance这个节点就没有存在的意义了。
点云配准问题：
上一个节点 laserOdometry 和本节点 laserMapping 中都涉及到点云配准问题。
点云的配准问题一般有一个source和一个target,配准的目的是旋转和平移source点云使得source点云与target点云尽量重叠。（不懂的可以看前面讲的ICP配准方法）
需要注意的是：
- 在上一节点 laserOdometry 中配准的source是新一帧的点云,target是前一帧的点云。
- 在本节点 laserMapping 中配准的source是新一帧的点云,target是前面的点云拼接成的地图。
当然,我们这里说的点云指的是特征点云, 分为corner和surface两种,corner对应corner map, surface对应surface map。
优化流程:
通过odom预测得到初始位姿,source中的corner在target的corner中寻找对应直线, source中的surface在target的surface中寻找对应平面,
通过点-线和点-面关联共同构建约束, 以点线距离和点面距离作为loss, 基于非线性最小二乘的方法进行优化.求解得出最优pose。
关键点：如果能将本节点生成的 特征地图（corner对应corner map, surface对应surface map） 存下来, 那么就可以将这些地图作为离线地图。我们可以在在线场景中，把这个地图输入给laserMapping节点,实现定位的功能（需要初始值）。
缺陷点：这个地图功能还不够完善，比如没有回环检测，这时候可以参考LeGO-LOAM或者SC-LeGO-LOAM，以后再做分析。
然后明确两个问题：
- 为什么要有这个建图节点？
- 建图节点又到底起了什么作用？
为什么要有建图节点？
由原理篇得，论文中将SLAM问题分为两步：
- 高频的运动估计（10Hz）
- 低频（1Hz）的环境建图
然而，上一部分Lidar里程计的结果不准确，由它拼起来的点云完全不成样子，而且Lidar里程计会不断发散，因此误差也会越来越大。
在第一部分提取特征的时候只关注了表面曲率，这不足以表达三维世界，这种方式并不能完美的得到两帧点云准确的配准点。
而且点云中的点是离散的，我们也无法保证上一帧的点在下一帧中仍会被扫到。
因此，论文提出第二步的建图，依靠其去优化Lidar里程计的位姿估计精度。其实道理也很简单，我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。因此我们对已经构建地图的信任程度远高于最近帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。
建图节点起到了什么作用？
它的作用就是利用地图来优化Lidar里程计的位姿估计结果。
在得到第一帧点云时你的lidar就扫到了数万个点，此时Lidar的位置我们把它作为（0,0,0），在不考虑测量噪声的情况下这数万个点都是相对精确的，我们把这数万个点所构成的环境作为此时的地图，此刻的Lidar坐标系定义为map坐标系。而后Lidar运动了一小段，我们通过Lidar里程计的方法估算了它的相对运动，于是就可以将此时的Lidar位姿及此时的点按照我们估计的相对运动情况，转换到上一时刻（map坐标系）的坐标系下。只不过由于里程计估计误差，地图可能拼歪了。既然这样，我是如果把地图完整的拼上去，是不是就可以优化此时Lidar的位姿了呢？这就是建图节点起到的关键作用。只不过拿当前扫描的点云和地图中所有点云去配准，这个计算消耗太大，因此为了保证实时性，作者在这里采用了一种低频处理方法，即调用建图节点的频率仅为调用里程计节点频率的十分之一。
节点概述
有上述的两个思路，我们看看论文怎么做的，这一部分论文的解析也是非常简短。
在 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk,tk+1]内，Lidar得到的点云为 P ‾ k {\overline P}_{k} Pk，Lidar里程计估计的相对运动为 T k L ( t k + 1 ) {T}_{k}^{\boldsymbol L}(t_{k+1}) TkL(tk+1)。定义在 t k t_k tk时刻的地图为 Q k − 1 Q_{k-1} Qk−1，且该时刻Lidar在地图中的的位姿为 T k − 1 W ( t k ) {T}_{k-1}^{\boldsymbol W}(t_{k}) Tk−1W(tk)。于是，我们可以根据 T k L ( t k + 1 ) {T}_{k}^{\boldsymbol L}(t_{k+1}) TkL(tk+1)将 T k − 1 W ( t k ) {T}_{k-1}^{\boldsymbol W}(t_{k}) Tk−1W(tk)扩展为 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1)并将 P ‾ k {\overline P}_{k } Pk转换到世界坐标系 W {W} W下得到 Q ‾ k {\overline Q}_{k} Qk。然后通过 不断调整 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1)得到最优的 T k W ( t k + 1 ) {T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1)从而实现 Q k − 1 {Q}_{k-1} Qk−1与 Q ‾ k {\overline Q}_{k} Qk的配准。
代码的整体框架：在订阅器订阅到了laserOdometry发布的消息后即可开始进行处理。继续从main函数开始
main函数
订阅器、发布器等初始化套路就不贴了
int frameCount = stackFrameNum - 1; //0
int mapFrameCount = mapFrameNum - 1; //4
ros::Rate rate(100);
bool status = ros::ok();
while (status) {
ros::spinOnce();
if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {
newLaserCloudCornerLast = false;
newLaserCloudSurfLast = false;
newLaserCloudFullRes = false;
newLaserOdometry = false;
frameCount++;
/***** 坐标转换 *****/
if (frameCount >= stackFrameNum) {
}
/***** 优化处理 *****/
if (frameCount >= stackFrameNum) {
}
status = ros::ok();
rate.sleep();
}
坐标转换
这部分十分清晰，是点云数据处理前的准备工作。首先计算 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1)，而后根据 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1)将测量得到的点坐标转换到世界坐标系 W {W} W下。两个坐标转换函数都采用欧拉角表示姿态旋转。
//控制跳帧数，>=这里实际并没有跳帧，只取>或者增大stackFrameNum才能实现相应的跳帧处理
if (frameCount >= stackFrameNum) {
//获取世界坐标系转换矩阵
transformAssociateToMap();
//将最新接收到的平面点和边沿点进行旋转平移转换到世界坐标系下(这里和后面的逆转换应无必要)
int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
for (int i = 0; i < laserCloudCornerLastNum; i++) {
pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
laserCloudCornerStack2->push_back(pointSel);
}
int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
for (int i = 0; i < laserCloudSurfLastNum; i++) {
pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
laserCloudSurfStack2->push_back(pointSel);
}
}
优化处理
这部分比较难理解。
原文是这样说的：
To find correspondences for the feature points, we store the point cloud on the map, Q k − 1 Q_{k-1} Qk−1, in 10m cubic areas. The points in the cubes that intersect with Q ‾ k {\overline Q}_k Qkare extracted and stored in a 3D KD-tree in {W}. We find the points in Q k − 1 Q_{k-1} Qk−1 within a certain region (10cm × 10cm × 10cm) around the feature points.
翻译过来就是：
将地图 Q k − 1 Q_{k-1} Qk−1保存在一个10m的立方体中，若cube中的点与当前帧中的点云 Q ‾ k {\overline Q}_k Qk有重叠部分就把他们提取出来保存在KD树中。我们找地图 Q k − 1 Q_{k-1} Qk−1中的点时，要在特征点附近宽为10cm的立方体邻域内搜索（实际代码中是10cm×10cm×5cm）。
原文中用两句话解决的问题，现实中需要用一大堆代码去填坑。实际处理中，我们首先将地图点云 Q k − 1 Q_{k-1} Qk−1保存在一个大cube中，并将其分割为一些子cube。
int laserCloudCenWidth = 10; // 邻域宽度, cm为单位
int laserCloudCenHeight = 5; // 邻域高度
int laserCloudCenDepth = 10; // 邻域深度
const int laserCloudWidth = 21; // 子cube沿宽方向的分割个数
const int laserCloudHeight = 11; // 高方向个数
const int laserCloudDepth = 21; // 深度方向个数
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; // 子cube总数 4851
而后我们就要找当前估计的Lidar位姿 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW(tk+1) 属于哪个子cube。I、J、K对应了cube的索引。可以看出，当坐标属于 [ − 25 , 25 ] [-25,25] [−25,25]时，cube对应与 ( 10 , 5 , 10 ) (10,5,10) (10,5,10)即正中心的那个cube。
PointType pointOnYAxis;// 当前Lidar坐标系{L}y轴上的一点(0,10,0)
pointOnYAxis.x = 0.0;
pointOnYAxis.y = 10.0;
pointOnYAxis.z = 0.0;
//获取y方向上10米高位置的点在世界坐标系下的坐标
pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);
//立方体中点在世界坐标系下的（原点）位置
//过半取一（以50米进行四舍五入的效果），由于数组下标只能为正数，而地图可能建立在原点前后，因此
//每一维偏移一个laserCloudCenWidth（该值会动态调整，以使得数组利用最大化，初始值为该维数组长度1/2）的量
int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;
//由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一
if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;
如果取到的子cube在整个大cube的边缘则将点对应的cube的索引向中心方向挪动一个单位，这样做主要是截取边沿cube。
//调整之后取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18
//如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位
while (centerCubeI < 3) {
for (int j = 0; j < laserCloudHeight; j++) {
for (int k = 0; k < laserCloudDepth; k++) {//实现一次循环移位效果
int i = laserCloudWidth - 1;
//指针赋值，保存最后一个指针位置
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//that's [i + 21 * j + 231 * k]
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//循环移位，I维度上依次后移
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];
}
//将开始点赋值为最后一个点
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI++;
laserCloudCenWidth++;
}
//如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
while (centerCubeI >= laserCloudWidth - 3) {//18
for (int j = 0; j < laserCloudHeight; j++) {
for (int k = 0; k < laserCloudDepth; k++) {
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//I维度上依次前移
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--;
}
处理完毕边沿点，接下来就是在取到的子cube的 5 ∗ 5 ∗ 5 5*5*5 5∗5∗5的邻域内找对应的配准点了。
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
//在每一维附近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 - 2; k <= centerCubeK + 2; k++) {
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth) {//如果索引合法
// NOTE: 由于ijk均为整数，坐标取值为中心点坐标
//计算子cube对应的点坐标,换算成实际比例，在世界坐标系下的坐标
float centerX = 50.0 * (i - laserCloudCenWidth);
float centerY = 50.0 * (j - laserCloudCenHeight);
float centerZ = 50.0 * (k - laserCloudCenDepth);
bool isInLaserFOV = false;//判断是否在lidar视线范围的标志（Field of View）
for (int ii = -1; ii <= 1; ii += 2) {
for (int jj = -1; jj <= 1; jj += 2) {
for (int kk = -1; kk <= 1; kk += 2) {
//上下左右八个顶点坐标
float cornerX = centerX + 25.0 * ii;
float cornerY = centerY + 25.0 * jj;
float cornerZ = centerZ + 25.0 * kk;
//原点到顶点距离的平方和
float squaredSide1 = (transformTobeMapped[3] - cornerX)
* (transformTobeMapped[3] - cornerX)
+ (transformTobeMapped[4] - cornerY)
* (transformTobeMapped[4] - cornerY)
+ (transformTobeMapped[5] - cornerZ)
* (transformTobeMapped[5] - cornerZ);
//pointOnYAxis到顶点距离的平方和
float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX)
+ (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
+ (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);
float check1 = 100.0 + squaredSide1 - squaredSide2
- 10.0 * sqrt(3.0) * sqrt(squaredSide1);
float check2 = 100.0 + squaredSide1 - squaredSide2
+ 10.0 * sqrt(3.0) * sqrt(squaredSide1);
if (check1 < 0 && check2 > 0) {//if |100 + squaredSide1 - squaredSide2| < 10.0 * sqrt(3.0) * sqrt(squaredSide1)
isInLaserFOV = true;
}
}
}
}
//记住视域范围内的cube索引，匹配用
if (isInLaserFOV) {
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j
+ laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
}
//记住附近所有cube的索引，显示用
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j
+ laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
这里还需要判断一下该点是否属于当前Lidar的可视范围内，可以根据余弦公式对距离范围进行推导。根据代码中的式子，只要点在 x x x轴 ± 60 ° ±60° ±60°的范围内都认为是FOV中的点(作者这么做是因为Lidar里程计的估计结果太不准确了，只能概略的取一个较大的范围)。于是我们就得到了在当前Lidar位置的邻域内有效的地图特征点。
所以，我们就不需要对庞大的所有地图点云进行处理了，只需要处理这些邻域cube内的地图特征点即可，可以节省大量的运算资源。为了保证当前帧的点云足够平滑，还对点云进行了滤波处理。
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();
/***********************************************************************
此处将特征点转移回local坐标系，是为了voxel grid filter的下采样操作不越
界？好像不是！后面还会转移回世界坐标系，这里是前面的逆转换，和前面一样
应无必要，可直接对laserCloudCornerLast和laserCloudSurfLast进行下采样
***********************************************************************/
int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();
for (int i = 0; i < laserCloudCornerStackNum2; i++) {
pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);
}
int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();
for (int i = 0; i < laserCloudSurfStackNum2; i++) {
pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);
}
laserCloudCornerStack->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);//设置滤波对象
downSizeFilterCorner.filter(*laserCloudCornerStack);//执行滤波处理
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();//获取滤波后体素点尺寸
laserCloudSurfStack->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
laserCloudCornerStack2->clear();
laserCloudSurfStack2->clear();
做完这些工作以后，我们就有了在当前Lidar所在位置附近的所有地图特征点以及当前帧的点云特征点，后面的工作就是怎么把这两坨点匹配在一起啦！于是我们再次拿出KD树，来寻找最邻近的5个点。
对点云协方差矩阵进行主成分分析：
- 若这五个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向；
- 若这五个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。
因此我们可以很轻易的根据特征向量找到直线上两点从而利用论文中点到直线的距离公式构建优化问题。平面特征也是相同的思路。
完成了优化问题的构建之后就可以对它进行求解了，求解方法还是L-M迭代。这部分代码与laserOdometry部分的几乎一致，代码略。
截止到这里，我们就完成了当前帧点云与地图点云的配准，并对Lidar里程计的运动估计结果进行完了优化。更新完成后，我们还需要将当前帧扫描得到的特征点云封装在不同的cube中，并在地图数组中保存。
//将surf points按距离（比例尺缩小）归入相应的立方体
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);
}
}
//特征点下采样
for (int i = 0; i < laserCloudValidNum; i++) {
int ind = laserCloudValidInd[i];
laserCloudCornerArray2[ind]->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);//滤波输出到Array2
laserCloudSurfArray2[ind]->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);
//Array与Array2交换，即滤波后自我更新
pcl::PointCloud<PointType>::Ptr laserCloudTemp = laserCloudCornerArray[ind];
laserCloudCornerArray[ind] = laserCloudCornerArray2[ind];
laserCloudCornerArray2[ind] = laserCloudTemp;
laserCloudTemp = laserCloudSurfArray[ind];
laserCloudSurfArray[ind] = laserCloudSurfArray2[ind];
laserCloudSurfArray2[ind] = laserCloudTemp;
}
最后就是将各种信息发布出去了。这里需要说明的是，为了保证运行效率环境点云每5帧发布一次。
//特征点汇总下采样，每隔五帧publish一次，从第一次开始
if (mapFrameCount >= mapFrameNum) {
mapFrameCount = 0;
laserCloudSurround2->clear();
for (int i = 0; i < laserCloudSurroundNum; i++) {
int ind = laserCloudSurroundInd[i];
*laserCloudSurround2 += *laserCloudCornerArray[ind];
*laserCloudSurround2 += *laserCloudSurfArray[ind];
}
laserCloudSurround->clear();
downSizeFilterCorner.setInputCloud(laserCloudSurround2);
downSizeFilterCorner.filter(*laserCloudSurround);
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}
Ps.这部分代码坑很大，还是得细品。