从零开始细扣LOAM系列第二篇--点云处理一：去畸变和去离群点

最新推荐文章于 2024-04-24 15:15:48 发布

墨色玫瑰

最新推荐文章于 2024-04-24 15:15:48 发布

阅读量527

点赞数 1

分类专栏：细扣LOAM系列文章标签： c++ 机器人自动驾驶

本文链接：https://blog.csdn.net/weixin_43959160/article/details/132128028

版权

细扣LOAM系列专栏收录该内容

2 篇文章 1 订阅

订阅专栏

前一篇文章简单介绍了LOAM的总体框架，本系列准备参考aloam代码结构分节探索，本文紧接着开始对算法进行位姿估计前的点云预处理进行详细探索，遵循先根据论文打通理论理解，然后详细解读源码的模式进行学习。

点云处理部分

首先需要明确的是，直接得到的点云是存在许多干扰的，LOAM论文中主要处理的部分是运动畸变、大入射角离群点和遮挡离群点。下面分别介绍这几种干扰形成原因和LOAM中的处理方案。

运动畸变

形成原因及示例

对于运动畸变，简单地说就是，雷达扫描周期内，雷达本身也是在运动中的，所以它的每个点是在不同参考系下测量得到的，但是却当成了同一个参考系来使用，这就导致得到的点云和实际的环境总有一点偏差。以一个平面运动时对一个圆形环境的测量为例，若一个点云采集周期内雷达发生平移运动，则测得的点云如下：
distortion

其中，黄色圆圈表示真实的环境，当激光雷达从黄色圆圈圆心向右平移运动，沿蓝色直线轨迹，对黄色圆圈的测量(测量激光束逆时针旋转)结果将会呈现蓝色螺旋线的样子。以平移运动的最后位置为例，即蓝色轨迹的右端，此时为激光雷达一圈测量周期的结束时间，激光雷达此时测量到的距离是蓝色轨迹右端到黄色圆圈右侧的距离，然后激光雷达认为本次测量的所有数据均在测量周期开始的位置测量所得，即蓝色轨迹的左端，错误的认为蓝色轨迹左端点距离黄色圆圈右端的距离是在蓝色轨迹右端所测量到的距离，从而形成了运动畸变。如果雷达本体进行旋转的话，这种畸变还会更大。这种畸变回给运动估计带来极大的误差，因此在匹配之前需要对其进行消除。

LOAM中去运动畸变策略

通过对运动畸变的阐述，可以想到一个非常直接的方案就是想办法算出或者估计出每个点采集的时刻雷达所处的位置，然后将一帧内所有点变换到统一坐标系下，这样的话完整的一帧点云看起来就不会有明显的运行畸变了。事实上，LOAM也确实是采用了这种策略，当帧间运动估计完成后，用该估计进行时间插值，获得每个点所在采集时刻的雷达位姿，从而将点云都转换到统一坐标系下，如下图所示，
Screenshot from 2023-08-05 16-47-08

其中 $P_{k-1}$ 为 $t_{k-1}$ 时刻的点云帧，采集周期为 $t_{k-1}$ 到 $t_{k}$ 之间的时间段， $\bar{P}_{k-1}$ 即为转换到帧结束时刻下的去畸变点云，用于下一帧运动估计时最临近特征点查找。在论文中所述，每个点采集时刻的位姿估计采用线性插值实现，loam_velodyne代码中也是采取这种插值，而ALOAM源码中采用球面线性插值的方式，不同的是一个采用高频IMU插值，一个直接使用上一帧估计结果插值。
以ALOAM中插值方式为例，设 $T_{k-1}$ 表示第k-1帧的帧间运动，在对第k帧点云去畸变时，默认在帧周期内，激光雷达的运动是均匀的， $T_{k-1,i}$ 表示第k-1帧点云中的第i个点采集时刻的雷达位姿估计， $t_k,t_{k-1}$ 分别表示k-1帧点云周期的起始时间和结束时间， $t_{k-1, i}$ 表示 k-1 帧点云中第i 个点的时刻，其插值方式可以不严谨的表述为：
$T_{k-1,i} = \frac{t_{k-1,i} - t_{k-1}}{t_{k} - t_{k-1}} \cdot T_{k-1}$
对于k-1帧中的每个点 $p_i$ 都可以采用 $\bar{p}_i = T_{k-1,i}\cdot p_i$ 来变换到第k-1帧的起始时刻。

需要强调的是，这里描述的去畸变处理是以获得了帧间运动估计为前提的，指的是将当前帧的前一帧点云转到前一帧的结束时刻，也就是当前帧的起始时刻吗，这样的话方便在估计当前帧运动的时候能够在一个没有畸变的参考点云中进行最邻近特征点查找。

下面我们看ALOAM中关于这部分的源码的具体实现，其中转换到帧结束位置的时候，之所以先转到起始坐标系再转到帧结束坐标系是为了开发方便。当一阵点云的帧间运动估计完成之后，q_last_curr和t_last_curr均被解算出来，然后调用TransformToEnd函数就会根据当前帧的位姿变换进行点的转换。首先计算点采集时刻在帧周期中的位置比例，然后根据该比例进行球面线性插值，再使用插值结果变换点的位置实现畸变去除。

// para_q 和 para_t 为使用ceres求解残差方程的时候输出的结果，q_last_curr和t_last_curr分别是他们的映射，在这里不深入讨论，我们可以直接认为它是类似引用的东西(并非引用)
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

void TransformToStart(PointType const *const pi, PointType *const po)
{
    // 插值位置，即当前点采集时刻在整个帧周期中处于什么位置
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
    else
        s = 1.0;

    // 球面线性插值
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    Eigen::Vector3d t_point_last = s * t_last_curr;
    // 点坐标变换
    Eigen::Vector3d point(pi->x, pi->y, pi->z);
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;

    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;
}

void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // 先转到起始坐标系
    pcl::PointXYZI un_point_tmp;
    TransformToStart(pi, &un_point_tmp);
    // 然后利用帧间变换转换到帧结束时刻坐标系
    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);

    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //移除时间信息
    po->intensity = int(pi->intensity);
}

需要强调的是，并不是每次调用TransformToStart函数采用的都是当前帧的位姿变换，这和调用的位置还有关系，只有在当前帧帧间位姿变换估计刚完成，下一次估计未开始的时候调用才属于这种情况。另一种情况请接着往下看。

运动估计中对运动畸变的处理

前面所说的去畸变方式并不能解决当前帧在运动估计过程中遇到的畸变问题，因此运动估计时也需要考虑该问题，快速能想到的一个解决办法是先粗略估计当前帧的运动情况，比如使用运动模型推算(如匀速运动模型或者匀加速运动模型等)或者采用IMU预积分量作为该估计，甚至直接在初始时认为运动量为零，然后在采用与前文相同的方案去畸变后变换到帧起始坐标下，然后在优化该粗略估计的运动量。根据LOAM论文所示，其采取的策略为利用时间插值直接计算点采集时刻位姿和帧间变换的关系，然后在构建残差的时候先将点转换到帧起始时刻坐标系，再计算残差，对残差进行优化得到当前帧的运动。说白了去畸变的基本策略还是没有变，只不过将这种畸变直接从构建残差的时候就开始考虑，将它融入到优化的目标方程中了。在ALOAM的源码实现中，采取了使用上一帧的位姿变换来粗略估计当前帧的方式。

其中转换将点转换到帧起始坐标系的函数与上文部分为同一个，就不重复贴出来了，，正是因为这种情况下调用TransformToStart是当前帧的帧间位姿变换尚未计算，存储的还是上一帧的位子姿变换，导致其使用的位姿变换为粗略估计。调用位置如下：

...
// 角特征点处理，求角特征距离上一帧中最近直线的的最小距离
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
    // 当前点转换到点云初始点坐标系下
    TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
    // 寻找上一帧点云中的最近角特征点
    kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
    ...
}

// 平面特征处理，求面特征距离上一帧点云中最近平面的最小距离
for (int i = 0; i < surfPointsFlatNum; ++i)
{
    // 当前点转换到点云初始点坐标系下
    TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
    // 寻找上一帧点云中的最近平面点
    kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
    ...
}

// 解算帧间位姿变换，计算出para_q 和 para_t，即q_last_curr和t_last_curr所取到的值
...

// 将待使用的准角特征点、准面点和全部点云均转换到帧结束时刻坐标系下  
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++)
{
    TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}

int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++)
{
    TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}

int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
    TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}

而LOAM源码中对运动畸变的处理则比ALOAM复杂得多，通过使用同步高频IMU测量来实现更精细的去畸变畸变，这部分内容后面单独总结。
注：准特征点概念讲到特征点提取时介绍

离群点处理

形成原因及示例

LOAM中处理的离群点指的是由于测量激光束以大入射角集中平面或者扫描线经过遮挡处导致的大误差点，这些点不适合作为特征点使用，会给匹配引入较大误差。下面以论文中的图片为例介绍形成原因。

Screenshot from 2023-08-05 20-29-24

其中(a)表示的就是大入射角离群点的情况，当激光束扫描到某一平面时，如果该平面和激光束的夹角是一个极小值，也就是说其入射角很大，接近90°，则一般认为这类点是不可靠的。

可以从两方面理解，其一可以从激光雷达的原理上理解（个人猜测，实际原因和这个关系应该不是很大，但是想到了也说一下），无论激光雷达发出的激光点有多细，但总归是有一定面积的，不可能是无穷小的一个点。当这一束激光以很大的入射角击中一个平面时，它在平面上的受光面将会因为角度原因被拉长，这时候就不能很好的保证该激光束所测量的距离的准确性，因此这种点一定要去掉。
如下图所示，当激光束垂直集中平面时，受光面很小，基本和激光光束大小一致
Screenshot from 2023-08-05 20-46-07
一旦激光束和平面的夹角变得很小了，那么受光面将会被拉长许多倍：

第二个原因，假设不考虑激光雷达测量这种点的时候的误差，当特征提取处理到这部分点时，由于激光束和平面夹角小，相邻两次扫描之间虽然角度很小，但是他们得到的两个点之间的距离还是会较远，这时候很容易被误识别成角特征点，这时候会与正常角度的测量产生冲突，导致残差优化错误，从而给帧间位姿估计引入不必要的误差。

因此，(a)中B点这种点通常不会被选择作为特征点。

下面看第二种离群点，也就是论文途中(b)所展示的C点的情况，严格来说不能算离群点，只不过是排除一下对这类点的误识别情况。

如果不做特殊处理的话，在这里C和D两个相邻点距离非常大，远大于各自和其他临近点的距离，都会被认为是角特征点。但是实际上其中C点产生的原因是C点所在平面的剩余部分被D所在区域所遮挡，导致平面的另一部分无法被激光雷达扫描到，从而呈现出类似角特征点的特点。但是这类点也很好区分，通常都是被遮挡平面的部分，所以对于边缘点提取过程来说，如果遇到和C、D点这种情况，其中较远的那个点就是非角特征点，支取其中距离近的点作为角特征点即可。