LeGO-LOAM 系列(3): featureAssociation node 分析

发布日期：2022-09-10 02:33:14 浏览次数：3 分类：技术文章

本文共 8332 字，大约阅读时间需要 27 分钟。

一、对应源文件

add_executable(featureAssociation src/featureAssociation.cpp)

二、原理

1. 点云特征提取

1.1 单个像素平滑度计算

选择同一行左右分别 $S/2$ 个点云

1.2 切分 sub-images

1800 x 16 分割为 6 个 300x16 的子图

1.3 每个子图上进行操作

(1). 平滑度阈值 $c_{th}$

平面点： $c<c_{th}$

边缘点： $c>c_{th}$

(2). 初始筛选特征点

每行选择前 $n\bar{F}_e$ 个 $c$ 最大的边缘点（不能是地面点云）

每行选择前 $n\bar{F}_p$ 个 $c$ 最小的边缘点（可以是地面点云）

1.4 二次特征点筛选

6 个子图的所有特征点汇总，得到： $\bar{F}_e$ 和 $\bar{F}_p$

从 $\bar{F}_e$ 中，每行选择 $nF_e$ 个 $c$ 最大的边缘点（不能是地面点云），组成集合： $F_e$

从 $\bar{F}_p$ 中，每行选择 $nF_p$ 个 $c$ 最小的边缘点（必须是地面点云），组成集合： $F_p$

1.5 结果可视化

c: $F_e$ 和 $F_p$

d: $\bar{F}_e$ 和 $\bar{F}_p$

2. 特征关联

2.1 关联原则

边缘特征：第 $t$ 帧的 $F_e$ 和第 $t-1$ 帧的 $\bar{F}_e$

平面特征：第 $t$ 帧的 $F_p$ 和第 $t-1$ 帧的 $\bar{F}_p$

关联方法和 LOAM 基本一致，除了下面的标签匹配

2.2 标签匹配

（1）对于平面点，仅仅选择第 $t-1$ 帧的 $\bar{F}_e$ 中标记为地面点的点云进行匹配

（2）对于边缘点，仅仅选择第 $t-1$ 帧的 $\bar{F}_p$ 中标记为非地面点的点云进行匹配

3. Two-step Odometry Optimization

3.1 残差项构造

边缘特征：点到直线的距离

平面特征：点到平面的距离

3.2 优化方法

Levenberg-Marquardt (L-M) 优化方法

（1）待优化变量：

相邻帧之间的相对变换

$T=(t_x, t_y, t_z, \theta_{roll}, \theta_{pitch}, \theta_{yaw})^T$

（2）step1

依据平面特征，优化

$[t_z, \theta_{roll}, \theta_{pitch}]^T$

（3）step2

依据边缘特征，优化

$[t_x, t_y, \theta_{yaw}]^T$ ，其中 step1 优化出来的 $[t_z, \theta_{roll}, \theta_{pitch}]^T$ 视为常量

注意：虽然 step1 也可以优化出 $[t_x, t_y, \theta_{yaw}]^T$ ，但是精度不高，所以 step2 里需要重新优化

3.3 优势

效率高

By using the proposed two-step optimization method, we observe that similar accuracy can be achieved while computation time is reduced by about 35%

三、ROS interface

1.1 subscribe topic

subLaserCloud = nh.subscribe
   
    ("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);subLaserCloudInfo = nh.subscribe
    
     ("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);subOutlierCloud = nh.subscribe
     
      ("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);subImu = nh.subscribe
      
       (imuTopic, 50, &FeatureAssociation::imuHandler, this);

1.2. publish topic

pubCornerPointsSharp = nh.advertise
   
    ("/laser_cloud_sharp", 1);pubCornerPointsLessSharp = nh.advertise
    
     ("/laser_cloud_less_sharp", 1);pubSurfPointsFlat = nh.advertise
     
      ("/laser_cloud_flat", 1);pubSurfPointsLessFlat = nh.advertise
      
       ("/laser_cloud_less_flat", 1);pubLaserCloudCornerLast = nh.advertise
       
        ("/laser_cloud_corner_last", 2);pubLaserCloudSurfLast = nh.advertise
        
         ("/laser_cloud_surf_last", 2);pubOutlierCloudLast = nh.advertise
         
          ("/outlier_cloud_last", 2);pubLaserOdometry = nh.advertise
          
            ("/laser_odom_to_init", 5);

四、算法 pipeline

1. adjustDistortion

1.1 作用

将所有点云的速度、位置变换至第一个点云时刻坐标系下

1.2 处理流程

for (int i = 0; i < cloudSize; i++) {    point.x = segmentedCloud->points[i].y;    point.y = segmentedCloud->points[i].z;    point.z = segmentedCloud->points[i].x;    // 调整ori大小，满足start
   
    
      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;    }    // intensity 小数部分保存相对时间    float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;    point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;    // imu rpy, 速度，位置 插值或者补偿    // 更新：imuRollCur, imuPitchCur, imuYawCur    //       imuVeloXCur, imuVeloYCur, imuVeloZCur    //       imuShiftXCur, imuShiftYCur, imuShiftZCur    if (i == 0) {        // 计算正余弦量        updateImuRollPitchYawStartSinCos()    } else {        // 速度变换至 i=0时刻        VeloToStartIMU();        // 点云位置变换到初始i=0时刻        TransformToStartIMU(&point);    }}

2. calculateSmoothness

2.1 作用

计算点云的平滑度

2.2 相关变量

// 存储平滑度cloudCurvature = new float[N_SCAN*Horizon_SCAN];  // 邻域点云被选择标记cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];   // 点云标签// 初始化为0，-1 代表 surfPointsFlat，<=0 代表surfPointsLessFlatScan// 2代表cornerPointsSharp，标记为1代表 cornerPointsLessSharpcloudLabel = new int[N_SCAN*Horizon_SCAN];    // 存储平滑度信息及其索引cloudSmoothness.resize(N_SCAN*Horizon_SCAN);

2.3 处理流程

void calculateSmoothness() {    int cloudSize = segmentedCloud->points.size();    // 取左右相邻5个点云    for (int i = 5; i < cloudSize - 5; i++) {            float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]                            + segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]                            + segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10                            + segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]                            + segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]                            + segInfo.segmentedCloudRange[i+5];                        cloudCurvature[i] = diffRange*diffRange;            cloudNeighborPicked[i] = 0;            cloudLabel[i] = 0;            cloudSmoothness[i].value = cloudCurvature[i];            cloudSmoothness[i].ind = i;        }    }

3. markOccludedPoints

3.1 作用

标记遮挡点云，后续不用于特征提取

3.2 处理流程

选择扫描角度近似，但是深度差值很大的点云

选择深度变化突变的点云

4. extractFeatures

4.1 作用

特征点提取，分为 6 个 subimages，逐行提取边缘点和平面点

4.2 处理流程

// 逐行提取特征点for (int i = 0; i < N_SCAN; i++) {    // 滤波前原始点云 lessflat    surfPointsLessFlatScan->clear();    for (int j = 0; j < 6; j++) {   // 分为 6 个 subimages        // sp, ep 代表第 i 行 第 j 个 subimag 点云起始、结束索引        int sp = (segInfo.startRingIndex[i] * (6 - j)    + segInfo.endRingIndex[i] * j) / 6;        int ep = (segInfo.startRingIndex[i] * (5 - j)    + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;        if (sp >= ep)            continue;        // 按照 smoothness 从小到大排序        std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());        // 选取边缘点（一定不能是地面点云）        int largestPickedNum = 0;        for (int k = ep; k >= sp; k--) {            int ind = cloudSmoothness[k].ind;            if (cloudNeighborPicked[ind] == 0 &&                cloudCurvature[ind] > edgeThreshold &&                segInfo.segmentedCloudGroundFlag[ind] == false) {                largestPickedNum++;                // 平滑度最大的 2 个点云属于 cornerPointSharp                // 平滑度最大的 20 个点云属于 cornerPointsLessSharp                if (largestPickedNum <= 2) {                         cloudLabel[ind] = 2;  // cornerPointSharp                    cornerPointsSharp->push_back(segmentedCloud->points[ind]);                    cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);                } else if (largestPickedNum <= 20) {                    cloudLabel[ind] = 1;   // cornerPointsLessSharp                    cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);                } else {                    break;                }                // 标记邻域点云，避免特征点集中                cloudNeighborPicked[ind] = 1;                for (int l = 1; l <= 5; l++) {                    ....                    cloudNeighborPicked[ind + l] = 1;                }                for (int l = -1; l >= -5; l--) {				    ....                    cloudNeighborPicked[ind + l] = 1;                }            }        }        // 选取平面点（一定是地面点云，和论文有出入？）        int smallestPickedNum = 0;        for (int k = sp; k <= ep; k++) {            int ind = cloudSmoothness[k].ind;            if (cloudNeighborPicked[ind] == 0 &&                cloudCurvature[ind] < surfThreshold &&                segInfo.segmentedCloudGroundFlag[ind] == true) {                // flat pts                cloudLabel[ind] = -1;                surfPointsFlat->push_back(segmentedCloud->points[ind]);                smallestPickedNum++;                if (smallestPickedNum >= 4) {                    break;                }                // 标记邻域点云，避免特征点集中                cloudNeighborPicked[ind] = 1;                for (int l = 1; l <= 5; l++) {                    ....                    cloudNeighborPicked[ind + l] = 1;                }                for (int l = -1; l >= -5; l--) {                    ....                     cloudNeighborPicked[ind + l] = 1;                }            }        }        // 保存 LessFlatPts        for (int k = sp; k <= ep; k++) {            if (cloudLabel[k] <= 0) {                surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);            }        }    }  // end for j    // surfPointsLessFlatScan 进行下采样    surfPointsLessFlatScanDS->clear();    downSizeFilter.setInputCloud(surfPointsLessFlatScan);    downSizeFilter.filter(*surfPointsLessFlatScanDS);    *surfPointsLessFlat += *surfPointsLessFlatScanDS;}  // end for i

5. publishCloud

结果可视化，

发布点云：cornerPointsSharp，cornerPointsLessSharp，surfPointsFlat，surfPointsLessFlat

6. updateInitialGuess

todo

7. updateTransformation

todo

8. integrateTransformation

todo

9. 结果可视化

publishOdometry

publishCloudsLast

五、参考

转载地址：https://blog.csdn.net/weixin_40072161/article/details/123979198 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！

上一篇：lego-loam运行常见错误

下一篇：legend3---lavarel常用操作代码2

发表评论

关于作者

喝酒易醉，品茶养心，人生如梦，品茶悟道，何以解忧？唯有杜康！

-- 愿君每日到此一游！

一、对应源文件

二、原理

1. 点云特征提取

1.1 单个像素平滑度计算

1.2 切分 sub-images

1.3 每个子图上进行操作

1.4 二次特征点筛选

1.5 结果可视化

2. 特征关联

2.1 关联原则

2.2 标签匹配

3. Two-step Odometry Optimization

3.1 残差项构造

3.2 优化方法

3.3 优势

三、ROS interface

1.1 subscribe topic

1.2. publish topic

四、算法 pipeline

1. adjustDistortion

1.1 作用

1.2 处理流程

2. calculateSmoothness

2.1 作用

2.2 相关变量

2.3 处理流程

3. markOccludedPoints

3.1 作用

3.2 处理流程

4. extractFeatures

4.1 作用

4.2 处理流程

5. publishCloud

6. updateInitialGuess

7. updateTransformation

8. integrateTransformation

9. 结果可视化

五、参考

发表评论

最新留言

关于作者

推荐文章