loam的学习记录2（laserOdometry节点）

laserOdometry

main函数
该main函数中除了订阅消息和发布消息外，还有一个while循环，且该循环较长，阅读起来并不友好。

ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_sharp", 2, laserCloudSharpHandler);

ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);

ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                  ("/laser_cloud_flat", 2, laserCloudFlatHandler);

ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                      ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);

ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> 
                                     ("/velodyne_cloud_2", 2, laserCloudFullResHandler);

ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> 
                            ("/imu_trans", 5, imuTransHandler);

订阅scanRegistration节点发布的消息

ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_corner_last", 2);

ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
                                      ("/laser_cloud_surf_last", 2);

ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> 
                                      ("/velodyne_cloud_3", 2);

ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);

发布消息

while (status) {
	...
}

如果ros的状态为OK则一直运行。

接下来的操作都在while循环中

if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 
        newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
        fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005)

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

if (!systemInited) {
	//将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用
	pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
	cornerPointsLessSharp = laserCloudCornerLast;
	laserCloudCornerLast = laserCloudTemp;
	
	//将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用
	laserCloudTemp = surfPointsLessFlat;
	surfPointsLessFlat = laserCloudSurfLast;
	laserCloudSurfLast = laserCloudTemp;
	
	//使用上一帧的特征点构建kd-tree
	kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合
	kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合
	
	//将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMapping
	sensor_msgs::PointCloud2 laserCloudCornerLast2;
	pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
	laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
	laserCloudCornerLast2.header.frame_id = "/camera";
	pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
	
	sensor_msgs::PointCloud2 laserCloudSurfLast2;
	pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
	laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
	laserCloudSurfLast2.header.frame_id = "/camera";
	pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
	
	//记住原点的翻滚角和俯仰角
	transformSum[0] += imuPitchStart;
	transformSum[2] += imuRollStart;
	
	systemInited = true;
	continue;
}

初始化部分：
cornerPointsLessSharp保存当前时刻的角点用于下一时刻，laserCloudCornerLast保存上一时刻的角点用于当前时刻。旋转角不存储偏航角是因为雷达扫描一圈，所以偏航角不管是什么角度都可以。
transformSum：当前帧相对于第一帧的状态转移量（包括旋转角+位移）

if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100)

只有足够多的角点和平面点才开始处理

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

这个循环就是非线性最小二乘进行优化，在循环中处理角点和特征点并构建相应的雅克比矩阵
transform：当前帧相对上一帧的状态转移量（包括旋转角+位移）

if (iterCount % 5 == 0)

每迭代5次查找最近点和次近点
对于特征线：
利用KD树找点i在t时刻点云中最近的一点j，并在j周围（上下几条线的范围内）找次近点l，把（j，l）称为点i在t时刻点云中的对应。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

在点pointSel（即论文中i）找最近点j，下标存在pointSearchInd中
函数nearestKSearch参数解释：

* \param[in] point a given \a valid (i.e., finite) query point
* \param[in] k the number of neighbors to search for
* \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
* \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
* a priori!)

closestPointInd = pointSearchInd[0];

closestPointInd保存最近点j的下标

for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {//向scanID增大的方向查找
	if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {//非相邻线
		break;
	}

	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 (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
		if (pointSqDis < minPointSqDis2) {//距离更近，要小于初始值5米
	  		//更新最小距离与点序
	  		minPointSqDis2 = pointSqDis;
	 		minPointInd2 = j;
		}
	}
}

在j周围找找次近点l，minPointInd2保存l的下标。

pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距离点，-1表示未找到满足的点
pointSearchCornerInd2[i] = minPointInd2;//另一个最近的，-1表示未找到满足的点

保存每次循环找到的j、l的下标

if (pointSearchCornerInd2[i] >= 0)

判断找到了点l，之所以没有判断点j是否找到是因为点l是根据点j找到的，既然l已经找到则点j必定找到

找到点i对应的特征线(j, l)后，计算点到直线的距离：

tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

//选择的特征点记为O，kd-tree最近距离点记为A，另一个最近距离点记为B
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = tripod1.x;
float y1 = tripod1.y;
float z1 = tripod1.z;
float x2 = tripod2.x;
float y2 = tripod2.y;
float z2 = tripod2.z;

分子的计算：

//向量OA OB的向量积(即叉乘)为：
//|  i      j      k  |
//|x0-x1  y0-y1  z0-z1|
//|x0-x2  y0-y2  z0-z2|
//模为：
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
           * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
           + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
           * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
           + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
           * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

分母的计算：

//两个最近距离点之间的距离，即向量AB的模
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

点到线的距离，d = |向量OA 叉乘 向量OB|/|AB|

float ld2 = a012 / l12;

if (s > 0.1 && ld2 != 0) {//只保留权重大的，也即距离比较小的点，同时也舍弃距离为零的
	laserCloudOri->push_back(cornerPointsSharp->points[i]);
	coeffSel->push_back(coeff);
}

将该特征点插入laserCloudOri。s是阻尼因子（权重）

特征面相比特征线多加了一个点m。在j这个线扫上找到另一点l，再在j的相邻线扫上找一点m。

代码中对应点到面的距离中对于变量pd我不能理解。

float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);

float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

点到面的距离由向量OA与法向量的点积除以法向量的模表示。

• 但是代码中还加了pd这个变量，是为什么？

至此，Jaccobian矩阵的三个元素都保存在coeffSel中

之后构建雅克比矩阵，求解增量方程Hx=g（其中，x因为公式编辑原因这里表示的是增量）
H:arx, ary, arz, atx, aty, atz; g:d2

matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = atx;
matA.at<float>(i, 4) = aty;
matA.at<float>(i, 5) = atz;
matB.at<float>(i, 0) = -0.05 * d2;

cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;

H = matAtA，g = matAtB。

cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

用QR分解得打增量matX

//x_(k+1) = x_k + delta_x
transform[0] += matX.at<float>(0, 0);
transform[1] += matX.at<float>(1, 0);
transform[2] += matX.at<float>(2, 0);
transform[3] += matX.at<float>(3, 0);
transform[4] += matX.at<float>(4, 0);
transform[5] += matX.at<float>(5, 0);

给自变量x增加增量。

float deltaR = sqrt(
            pow(rad2deg(matX.at<float>(0, 0)), 2) +
            pow(rad2deg(matX.at<float>(1, 0)), 2) +
            pow(rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
            pow(matX.at<float>(3, 0) * 100, 2) +
            pow(matX.at<float>(4, 0) * 100, 2) +
            pow(matX.at<float>(5, 0) * 100, 2));

if (deltaR < 0.1 && deltaT < 0.1) {//迭代终止条件
	break;
}

如果增量小于一定的值，增停止迭代。

接下来是坐标变换。

float rx, ry, rz, tx, ty, tz;
//求相对于原点的旋转量,垂直方向上1.05倍修正?
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                  	-transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);

float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 
        	- sin(rz) * (transform[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 
        	+ cos(rz) * (transform[4] - imuShiftFromStartY);
float z1 = transform[5] * 1.05 - imuShiftFromStartZ;

float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;

//求相对于原点的平移量
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

//根据IMU修正旋转量
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                 	imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

//得到世界坐标系下的转移矩阵
transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;

这一部分我看不懂，就不多做解释免得误导大家了。