Open3D Ransac拟合空间直线

目录

一、概述

1.1实现步骤

1.2优势与局限

二、代码实现

2.1关键代码

2.2完整代码

三、实现效果

前期试读，后续会将博客加入该专栏，欢迎订阅

Open3D点云算法与点云深度学习案例汇总（长期更新）-CSDN博客

一、概述

        RANSAC（RANdom SAmple Consensus）是一种迭代算法，用于从包含大量异常值的数据集中拟合模型。其核心思想是通过在数据集中随机抽取子集来拟合模型，并评估模型的适用性，最终选择内点最多的模型作为最佳拟合。

1.1实现步骤

以下是 RANSAC 拟合空间直线的详细步骤：

1. 随机采样：从点云数据集中随机选择两个点（最少点数目来拟合直线）。
2. 拟合模型：使用这两个点拟合一条直线。通过计算方向向量 𝑣和直线上任意一点 𝑝来定义直线。
3. 计算内点：对于点云数据中的每个点，计算其到拟合直线的距离。距离小于预设阈值的点被认为是内点。
4. 评估模型：记录内点的数量。如果当前直线模型的内点数量超过之前的最大内点数量，则更新最佳模型和内点集合。
5. 重复：重复上述步骤若干次（预设的迭代次数），每次都记录最佳模型和内点集合。
6. 输出结果：迭代结束后，输出内点数量最多的直线模型作为最终结果。

1.2优势与局限

优势：

• 鲁棒性：能够有效处理数据集中存在的大量噪声和异常值。
• 简单易用：算法相对简单，易于实现和理解。

局限：

• 计算效率：当点云数据量较大时，算法的计算时间可能较长。
• 参数依赖：需要预设距离阈值、采样点数和迭代次数等参数，这些参数的选择对算法性能有较大影响。
• 模型假设：RANSAC 需要针对不同应用场景选择合适的模型（如直线、平面等），对复杂场景的适应性有限。

二、代码实现

2.1关键代码

def ransac_fit_line(points, distance_threshold=0.1, ransac_n=2, num_iterations=1000):
    """
    使用 RANSAC 算法拟合空间直线。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 3)。
    distance_threshold (float): 内点距离阈值，默认为 0.1。用于判断一个点是否属于拟合的直线。
    ransac_n (int): 每次随机采样的点的数量，默认为 2。用于拟合直线。
    num_iterations (int): RANSAC 算法的迭代次数，默认为 1000。增加迭代次数可以提高找到最佳拟合的概率。

    返回:
    best_line (numpy.ndarray): 拟合的直线模型参数，形状为 (2, 3)。包含拟合直线的两个端点。
    best_inliers (list): 内点索引列表。包含符合拟合直线的点的索引。
    best_direction (numpy.ndarray): 拟合直线的方向向量。
    """
    best_inliers = []  # 初始化最佳内点列表为空
    best_line = None  # 初始化最佳直线模型为空
    num_points = len(points)  # 获取点云中的点的数量

    for _ in range(num_iterations):  # RANSAC 算法迭代次数
        # 随机选择 ransac_n 个点
        sample_indices = random.sample(range(num_points), ransac_n)
        sample_points = points[sample_indices]

        # 拟合直线：计算两个采样点之间的方向向量
        v = sample_points[1] - sample_points[0]
        v /= np.linalg.norm(v)  # 将方向向量单位化
        line_point = sample_points[0]  # 选择第一个采样点作为直线上的一个点

        # 计算内点：判断哪些点与拟合的直线距离小于阈值
        inliers = calculate_inliers(points, line_point, v, distance_threshold)

        # 如果找到的内点数量比当前最佳内点数量多，则更新最佳内点和最佳直线模型
        if len(inliers) > len(best_inliers):
            best_inliers = inliers
            best_line = sample_points
            best_direction = v  # 更新最佳方向向量

    return best_line, best_inliers, best_direction  # 返回最佳直线模型、内点列表和方向向量

2.2完整代码

import open3d as o3d
import numpy as np
import random


def generate_noisy_line(num_points=1000, noise_level=0.1):
    """
    生成一条带有噪声的直线点云。

    参数:
    num_points (int): 点云中的点的数量。
    noise_level (float): 噪声水平，默认为 0.1。

    返回:
    point_cloud (numpy.ndarray): 生成的点云数据。
    """
    x = np.linspace(-5, 5, num_points)
    y = 2 * x + 1
    z = np.zeros_like(x)

    # 添加噪声
    noise = np.random.normal(0, noise_level, (num_points, 3))
    points = np.vstack((x, y, z)).T + noise

    return points


def calculate_inliers(points, line_point, v, distance_threshold):
    """
    计算点云中符合距离阈值的内点。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 3)。
    line_point (numpy.ndarray): 直线上一点。
    v (numpy.ndarray): 直线方向向量。
    distance_threshold (float): 距离阈值。

    返回:
    inliers (list): 内点索引列表。
    """
    inliers = []
    for i in range(points.shape[0]):
        point = points[i]
        distance = np.linalg.norm(np.cross(point - line_point, v))
        if distance < distance_threshold:
            inliers.append(i)
    return inliers


def ransac_fit_line(points, distance_threshold=0.1, ransac_n=2, num_iterations=1000):
    """
    使用 RANSAC 算法拟合空间直线。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 3)。
    distance_threshold (float): 内点距离阈值，默认为 0.1。
    ransac_n (int): 每次随机采样的点的数量，默认为 2。
    num_iterations (int): RANSAC 算法的迭代次数，默认为 1000。

    返回:
    best_line (numpy.ndarray): 拟合的直线模型参数，形状为 (2, 3)。
    best_inliers (list): 内点索引列表。
    """
    best_inliers = []
    best_line = None
    num_points = len(points)

    for _ in range(num_iterations):
        # 随机选择 ransac_n 个点
        sample_indices = random.sample(range(num_points), ransac_n)
        sample_points = points[sample_indices]

        # 拟合直线
        v = sample_points[1] - sample_points[0]
        v /= np.linalg.norm(v)
        line_point = sample_points[0]

        inliers = calculate_inliers(points, line_point, v, distance_threshold)

        if len(inliers) > len(best_inliers):
            best_inliers = inliers
            best_line = sample_points

    return best_line, best_inliers, v


def create_lineset_from_line_model(line_model, direction, length_multiplier=2.0):
    """
    从 RANSAC 拟合的线模型创建 LineSet 对象，并设置线段长度。

    参数:
    line_model (numpy.ndarray): 直线模型参数，形状为 (2, 3)。
    direction (numpy.ndarray): 直线方向向量。
    length_multiplier (float): 线段长度倍数，默认为 2.0。

    返回:
    lineset (open3d.geometry.LineSet): LineSet 对象。
    """
    mid_point = (line_model[0] + line_model[1]) / 2
    length = np.linalg.norm(line_model[1] - line_model[0]) * length_multiplier
    start_point = mid_point - direction * length / 2
    end_point = mid_point + direction * length / 2
    points = [start_point, end_point]
    lines = [[0, 1]]
    colors = [[1, 0, 0] for _ in range(len(lines))]

    lineset = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(points),
        lines=o3d.utility.Vector2iVector(lines),
    )
    lineset.colors = o3d.utility.Vector3dVector(colors)

    return lineset


# 生成带有噪声的直线点云
num_points = 100
noise_level = 0.1
points = generate_noisy_line(num_points, noise_level)

# 使用 RANSAC 算法拟合直线
distance_threshold = 0.1
ransac_n = 2
num_iterations = 500
line_model, inliers, direction = ransac_fit_line(points, distance_threshold, ransac_n, num_iterations)

# 创建 LineSet 对象，设置线段长度为点云范围的2倍
lineset = create_lineset_from_line_model(line_model, direction, length_multiplier=2.0)

# 可视化点云和拟合的直线
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 1, 0])

o3d.visualization.draw_geometries([pcd, lineset, inlier_cloud], window_name="RANSAC Line Fitting",
                                  width=800, height=600, left=50, top=50)

三、实现效果