点云算法-提取kitti路面点云

目录

一、ransac原理

二、ransac 地面分割原理

三、ransac常见应用

四、代码

五、截图

六、总结

---

一、ransac原理

RANSAC是“random sample consensus（随机抽样一致）”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。

它是一种不确定的算法,它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。

划重点：

1、定义一个要估计的模型

2、迭代求解模型参数

3、迭代的条件通过概率进行判断是否合理

RANSAC的基本假设是： 
（1）数据由“局内点”组成，例如：数据的分布可以用一些模型参数来解释； 
（2）“局外点”是不能适应该模型的数据； 
（3）除此之外的数据属于噪声。 
局外点产生的原因有：噪声的极值；错误的测量方法；对数据的错误假设。 
RANSAC也做了以下假设：给定一组（通常很小的）局内点，存在一个可以估计模型参数的过程；而该模型能够解释或者适用于局内点。

二、ransac 地面分割原理

根据刚总结的：

1、定义一个要估计的模型

2、迭代求解模型参数

3、迭代的条件通过概率进行判断是否合理

对应进行算法设计：

1、定义平面表达方程为 aX + bY + cZ +D= 0，通过道理，如果要提取的数据能够通过模型来描述，如直线、球、圆柱、平面等的拟合与分割，都可以考虑使用ransac思路

2、迭代过程

• 求解法向量用于求解平面方程abcd参数
• 通过计算距离，判断是否是比较好的迭代效果，如果是，更新参数

    计算距离：distance = abs(pointn_1.dot(N)) / np.linalg.norm(N)  # 求距离

• 计算就算最佳迭代次数

 

iters = math.log(1 - P) / math.log(1 - pow(total_inlier / n, 3))

3、概率判断条件

当内点个数满足设置的要求

 # 判断是否当前模型已经符合超过 inline_ratio
        if total_inlier > n * (1 - outline_ratio):
            break

三、代码

import numpy as np
import open3d as o3d
import struct
import matplotlib.pyplot as plt
from pandas import DataFrame
from pyntcloud import PyntCloud
import math
import random
#import Spectral as sp
from collections import defaultdict


# 功能：从kitti的.bin格式点云文件中读取点云
# 输入：
#     path: 文件路径
# 输出：
#     点云数组
def read_velodyne_bin(path):
    '''
    :param path:
    :return: homography matrix of the point cloud, N*3
    '''
    pc_list = []
    with open(path, 'rb') as f:
        content = f.read()
        pc_iter = struct.iter_unpack('ffff', content)
        for idx, point in enumerate(pc_iter):
            pc_list.append([point[0], point[1], point[2]])
    return np.asarray(pc_list, dtype=np.float32)


# 功能：从点云文件中滤除地面点
# 输入：
#     data: 一帧完整点云
# 输出：
#     segmengted_cloud: 删除地面点之后的点云
def ground_segmentation(data):
    # 作业1
    # 屏蔽开始
    # 初始化数据
    idx_segmented = []
    segmented_cloud = []
    iters = 100  # 最大迭代次数  000002.bin：10
    sigma = 0.4  # 数据和模型之间可接受的最大差值   000002.bin：0.5   000001.bin: 0.2  000000.bin: 0.15  002979.bin：0.15  004443.bin：0.4
    ##最好模型的参数估计和内点数目,平面表达方程为   aX + bY + cZ +D= 0
    best_a = 0
    best_b = 0
    best_c = 0
    best_d = 0
    pretotal = 0  # 上一次inline的点数
    # 希望的到正确模型的概率
    P = 0.99
    n = len(data)  # 点的数目
    outline_ratio = 0.6  # e :outline_ratio   000002.bin：0.6    000001.bin: 0.5  000000.bin: 0.6   002979.bin：0.6
    for i in range(iters):
        ground_cloud = []
        idx_ground = []
        # step1 选择可以估计出模型的最小数据集，对于平面拟合来说，就是三个点
        sample_index = random.sample(range(n), 3)  # 重数据集中随机选取3个点
        point1 = data[sample_index[0]]
        point2 = data[sample_index[1]]
        point3 = data[sample_index[2]]
        # step2 求解模型
        ##先求解法向量
        point1_2 = (point1 - point2)  # 向量 poin1 -> point2
        point1_3 = (point1 - point3)  # 向量 poin1 -> point3
        N = np.cross(point1_3, point1_2)  # 向量叉乘求解 平面法向量
        ##slove model 求解模型的a,b,c,d
        a = N[0]
        b = N[1]
        c = N[2]
        d = -N.dot(point1)
        # step3 将所有数据带入模型，计算出“内点”的数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
        total_inlier = 0
        pointn_1 = (data - point1)  # sample（三点）外的点 与 sample内的三点其中一点 所构成的向量
        distance = abs(pointn_1.dot(N)) / np.linalg.norm(N)  # 求距离
        ##使用距离判断inline
        idx_ground = (distance <= sigma)
        total_inlier = np.sum(idx_ground == True)  # 统计inline得点数
        ##判断当前的模型是否比之前估算的模型
        if total_inlier > pretotal:  # log(1 - p)
            iters = math.log(1 - P) / math.log(1 - pow(total_inlier / n, 3))  # N = ------------
            pretotal = total_inlier  # log(1-[(1-e)**s])
            # 获取最好得 abcd 模型参数
            best_a = a
            best_b = b
            best_c = c
            best_d = d

        # 判断是否当前模型已经符合超过 inline_ratio
        if total_inlier > n * (1 - outline_ratio):
            break
    print("iters = %f" % iters)
    # 提取分割后得点
    idx_segmented = np.logical_not(idx_ground)
    ground_cloud = data[idx_ground]
    segmented_cloud = data[idx_segmented]
    return ground_cloud, segmented_cloud

    # 屏蔽结束

    # print('origin data points num:', data.shape[0])
    # print('segmented data points num:', segmengted_cloud.shape[0])
    # return segmengted_cloud


def main():
    iteration_num = 1  # 文件数

    # for i in range(iteration_num):
    filename = 'data/000001.bin'  # 数据集路径
    print('clustering pointcloud file:', filename)

    origin_points = read_velodyne_bin(filename)  # 读取数据点
    origin_points_df = DataFrame(origin_points, columns=['x', 'y', 'z'])  # 选取每一列 的 第0个元素到第二个元素   [0,3)
    point_cloud_pynt = PyntCloud(origin_points_df)  # 将points的数据 存到结构体中
    point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)  # 实例化
    #
    #o3d.visualization.draw_geometries([point_cloud_o3d])  # 显示原始点云

    # 地面分割
    ground_points, segmented_points = ground_segmentation(data=origin_points)

    ground_points_df = DataFrame(ground_points, columns=['x', 'y', 'z'])  # 选取每一列 的 第0个元素到第二个元素   [0,3)
    point_cloud_pynt_ground = PyntCloud(ground_points_df)  # 将points的数据 存到结构体中
    point_cloud_o3d_ground = point_cloud_pynt_ground.to_instance("open3d", mesh=False)  # 实例化
    point_cloud_o3d_ground.paint_uniform_color([0, 0, 255])
    o3d.visualization.draw_geometries([point_cloud_o3d_ground])  # 显示地面点云


if __name__ == '__main__':
    main()

四、截图

五、总结

ransac模型的拟合的取决于物体的密度分布，在三维场景较为适合，但是ransac具有随机性，所以每次运行的结果不尽相同。

ransac应用广泛，也有使用ransac进行影像处理的。比如下边来之一片论文在摘要：

本文首先介绍 RANSAC 算法的发展、原理及其改进算法。重点介绍其在遥感图像处理中几何校正和辐射校正的应用。在几何校正中一般是得到控制点对后，直接进行矫正模型的拟合，没有考虑异常数据的影响。本文利用 RANSAC 算法，剔除误匹配点后再拟合模型。在利用 RANSA算法迭代得出稳健的模型参数前，首先要选择合适的几何模型，常用的几何模型是基本矩阵和单应矩阵。在遥感图像几何处理部分，本文主要针对几何处理中的图像配准进行实验。图像配准分为基于灰度特征和基于特征点的匹配方法。本文实验中图像配准基于特征点,计算基本矩阵筛选出影响模型参数的误匹配点:实验表明 RANSAC 算法剔除误匹配点后匹配效果明显，校正精度显著提高至1到2个像元误差。
遥感图像辐射校正分为绝对辐射校正和相对辐射校正。本文主要针对相对辐射校正线性回归法应用 RANSAC 算法，在寻找两幅图像重叠区域像素间线性关系中，剔除异常数据点，最大限度的利用支持线性模型的数据点拟合得到两图像的辐亮度线性变换模型参数。本文针对一些辐射校正后颜色平衡效果不好的情况，提出对像素点先聚类再拟合的方法。
最后介绍对 RANSAC算法的一些改进以便提高其运算效率。基于 RANSAC算法原理，主要可以从两方面对其改进:每次迭代样本的选取和模型参数的检验。本文除了介绍预检验模型参数的原理，更主要是根据样本选取提出了更加有效的样本预检验模型，即在原来预检验的基础上增加样本预检验。这样不仅减少检验模型参数的计算量同时也提高了样本选取的效率。

其主要目标，还是用来去掉数据集中的外点，增加数据的精度。