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

原理讲解

LeGO-LOAM中前端改进中很重要的⼀点就是充分利⽤了地⾯点,那⾸先⾃然是提取对地⾯点的提取
在这里插入图片描述
如上图,相邻的两个扫描线束的同⼀列打在地⾯上如AB点所⽰,他们的垂直⾼度差 h = ∣ z 0 − z 1 ∣ h=|z_{0}-z_{1}| h=z0z1 ⽔平距离差 d = ( x 0 − x 1 ) 2 + ( y 0 − y 1 ) 2 d = \sqrt (x_{0}-x_{1})^{2}+(y_{0}-y_{1})^{2} d=( x0x1)2+(y0y1)2 计算垂直⾼度差和⽔平⾼度差的⾓度
θ = a t a n 2 ( h , d ) θ = atan2(h,d) θ=atan2(h,d)
理想情况下, 应该接近0,考虑到⼀⽅⾯激光雷达安装也⽆法做到绝对⽔平,另⼀⽅⾯,地⾯也不是绝
对⽔平,因此,这个⾓度会略微⼤于0,考虑到作者实际在草坪之类的场景下运动,因此这个值被设置成
10度。
实际上此种地⾯分离算法有⼀些简单,我们可以结合激光雷达安装⾼度等其他先验信息进⾏优化,避免将桌子这些当作地面点

代码讲解

  1. 获取每个点的id
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;
  1. 根据id获取点云,并判断是否为空值,-1 证明是空点nanPoint
if (fullCloud->points[lowerInd].intensity == -1 ||
	fullCloud->points[upperInd].intensity == -1){
	groundMat.at<int8_t>(i,j) = -1;
continue;
}
  1. 由上下两线之间点的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;
}
  1. 找到所有点中的地面点或者距离为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;
    	}
  1. 如果有节点订阅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]);
                }
            }
        }
    }
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Rhys___

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值