基于点云数据的道路面自动化提取——生长算法

基于点云数据的道路面自动化提取——生长算法

摘要

　　为了信息化管理和维护在役的道路基础设施，同时为高精度地图的制作和城市规划管理提供数据支撑，行业内开始探索“三维实景数据采集技术”在道路基础设施数字化中的应用。三维实景数据采集技术，一般狭义上指代激光雷达扫描技术，可对物理世界中道路场景的三维坐标、颜色和和反射率等信息进行采集，并可实现计算机内道路环境的三维重构，为道路信息的提取提供了数字化平台。

　　在采集数据中，道路路面是道路基础设施的骨架信息，道路标志、标线、平整度等信息的提取都需要在路面提取的基础上开展。但是计算机和采集设备无法自行对数据进行标记分类，而且由于采集数据量极为庞大，单个数据特征不明显，采用人工筛选的办法极为耗时，且成本高昂。 如何快速和高精度的对数据进行标记分类，从中提取出道路的路面区域，已经是当前道路基础设施数字化领域面对的重要问题。

　　本文提取道路路面可以分为两个部分，第一部分，地面分离，使地面与非地面两部分点云数据实现分离。第二部分，路面提取，从地面点云当中分离出路面点云。

准备工作

点云数据

　　首先，你需要准备一份点云数据，数据量不要太大，100万以内，python算得有点慢，其次点云的密度尽量高一些。本人准备的数据包含201001个点，道路场景如下：

​​

运行环境

　　本文使用Python，版本为3.10.10，矩阵计算使用的库为numpy，版本为1.24.2，点云处理的库为opend3d，版本为0.17.0。

具体过程

读取数据

　　首先导入所用到的数据库，使用open3d库读取点云数据。

import numpy as np
import open3d as o3d
from abc import ABC, abstractmethod

class ReadDatas:

    def __init__(self, path: str) -> None:
        self.path: str = path
        self.pcd = self.read_point_cloud()
        self.pcd_np = np.asarray(self.pcd.points)
        self.pcd_hmax = self.pcd.get_max_bound()[2]
        self.pcd_hmin = self.pcd.get_min_bound()[2]
        self.pcd_num = len(self.pcd_np)
        self.theta_threshold = 20
        self.cosine_threshold = np.cos(np.deg2rad(self.theta_threshold))
        self.curvature_threshold = 0.035
      

    def read_point_cloud(self):
        '''读取点云文件'''
        pcd = o3d.io.read_point_cloud(self.path)
        return pcd

    def np_to_o3d(self, points):
        name = ''
        name = o3d.geometry.PointCloud()
        name.points = o3d.utility.Vector3dVector(points)
        name.estimate_normals()

创建基础算法

　　在地面分离和路面提取当中，会用到一些共同算法，本文把这些基础算法都封装到同一个类中。

class BaseAlgorithm(ABC):

    def __init__(self) -> None:
        self.data = None

    def set_data(self, data) -> None:
        self.data = data

    def fit_plane(self, points):
        ''' 利用svd奇异值求解，拟合平面'''
        centroid = np.mean(points, axis=0)
        centered = points - centroid
        u, _, _ = np.linalg.svd(centered.T, full_matrices=False, compute_uv=True)
        normal = u[:, -1]
        intercept = -np.dot(normal, centroid)
        return normal, intercept

    def seed_select(self, cloud):
        vis = o3d.visualization.VisualizerWithEditing()
        vis.create_window(window_name='种子点选取', visible=True)
        vis.add_geometry(cloud)
        vis.run()
        seed = vis.get_picked_points()
        vis.destroy_window()
        return seed

地面分离

　　从点云数据中使地面与非路面分离，有效的降低了数据量的大小，排除了非地面点云数据对后面路面提取步骤的干扰，提升了路面的提取速度与效率。

　　地面分割其难度主要体现在以下几个方面：

• 地面形状的多样性：地面在不同场景下的形状和高程特征往往各异，如平坦地面、山地地面、道路地面等，这就要求地面分割算法能够适应不同形状的地面，并能准确地划分地面点和非地面点。
• 点云噪声的干扰：点云数据通常会受到各种噪声和干扰，如传感器误差、大气光学、光线衍射等，这些噪声和干扰会影响地面分割的准确性和可靠性。
• 非地面目标的复杂性：点云中除了地面外，还包含着各种非地面目标，如建筑物、树木、车辆等，这些目标的形状和高程特征也各异，有些与地面接近甚至重叠，这就要求地面分割算法能够准确地区分出这些非地面目标。
• 数据量的大：点云数据通常非常庞大，因此地面分割算法需要具备高效的处理能力和大规模数据的处理能力。

　　地面分离采用的事故地平面滤波算法。地平面滤波算法是一种改进的 RANSAC 算法，主要用于从点云数据中提取地面平面。该算法由 Dimitris Zermas 在2017年提出，其主要思想是利用先验知识来消除典型平面拟合技术中的随机选择，从而提高拟合效率。

　　在道路场景中，地面往往是相对平坦的，而且最低高度值的点很有可能属于地面。因此，该算法利用这一先验知识来确定种子点。假设这一批种子点为最低点。该算法通常会将点云数据按照车辆行进方向等距划分成不同的片段，以处理道路上的起伏情况。对于每个点云片段，执行如下操作。

1. 确定一组具有低高度值的种子点。
2. 使用SVD算法并用于估计种子点构成地平面。
3. 计算点云数据中其余点到估计平面的距离h，距离小于设置阈值ground_h的点加入到种子点中。
4. 重复这个步骤iterations次，最后所有的种子点即为所求的地面。

　　当然，本人认为高度最低的点不一点使种子点，可能使某些干扰点，所以对算法进行了修改，设点云数据中高度最低的nlrp个点云高度的平均值加上thseeds的和为A，高度值小于A的值都为地面点。

　　算法代码如下：

class GpfGroundExtractor(BaseAlgorithm):

    def _process_data(self, nlrp: int = 1000, thseeds: float = 0.1, ground_h: float = 0.15, iterations: int = 10):
        xyz = self.data.pcd_np
        xyz = xyz[np.lexsort(xyz.T)]
        elevation_nlrp_min_mean: float = np.average(xyz[:nlrp, 2])
        seed = xyz[xyz[:, 2] < thseeds + elevation_nlrp_min_mean]

        for i in range(iterations):
            normal, intercept = self.fit_plane(seed)
            h = (xyz[:, 0] * normal[0] + xyz[:, 1] * normal[1] + xyz[:, 2] * normal[2] + intercept) / np.linalg.norm(
                normal)
            seed = xyz[np.abs(h) < ground_h]

        self.data.ground = seed
        self.data.no_ground = xyz[np.abs(h) >= ground_h]
        self.data.ground_num = len(self.data.ground)

　　运行以下代码：

if __name__ == '__main__':
    path = '你的点云数据的路径'
    pcd =  ReadDatas(path)
    gpf = GpfGroundExtractor()
    gpf.set_data(pcd)
    gpf._process_data()
	ground_o3d = self.gpf.np_to_o3d(self.pcd.ground)
	o3d.visualization.draw_geometries([ground_o3d])

　　你就可以看到提取出来的地面了。

​​

路面提取

　　点云数据经过地面分割后分割成两部分，一部分是非地面点云，通常包括建筑物、汽车和人群等，另一部分是地面点，包括草地、人行道和路面等，路面提取的内容主要是从地面点中提取出属于路面点云。

• 点云数据经过地面分割后分割成两部分，一部分是非地面点云，通常包括建筑物、汽车和人群等，另一部分是地面点，包括草地、人行道和路面等，路面提取的内容主要是从地面点中提取出属于路面点云。
  路面提取的难点主要体现在下面几点：
• 杂散点干扰：点云中可能存在一些杂散点，例如噪声点、树木等，这些点可能会干扰路面点的提取。在路面点提取过程中，需要使用一些滤波算法去除这些干扰点，以提高路面点的准确性和可靠性。
• 车辆遮挡：在车辆行驶过程中，可能会出现车辆遮挡的情况，这可能会影响点云数据的质量和精度，因此需要考虑车辆遮挡的影响，使用合适的算法进行处理。道路起伏程度大：一些路面提取的算法依赖于道路路面平整的假设，如果路面起伏过大容易出现过分割的现象，即将部分属于道路路面的点的部分分割掉。

　　区域生长算法是一种常用的图像分割算法，通过选取点云集中的一点或者多点作为种子点，种子点根据一定的生长规则，从自身开始不断向周围进行扩散，直至停止，被扩散到的区域即为提取的区域。

种子点选取

　　使用区域生长算法提取道路路面的过程中，种子点的选取十分重要。按照上述原理，必须选择一个在道路上点作为种子点，所以不能采用随机取样的方法。通常业内选择平均曲率或者高程最小的点作为种子点，但在实际过程中，这些点未必在道路上，这导致了最后提取道路路面的失败。在程序中，种子点的选取采用的手动选取的方式，采取方法所示，需要使用者在图形交互页面点击一个点作为种子点，这保证选取的种子点一定是在道路上的点。

​​

区域分割

　　通常种子点的生长规则是把种子点与其各邻域点法向量夹角和各邻域点的曲率值作为约束条件，而这约束生长规则成功提取道路路面点存在前提条件，即道路路面与其周围环境的交界处存在较大曲率与法向量变化。但是在实际的道路环境当中，为了方便轮椅等过街穿行，道路与人行道有一段相对低矮平整的交接处，按照上述的生长规则，种子点可能会随着这段交界处扩散到人行道上，导致提取道路路面失败，在本程序中，可以通过图形化操作界面预先将这一交界处进行分割，排除连接处对提取过程的干扰。

生长算法

　　区域生长算法的流程图

​​

　　区域生长算法的具体步骤：

• 通过可视化的交互界面输入初始种子点。
• 对道路与人行道这些交界处进行切割。
• 输入法向量夹角与曲率的阈值。
• 种子点搜索自身最近的几个邻域点。
• 计算种子点与最近邻域点之间的法向量夹角，夹角小于法向量夹角阈值的邻域点进入下一步。
• 邻域点的曲率与曲率的阈值进行比较，小于阈值的点如果不在种子点序列中，加入到种子点当中。
• 计算下一个种子点，重复4到6步直至再无下一个种子点。

　　结果如下：

​​

　　具体代码：

class ReGrowSegment(BaseAlgorithm):

    def __init__(self, data, theta_threshold = 20, curvature_threshold = 0.035) -> None:
        self.data = data
        self.ground_np = self.np_to_o3d(self.data.ground)
        self.seed = self.seed_select(self.ground_np)
        self.no_paves = self.find_no_paves(self.ground_np)
        # np_clouds = np.asarray(clouds)
        self.no_paves = self.find_index(self.data.ground, self.no_paves)
        self.nebor_all = self.find_neighbour_points(self.ground_np)
        self.curvity = self.curvature_calculation(self.ground_np)
        self.paves = np.array(self.seed)
        self.cosine_threshold = np.cos(np.deg2rad(theta_threshold))
        self.curvature_threshold = curvature_threshold
      

    def _process_data(self) -> None:

        while len(self.seed) > 0:
            seed_now = self.seed[0]
            nebor = self.nebor_all[seed_now]
            nebor = np.asarray(nebor)

            nebor_np = nebor[np.isin(nebor, self.paves, invert=True)]
            nebor_new = nebor_np[np.isin(nebor_np, self.seed, invert=True)]
            nebor_new = nebor_new[np.isin(nebor_new, self.no_paves, invert=True)]

            if len(nebor_new) > 0:
                curr_seed_normal = self.ground_np.normals[seed_now]  # 当前种子点的法向量
                seed_nebor_normal = [self.ground_np.normals[int(i)] for i in nebor_new]  # 种子点邻域点的法向量
                dot_normal = np.fabs(np.dot(seed_nebor_normal, curr_seed_normal))
                nebor_new = nebor_new.astype('int64')
                curvity_now = self.curvity[nebor_new]
                a = dot_normal > self.cosine_threshold
                b = curvity_now < self.curvature_threshold
                c = a & b
                paves_new = nebor_new[c]
                self.paves = np.append(self.paves, paves_new)
                self.seed = np.append(self.seed, paves_new)

            self.seed = np.delete(self.seed, [0])

        self.data.paves = self.paves
        self.data.paves_num = len(self.data.paves)

　　把上述代码放在同一文件中，运行一下代码：

if __name__ == '__main__':
    path = '你的点云数据的路径'
    pcd =  ReadDatas(path)
    gpf = GpfGroundExtractor()
    gpf.set_data(pcd)
    gpf._process_data()
	reg = ReGrowSegment(pcd)
	reg._process_data()
	paves = ground_o3d.select_by_index(pcd.paves)
	no_paves = ground_o3d.select_by_index(pcd.paves, invert=True)
	no_ground = gpf.np_to_o3d(pcd.no_ground)
	paves.paint_uniform_color([1,0,0])
	o3d.visualization.draw_geometries([no_paves,paves,no_ground])

　　‍