LeGO-LOAM相对LOAM的改进---地面分离的方法-算法原理+代码讲解

原理讲解

LeGO-LOAM中前端改进中很重要的⼀点就是充分利⽤了地⾯点，那⾸先⾃然是提取对地⾯点的提取

如上图，相邻的两个扫描线束的同⼀列打在地⾯上如AB点所⽰，他们的垂直⾼度差$$h=|z_0-z_1|$$ ⽔平距离差$$d=\sqrt{(}{x}_0-x_1{)}^2+(y_0-y_1{)}^2$$ 计算垂直⾼度差和⽔平⾼度差的⾓度
$$\theta =atan2(h,d)$$
理想情况下， 应该接近0,考虑到⼀⽅⾯激光雷达安装也⽆法做到绝对⽔平，另⼀⽅⾯，地⾯也不是绝
对⽔平，因此，这个⾓度会略微⼤于0,考虑到作者实际在草坪之类的场景下运动，因此这个值被设置成
10度。
实际上此种地⾯分离算法有⼀些简单，我们可以结合激光雷达安装⾼度等其他先验信息进⾏优化,避免将桌子这些当作地面点

代码讲解

1. 获取每个点的id

lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;

2. 根据id获取点云，并判断是否为空值，-1 证明是空点nanPoint

if (fullCloud->points[lowerInd].intensity == -1 ||
	fullCloud->points[upperInd].intensity == -1){
	groundMat.at<int8_t>(i,j) = -1;
continue;
}

3. 由上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1

diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

if (abs(angle - sensorMountAngle) <= 10){
    groundMat.at<int8_t>(i,j) = 1;
    groundMat.at<int8_t>(i+1,j) = 1;
}

4. 找到所有点中的地面点或者距离为FLT_MAX(rangeMat的初始值)的点，并将他们标记为-1 ，FLT_MAX的含义为无效点

    	if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
        	labelMat.at<int>(i,j) = -1;
    	}

5. 如果有节点订阅groundCloud，那么就需要把地面点发布出来，把点放到groundCloud队列中去

            if (groundMat.at<int8_t>(i,j) == 1)
                groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

代码注释

    void groundRemoval(){
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;

        for (size_t j = 0; j < Horizon_SCAN; ++j){
            // groundScanInd 是在 utility.h 文件中声明的线数，groundScanInd=7
            for (size_t i = 0; i < groundScanInd; ++i){

                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                // 初始化的时候用nanPoint.intensity = -1 填充
                // 都是-1 证明是空点nanPoint
                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    groundMat.at<int8_t>(i,j) = -1;
                continue;
                }

				// 由上下两线之间点的XYZ位置得到两线之间的俯仰角
				// 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
				// 否则，则不是地面点，进行后续操作
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

                if (abs(angle - sensorMountAngle) <= 10){
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }
            }
        }

		// 找到所有点中的地面点或者距离为FLT_MAX(rangeMat的初始值)的点，并将他们标记为-1
		// rangeMat[i][j]==FLT_MAX，代表的含义是什么？ 无效点
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
                    labelMat.at<int>(i,j) = -1;
                }
            }
        }

		// 如果有节点订阅groundCloud，那么就需要把地面点发布出来
		// 具体实现过程：把点放到groundCloud队列中去
        if (pubGroundCloud.getNumSubscribers() != 0){
            for (size_t i = 0; i <= groundScanInd; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    if (groundMat.at<int8_t>(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                }
            }
        }
    }