激光雷达点云数据内部空点补全

最新推荐文章于 2025-07-15 03:35:50 发布

原创

最新推荐文章于 2025-07-15 03:35:50 发布 · 9.7k 阅读

39 ·

CC 4.0 BY-SA版权

文章标签：

#PCL #ROS

本文介绍了如何使用PCL库补全激光雷达点云数据内部的空点，主要针对有序点云数据，通过线性差值算法在同一线激光扫描的点中寻找最近点进行补全，特殊情况处理包括角度的对偶性和边界处理。

title: 激光雷达点云数据内部空点补全
date: 2018-03-28 22:55:54
categories: ROS
tags:
- ROS
- PCL

欢迎访问我的个人博客：zengzeyu.com

前言

点云数据区别于图像数据，不管是二维图像还是三维图像，图像数据都充满整个区域，二维图像中每个像素点都有值，灰度值、RGB值等；三维图像中有体数据（Voxel），根据光线投影算法等，可计算出每个体数据对应值，从而显示于显示器中。点云数据由于其扫描生成数据过程的特性，就决定了其在数据方面与图像数据不同，以机械式激光雷达为例，当出现以下情况时，该位置扫描生成的点云数据不存在（即为NAN点）：

激光发射器发射出去的激光未收到返回光束
激光接收器接收到的返回激光强度超出阈值范围

另外，在数据处理阶段，可根据需要对部分数据进行滤波处理，赋值为NAN点，也能造成该出点云缺失情况。

PCL库中有自带判断点云数据是否含有NAN点的函数： pcl::PointCloud<pcl::PointXYZ>::is_dense()，以及过滤NAN点函数：pcl::removeNaNFromPointCloud()。

本文旨在补全内部空洞点，而不是去掉空洞点。

点云NAN点补全

本文补全原则基于有序点云（organised）进行处理，非有序点云无法进行处理（unorganised）。
以自动驾驶中使用的机械式激光雷达速腾聚创16线激光雷达RS-LiDAR-16为例，其生成的有序点云（organised）点云尺寸为 16 x 2016： 16为激光线数，2016为每一线激光绕中心一周旋转储存的点个数，因此有 16 x 2016 = 32256 个点，而实际得到的点数据基本不可能是32256，必有缺失。

补全规则

在每一线激光扫描得到一行点数据中，查找与NAN点最近的点进行补全，如果本行数据全部为NAN（虽然不可能发生），则此行可删除，调整点云尺寸。
该规则基于的原则：在同一线激光扫描得到的点中，由于水平方向数据分辨率很高，所以一行数据中每个点与其邻域内点相似。

算法设计

算法思路

为了简化叙述，本文将一线激光扫描得到数据缩小为 360 个点，即一帧点云尺寸变为 16x360。
以一线激光扫描数据为例，默认激光旋转方向为顺时针方向，采用线性差值方法进行补全，由于一线激光扫描一圈得到的数据在360°内任意位置都是对偶的，所以在空点附近查找两边非空点，用其值进行补全，具体参考下图示意。
图上半部分为一线激光雷达扫描得到数据鸟瞰图，其中黑色方块代表非空点，白色方块代表空点；下半部分为点距离图，根据线性插值方法可以补全非空点。非空点距离计算采用极坐标方式，首先得到线性插值得到的range，再使用当前转角转换到笛卡尔坐标系下，可得到其 x， y 坐标值，z 坐标值也采用同样的插值方法计算。

其中一个特殊情况：转角为 0° 与转角为 360° 是等效的，在查找过程中，当转角顺时针查找到 360 °时则置为 0°，当转角逆时针查找到 0° 时则置为 360°。

算法流程

根据以上思路，设计算法如下:

    def fix_nan_point(self, in_cloud):
        #fix edeg nan point 1st
        in_cloud = self.fix_left_edge_nan_point( in_cloud )
        in_cloud = self.fix_right_edge_nan_point( in_cloud )
        #fix centrol nan point
        for i in range(in_cloud.shape[0]):
            for j in range(1, in_cloud.shape[1]):
                if in_cloud[i, j, -1] == -1:
                    nan_size = 1
                    left = j - 1
                    right = j + 1
                    while in_cloud[i, left, -1] == -1:
                        left -= 1
                        nan_size += 1
                    while in_cloud[i