3D激光SLAM:ALOAM---后端lasermapping地图栅格化处理与提取_if (t_w_curr.x() + 25.0 < 0) centercubei--; if (t_

前言

栅格点云地图处理原因
不同于前端的scan-to-scan的过程,ALOAM的后端是scan-to-map的算法,具体来说就是把当前帧和地图进行匹配,得到更准确的位姿同时也可以构建更好的地图.由于是scan-to-map的算法,因此计算量会明显高于scan-to-scan的前端,所以后端通常处于一个低频的运行频率,但是由于scan-to-map的精度往往优于scan-to-scan.因此后端也有比前端更高的精度.为了提高后端的处理速度,所以要进行地图的栅格化处理

栅格地图构成原理
地图通常是当前帧通过匹配得在地图坐标系下的准确位姿之后拼接而成.如果保留所有拼接的点云,此时随着时间的运行,内存很容易吃不消,为此考虑存储离当前帧比较近的部分地图,同时,为了便于地图更新和调整,在原始LOAM中,使用的是基于栅格的地图存储方式.具体来说,将整个地图分成212111个栅格,每个栅格是一个边长50m的正方体,当地图逐渐累加时,栅格之外的部分就被舍弃,这样可以保证内存空间不会随着程序的运行而爆炸.

水平方向上就是2150=1050m的地图 纵向1150=550m

栅格地图调整
如果当前位姿原理栅格覆盖的范围,则地图就没有意义了,因此栅格地图也需要随着当前位姿动态调整,从而保证我们可以从栅格地图中取出离当前位姿比较近的点云来进行scan-to-map的算法,获得最优位姿估计
当当前位姿即将到达地图栅格边界时,调整栅格地图,具体做法会在下面代码分析中展示

代码解析

具体代码在lasermapping.cpp中

transformAssociateToMap();//初值估计
1

初值估计, 和接收到里程计的数据的操作一致
将当前帧到里程计坐标系下的位姿,转到当前帧到map坐标系下,根据里程计坐标系和map坐标系的变换关系
里程计回调函数是为了以前端的频率向外发布位姿
这里主要提供一个估计的初值

			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;
1
2
3

t_w_curr就是当前帧的位置(估计初值),因为ALOAM针对机械式雷达,360度扫描,所以没有考虑旋转,如果要是livox的固态雷达,则必须考虑旋转了.
上面代码的功能就是根据初值估计值计算寻找当前位姿在地图中的索引,一个各自边长是50m
当前位置/50然后加上偏移量.

因为栅格是212111,所以初始是10,10,5,为了上初始时刻,在栅格地图的中心.
加25就是四舍五入的操作.

			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--;
1
2
3
4
5
6

同样是取整的操作,C语言的取整是向0取整,所以要自减1

下面是动态调整栅格地图的部分,即处理快要出边界的情况,对栅格地图做动态调整

首先是当x轴,当前帧栅格索引小于三,说明块出边界了,让整体向x方向移动

			while (centerCubeI < 3)
			{
				for (int j = 0; j < laserCloudHeight; j++)
				{
					for (int k = 0; k < laserCloudDepth; k++)
					{ 
1
2
3
4
5
6

判断当前帧位置索引小于3了,j和k都不动,所以整体for循环

						int i = laserCloudWidth - 1;
						//从x最大值开始
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
1
2
3
4
5
6

i值先取最大,从x最大值开始处理.然后取出了最右边的一片点云

						//整体右移
						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];
						}
1
2
3
4
5
6
7
8
9
10

整体向右移
第一次循环
最后一次循环

						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
1
2
3
4

此时i=0,也就是最左边的格子赋值了之前最右边的格子

						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
1
2

该点云清零,由于是指针操作,相当于最左边的格子清空了

				//索引右移
				centerCubeI++;
				laserCloudCenWidth++;
			}
1
2
3
4

索引右移

while (centerCubeI >= laserCloudWidth - 3)
1

然后是 当前帧位置接近右边的边界,和上面的类似,相当于整体左移

while (centerCubeJ < 3)
1

while (centerCubeJ >= laserCloudHeight - 3)
1

while (centerCubeK < 3)
1

while (centerCubeK >= laserCloudDepth - 3)
1

然后就是J方向和K方向的处理,和I类似,不再赘述

上面随着当前帧位置调整完栅格地图后,下面则根据当前位置取出该位置附近的地图

			int laserCloudValidNum = 0;
			int laserCloudSurroundNum = 0;
			// 从氮气格子为中心,选出一定范围内的点云
			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++;
						}
					}
				}
			}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

以当前帧的位置索引为中心,I方向前后取2个格子,J方向前后去2个格子,K方向前后取一个格子.
然后通过循环把格子里面的点的索引记下来.

			//清空上一次从地图出取出的 角点地图和面点地图 
			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();
1
2
3
4
5
6
7
8
9
10
11

根据上面获得的需要格子的索引,把角点和面点分别取出来,构成用来优化当前帧的局部地图

			pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
			downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
			downSizeFilterCorner.filter(*laserCloudCornerStack);
			int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

			pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
			downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
			downSizeFilterSurf.filter(*laserCloudSurfStack);
			int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
1
2
3
4
5
6
7
8
9

之后就是对角点地图和面点地图进行体素滤波

总结

• 根据前端的结果得到后端一个估计的初值位姿
• 根据位置计算当前帧的栅格索引
• 处理当I J K方向索引过于边界后的栅格位置调整
• 根据前帧的栅格索引,取出周围一定数量的栅格中的点云,构成角度地图和面点地图
• 进行角点地图和面点地图体素滤波处理