loam的学习记录3（laserMapping节点）

laserMapping节点

在k+1帧的最后，雷达里程计产生一个未失真的点云，同时产生一个位姿转换，它包含了在tk+1和tk+2帧之间的雷达的运动。建图算法在世界坐标系下匹配和注册重投影的点云。

main函数

laserMapping节点除了订阅了laserOdometry节点发布的消息，还订阅了“/imu/data”；发布的消息有"/laser_cloud_surround"，"/velodyne_cloud_registered"，"/aft_mapped_to_init"。

transformTobeMapped：重投影到世界坐标系下的变换向量（旋转角+位移）

if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
        fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
        fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005)

接收到角点、平面点、imu消息并且相应的时间戳小于一定值才进入

transformAssociateToMap()

坐标系转移到世界坐标系下,更新transformTobeMapped

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);
}

将最新接收到的平面点和边沿点进行旋转平移转换到全局世界坐标系下，并插入到对应的容器中

laserCloudCornerStack2：存放下采样前的角点，作为中间变量使用
laserCloudCornerStack：存放下采样后的角点

PointType pointOnYAxis;
pointOnYAxis.x = 0.0;
pointOnYAxis.y = 10.0;
pointOnYAxis.z = 0.0;
//获取y方向上10米高位置的点在世界坐标系下的坐标
pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);

• y方向上10米高还是不懂

int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;

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;

I、J、K对应了cube的索引

while (centerCubeI < 3)

while (centerCubeI >= laserCloudWidth - 3)

调整取值范围:3 < centerCubeI < 18，确保位姿在cube中的相对位置（centerCubeI，centerCubeJ，centerCubeK）能够有一个5 * 5 * 5的邻域。

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++) {

在取到的子cube的5 * 5 * 5的邻域内找对应的配准点

//构建特征点地图，查找匹配使用
for (int i = 0; i < laserCloudValidNum; i++) {
	*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
  	*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

laserCloudCornerArray是50米为单位的立方体地图，将该局部地图添加到角点地图laserCloudCornerFromMap中去；平面点同理

laserCloudCornerStack->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);//设置滤波对象
downSizeFilterCorner.filter(*laserCloudCornerStack);//执行滤波处理
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();//获取滤波后体素点尺寸

下采样滤波，最终得到laserCloudCornerStack

laserCloudCornerStack2->clear();

清除laserCloudCornerStack2
对于平面点同理

做完这些工作以后，我们就有了在当前Lidar所在位置附近的所有地图特征点以及当前帧的点云特征点，后面的工作就是怎么把这两坨点匹配在一起啦！

if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100)

角点和平面点大于一定值才进入

kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);//构建kd-tree
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);

• 怎么构建的kd树？

for (int iterCount = 0; iterCount < 10; iterCount++)

非线性最小二乘

if (deltaR < 0.05 && deltaT < 0.05) {
	break;
}

迭代终止条件

以上对Lidar里程计的运动估计结果进行了优化

//将corner points按距离（比例尺缩小）归入相应的立方体
for (int i = 0; i < laserCloudCornerStackNum; i++) {
	//转移到世界坐标系
	pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

	//按50的比例尺缩小，四舍五入，偏移laserCloudCen*的量，计算索引
	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) {//只挑选-laserCloudCenWidth * 50.0 < point.x < laserCloudCenWidth * 50.0范围内的点，y和z同理
	 	//按照尺度放进不同的组，每个组的点数量各异
 		int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
 		laserCloudCornerArray[cubeInd]->push_back(pointSel);
	}
}

将当前帧扫描得到的特征角点封装在不同的cube中，并在地图数组中保存；平面点同理

mapFrameCount++;
//特征点汇总下采样，每隔五帧publish一次，从第一次开始
if (mapFrameCount >= mapFrameNum)

这之后的工作就是发布消息了