lego-loam阅读理解笔记二

最新推荐文章于 2023-09-11 21:09:49 发布

高飞007

最新推荐文章于 2023-09-11 21:09:49 发布

阅读量2.7k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：机器人定位导航SLAM 文章标签： 3D激光SLAM 机器人 3D点云定位导航

本文链接：https://blog.csdn.net/qingdu007/article/details/108307913

本文是lego-loam阅读理解笔记的第二部分，主要解析了featureAssociation.cpp中的关键函数，包括runFeatureAssociation()的激光点云坐标变换与畸变补偿，adjustDistortion()的IMU投影与时间对齐，calculateSmoothness()的平滑度计算，markOccludedPoints()的不可靠点标记，以及extractFeatures()的特征点提取过程。通过对这些函数的解读，深入理解lego-loam在3D激光SLAM中的点云处理策略。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言

一、runFeatureAssociation()解读

二、adjustDistortion()解读

三、calculateSmoothness()解读

四、markOccludedPoints()解读

五、extractFeatures()理解

六、剩下函数简要注释

前言

featureAssociation.cpp主要实现点云相对于IMU的坐标变换，特征提取，特征匹配等功能

一、runFeatureAssociation()解读

该函数是整个cpp的主要功能函数

void runFeatureAssociation()
    {
        //没有新的数据来，就不处理
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){
            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }else{
            return;
        }
        // 特征提取
        adjustDistortion(); //主要进行的处理是将点云数据进行坐标变换，进行插补等工作
        calculateSmoothness(); // 按照论文中的方法计算曲率，并保存结果
        markOccludedPoints();// 标记瑕点,loam论文中:1.平行scan的点(可能看不见);2.会被遮挡的点
        extractFeatures();//根据曲率对点云分类,然后分别保存到cornerPointsSharp等等队列
        publishCloud(); // 发布cornerPointsSharp等4种类型的点云数据便于可视化
		//Feature Association
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }
        updateInitialGuess(); // 预测位姿
        updateTransformation(); // 更新变换
        integrateTransformation(); // 积分总变换
        publishOdometry();
        publishCloudsLast(); // cloud to mapOptimization
    }
};

二、adjustDistortion()解读

该函数实现点的水平夹角在π～3π之间，一帧点云空间时间都统一到IMU坐标系中。也就是激光点云的畸变补偿。激光雷达在不断的扫描时，也在运动，所以一帧从头扫到尾，激光点坐标并没有在同一坐标系中，如图1所示。

图 1

所以，要计算每一点激光点相对于起始点的时间间隔，图1中的b点相对起始点a点的时间间隔为 $△t$ △t。本论文是把一帧激光点云统一到IMU坐标系中

//点归为到π～3π
float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
ori -= 2 * M_PI;

if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori += 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
ori -= 2 * M_PI;
}

计算每一个点相对于起始点的间隔数，其中segInfo.orientationDiff = 377°，这个值在阅读理解笔记一中有提到过

float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff; //计算每一个点相对于起始点的间隔数

计算每一个点相对于起始点的时间。scanPeriod是激光雷达水平相邻两束激光的间隔时间

float pointTime = relTime * scanPeriod; //计算每一个点相对于起始点的时间

                 if (i == 0) {  //某帧激光的第一个点
                    imuRollStart = imuRollCur;
                    imuPitchStart = imuPitchCur;
                    imuYawStart = imuYawCur;
                    imuVeloXStart = imuVeloXCur;
                    imuVeloYStart = imuVeloYCur;
                    imuVeloZStart = imuVeloZCur;
                    imuShiftXStart = imuShiftXCur;
                    imuShiftYStart = imuShiftYCur;
                    imuShiftZStart = imuShiftZCur;
                    if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                    }else{
                        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                        float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 
                                                         / (imuTime[imuPointerFront] - imuTime

最低0.47元/天解锁文章