loam的学习记录1（scanRegistration节点）

LOAM激光雷达里程计详解

最新推荐文章于 2022-11-03 23:58:20 发布

原创最新推荐文章于 2022-11-03 23:58:20 发布 · 1.9k 阅读

13 ·

CC 4.0 BY-SA版权

文章标签：

#loam #slam

loam 专栏收录该内容

6 篇文章

订阅专栏

本文深入解析LOAM激光雷达里程计的工作原理，重点介绍其核心算法：点云预处理、曲率计算、瑕点排除及特征点分类。通过详细步骤解释如何从原始点云数据中提取有用信息，实现环境建图。

main函数
main函数中就是订阅了2个话题和发布了6个话题。
接下来我主要讲解两个回调函数laserCloudHandler和imuHandler。

回调函数laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)：

对接收到的点云进行预处理；
计算曲率；
排除瑕点。
对角点和平面点进行分类

1：首先说一下对接收到的点云进行预处理
该函数接受的参数是sensor_msgs::PointCloud2类型的智能指针。

pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

将sensor_msg类型转换成pcl的点云形式。

//lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                 		laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

计算点云的起始和终止角度。

std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);

laserCloudScans保存具有不同特征点的点云。

for (int i = 0; i < cloudSize; i++)

遍历所有的点云，计算每个点的角度。

在循环中：

//计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;
//仰角四舍五入(加减0.5截断效果等于四舍五入)
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 
if (roundedAngle > 0){
  	scanID = roundedAngle;
}
else {
  	scanID = roundedAngle + (N_SCANS - 1);
}

计算laserCloudScans所需要的下标scanID。

//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）
float relTime = (ori - startOri) / (endOri - startOri);
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
point.intensity = scanID + scanPeriod * relTime;

if (imuPointerLast >= 0)

如果接收到imu数据，使用imu矫正点云畸变。

laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器

pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {//将所有的点按照线号从小到大放入一个容器
	*laserCloud += laserCloudScans[i];
}

将所有的线扫的点云放入大的点云中。

2：计算曲率

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;
    //曲率计算
    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
	...
}

曲率用该点的前后各5个点来近似表达

3：排除瑕点

for (int i = 5; i < cloudSize - 6; i++)

该循环中排除瑕点，循环中的代码如下：

float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
//计算有效曲率点与后一个点之间的距离平方和
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;

if (diff > 0.1) {//前提:两个点之间距离要大于0.1
	//点的深度
	float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 
	laserCloud->points[i].y * laserCloud->points[i].y +
	laserCloud->points[i].z * laserCloud->points[i].z);
	
	//后一个点的深度
	float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 
	laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
	laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
	
	if (depth1 > depth2) {
	  diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
	  diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
	  diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
	
	  //边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上
	  if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点
	      //该点及前面五个点（大致都在斜面上）全部置为筛选过
	    cloudNeighborPicked[i - 5] = 1;
	    cloudNeighborPicked[i - 4] = 1;
	    cloudNeighborPicked[i - 3] = 1;
	    cloudNeighborPicked[i - 2] = 1;
	    cloudNeighborPicked[i - 1] = 1;
	    cloudNeighborPicked[i] = 1;
	  }
	} else {
	  diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
	  diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
	  diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
	
	  if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
	    cloudNeighborPicked[i + 1] = 1;
	    cloudNeighborPicked[i + 2] = 1;
	    cloudNeighborPicked[i + 3] = 1;
	    cloudNeighborPicked[i + 4] = 1;
	    cloudNeighborPicked[i + 5] = 1;
	    cloudNeighborPicked[i + 6] = 1;
	  }
	}
}

这是对论文中瑕点b的判定标准。
为了表达的更加清楚，我将雷达扫描的第i个点记为A，第i+1个点记为B，diff>0.1 表明AB两点有一定的距离，depth1为A点的深度，depth2为B点的深度。
在depth1 > depth2的情况下，对laserCloud->points[i].x * depth2 / depth1的解读我拆成两部分：laserCloud->points[i].x/ depth1表示先将OA（X轴方向）除以depth1即把OA变换为OA方向上的单位向量，再乘以depth2表示长度变成与OB长度相等，重新变换长度后我将该点记为A’。则diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1表示A‘与B在X轴方向的距离。
sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ)表示A’B的距离（记为dAB)，dAB/depth2 < 0.1 表示角A’OB很小（小于一定的阈值），将该点及前面五个点剔除。
depth1 < depth2的情况与之类似，我这里就不赘述了。
作图直观地感受下：
在这里插入图片描述

float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
//与前一个点的距离平方和
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;

//点深度的平方和
float dis = laserCloud->points[i].x * laserCloud->points[i].x
          + laserCloud->points[i].y * laserCloud->points[i].y
          + laserCloud->points[i].z * laserCloud->points[i].z;

//与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
	cloudNeighborPicked[i] = 1;
}

这是对论文中瑕点b的判定标准。我这里就直接上图说明了：
在这里插入图片描述

4：对角点和平面点进行分类

for (int i = 0; i < N_SCANS; i++) {
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    //将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点
    for (int j = 0; j < 6; j++) {
    	...
	}
}

//按曲率从小到大冒泡排序
for (int k = sp + 1; k <= ep; k++) {
	for (int l = k; l >= sp + 1; l--) {
		//如果后面曲率点大于前面，则交换
		if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
			int temp = cloudSortInd[l - 1];
			cloudSortInd[l - 1] = cloudSortInd[l];
			cloudSortInd[l] = temp;
		}
	}
}

for (int k = ep; k >= sp; k--){
	int ind = cloudSortInd[k];
	...
}

挑选每个分段的曲率很大和比较大的点（找角点）

//如果曲率大的点，曲率的确比较大，并且未被筛选过滤掉
if (cloudNeighborPicked[ind] == 0 &&
     cloudCurvature[ind] > 0.1)

if (largestPickedNum <= 2) {//挑选曲率最大的前2个点放入sharp点集合
	cloudLabel[ind] = 2;//2代表点曲率很大
	cornerPointsSharp.push_back(laserCloud->points[ind]);
	cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else if (largestPickedNum <= 20) {//挑选曲率最大的前20个点放入less sharp点集合
	cloudLabel[ind] = 1;//1代表点曲率比较尖锐
	cornerPointsLessSharp.push_back(laserCloud->points[ind]);
} else {
	break;
}
cloudNeighborPicked[ind] = 1;//筛选标志置位

//将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀
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;
}

找平面点同理。

sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);

将pcl点云信息转换成ros的sensor_msg形式，pubLaserCloud发布点云的信息。其余的点云信息、imu信息的发布同理。

回调函数imuHandler

float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

将IMU坐标系下的加速度转换到世界坐标系下，同时减去重力的影响。

imuPointerLast = (imuPointerLast + 1) % imuQueLength;

循环移位效果，形成环形数组

imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;

对IMU信息（时间戳、欧拉角、加速度）赋值。