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

scofield_ly

已于 2022-05-04 23:03:26 修改

阅读量478

点赞数

分类专栏： # lidar_slam 文章标签： slam

于 2022-04-05 22:33:52 首次发布

本文链接：https://blog.csdn.net/weixin_40072161/article/details/123979198

版权

lidar_slam 专栏收录该内容

8 篇文章 8 订阅

订阅专栏

一、对应源文件

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<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);

1.2. publish topic

pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);

pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/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<ori<end
    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;
    }

    // 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代表 cornerPointsLessSharp
cloudLabel = 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