Open3D Ransac拟合二维圆

目录

一、概述

1.1算法原理

1.2数学描述

二、代码实现

2.1关键函数

2.2完整代码

三、实现效果

---

前期试读，后续会将博客加入该专栏，欢迎订阅
Open3D点云算法与点云深度学习案例汇总（长期更新）-CSDN博客

一、概述

1.1算法原理

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

具体到拟合二维圆的情况，RANSAC 的步骤如下：

1. 随机采样：从点云数据集中随机选择三个点。这三个点足以确定一个圆。
2. 拟合模型：计算通过这三个点确定的圆，包括圆心和半径。利用这些点的几何关系，建立用于计算圆心和半径的方程。
3. 计算内点：对于点云数据中的每个点，计算其到拟合圆的距离。距离小于预设阈值的点被认为是内点。
4. 评估模型：记录内点的数量。如果当前圆模型的内点数量超过之前的最大内点数量，则更新最佳模型和内点集合。
5. 重复：重复上述步骤若干次（预设的迭代次数），每次都记录最佳模型和内点集合。
6. 输出结果：迭代结束后，输出内点数量最多的圆模型作为最终结果。

1.2数学描述

二、代码实现

2.1关键函数

def ransac_fit_circle(points, distance_threshold=0.05, num_iterations=1000):
    """
    使用 RANSAC 算法拟合二维圆形。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 2)。
        包含点云数据的二维数组，其中 N 是点的数量。
    distance_threshold (float): 内点距离阈值，默认为 0.05。
        用于判断一个点是否属于拟合的圆，如果点到圆的距离小于该阈值，则认为该点是内点。
    num_iterations (int): RANSAC 算法的迭代次数，默认为 1000。
        算法进行迭代的次数。更多的迭代次数可以提高找到最佳拟合的概率。

    返回:
    best_circle (tuple): 拟合的圆形参数 (cx, cy, r)。
        包含圆心坐标 (cx, cy) 和半径 r 的元组。
    best_inliers (list): 内点索引列表。
        包含所有内点索引的列表。
    """
    best_inliers = []  # 用于存储当前找到的最佳内点集合
    best_circle = None  # 用于存储当前找到的最佳圆形参数
    num_points = len(points)  # 获取点云数据中的点数量

    for _ in range(num_iterations):  # 迭代 RANSAC 算法
        # 随机选择3个点
        sample_indices = random.sample(range(num_points), 3)  # 从点云中随机选择3个点的索引
        sample_points = points[sample_indices]  # 获取3个采样点的坐标

        # 拟合圆
        # 使用三个点的坐标来计算圆心和半径
        A = np.array(
            [[2 * (sample_points[1][0] - sample_points[0][0]), 2 * (sample_points[1][1] - sample_points[0][1])],
             [2 * (sample_points[2][0] - sample_points[0][0]), 2 * (sample_points[2][1] - sample_points[0][1])]])
        b = np.array(
            [sample_points[1][0] ** 2 - sample_points[0][0] ** 2 + sample_points[1][1] ** 2 - sample_points[0][1] ** 2,
             sample_points[2][0] ** 2 - sample_points[0][0] ** 2 + sample_points[2][1] ** 2 - sample_points[0][1] ** 2])

        # 检查矩阵 A 的秩，以确保其可解
        if np.linalg.matrix_rank(A) < 2:
            continue  # 如果 A 的秩小于 2，跳过本次迭代

        # 计算圆心坐标
        center = np.linalg.lstsq(A, b, rcond=None)[0]
        cx, cy = center[0], center[1]
        # 计算半径
        r = np.linalg.norm(sample_points[0] - center)

        # 计算内点
        inliers = []
        for i in range(num_points):  # 遍历所有点
            distance = np.abs(np.linalg.norm(points[i] - center) - r)  # 计算点到圆的距离
            if distance < distance_threshold:  # 如果距离小于阈值，则为内点
                inliers.append(i)

        # 更新最佳圆形参数和内点集合
        if len(inliers) > len(best_inliers):
            best_inliers = inliers  # 更新最佳内点集合
            best_circle = (cx, cy, r)  # 更新最佳圆形参数

    return best_circle, best_inliers  # 返回最佳圆形参数和内点集合

2.2完整代码

import open3d as o3d
import numpy as np
import random


def generate_noisy_circle(radius=1.0, num_points=1000, noise_level=0.05):
    """
    生成一个带有噪声的二维圆形点云。

    参数:
    radius (float): 圆的半径，默认为 1.0。
    num_points (int): 点云中的点的数量，默认为 1000。
    noise_level (float): 噪声水平，默认为 0.05。

    返回:
    numpy.ndarray: 生成的点云数据。
    """
    theta = np.linspace(0, 2 * np.pi, num_points)
    x = radius * np.cos(theta)
    y = radius * np.sin(theta)

    points = np.vstack((x, y)).T

    # 添加噪声
    noise = np.random.normal(0, noise_level, points.shape)
    noisy_points = points + noise

    return noisy_points


def ransac_fit_circle(points, distance_threshold=0.05, num_iterations=1000):
    """
    使用 RANSAC 算法拟合二维圆形。

    参数:
    points (numpy.ndarray): 点云数据，形状为 (N, 2)。
    distance_threshold (float): 内点距离阈值，默认为 0.05。
    num_iterations (int): RANSAC 算法的迭代次数，默认为 1000。

    返回:
    best_circle (tuple): 拟合的圆形参数 (cx, cy, r)。
    best_inliers (list): 内点索引列表。
    """
    best_inliers = []
    best_circle = None
    num_points = len(points)

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

        # 拟合圆
        A = np.array(
            [[2 * (sample_points[1][0] - sample_points[0][0]), 2 * (sample_points[1][1] - sample_points[0][1])],
             [2 * (sample_points[2][0] - sample_points[0][0]), 2 * (sample_points[2][1] - sample_points[0][1])]])
        b = np.array(
            [sample_points[1][0] ** 2 - sample_points[0][0] ** 2 + sample_points[1][1] ** 2 - sample_points[0][1] ** 2,
             sample_points[2][0] ** 2 - sample_points[0][0] ** 2 + sample_points[2][1] ** 2 - sample_points[0][1] ** 2])

        if np.linalg.matrix_rank(A) < 2:
            continue

        center = np.linalg.lstsq(A, b, rcond=None)[0]
        cx, cy = center[0], center[1]
        r = np.linalg.norm(sample_points[0] - center)

        # 计算内点
        inliers = []
        for i in range(num_points):
            distance = np.abs(np.linalg.norm(points[i] - center) - r)
            if distance < distance_threshold:
                inliers.append(i)

        if len(inliers) > len(best_inliers):
            best_inliers = inliers
            best_circle = (cx, cy, r)

    return best_circle, best_inliers


def create_circle_mesh(cx, cy, r, num_segments=100):
    """
    创建一个圆形的 Mesh，用于可视化。

    参数:
    cx (float): 圆心的 x 坐标。
    cy (float): 圆心的 y 坐标。
    r (float): 圆的半径。
    num_segments (int): 圆的分段数量，默认为 100。

    返回:
    open3d.geometry.LineSet: 圆形 LineSet 对象。
    """
    theta = np.linspace(0, 2 * np.pi, num_segments)
    x = r * np.cos(theta) + cx
    y = r * np.sin(theta) + cy
    z = np.zeros_like(x)

    circle_points = np.vstack((x, y, z)).T
    lines = [[i, (i + 1) % num_segments] for i in range(num_segments)]

    line_set = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(circle_points),
        lines=o3d.utility.Vector2iVector(lines)
    )

    # 设置圆形的颜色为红色
    colors = [[1, 0, 0] for _ in range(len(lines))]
    line_set.colors = o3d.utility.Vector3dVector(colors)

    return line_set


# 生成带有噪声的二维圆形点云
radius = 1.0
num_points = 1000
noise_level = 0.05
points = generate_noisy_circle(radius, num_points, noise_level)

# 使用 RANSAC 算法拟合圆形
distance_threshold = 0.05
num_iterations = 1000
best_circle, inliers = ransac_fit_circle(points, distance_threshold, num_iterations)

if best_circle is not None:
    # 提取拟合圆的参数
    cx, cy, r = best_circle

    # 创建点云对象
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(np.hstack((points, np.zeros((points.shape[0], 1)))))

    # 给点云设置颜色为蓝色
    pcd.paint_uniform_color([0, 0, 1])

    # 创建拟合圆的 Mesh 对象
    circle_mesh = create_circle_mesh(cx, cy, r)

    # 可视化点云和拟合的圆
    o3d.visualization.draw_geometries([pcd, circle_mesh], window_name="RANSAC Circle Fitting",
                                      width=800, height=600, left=50, top=50)
else:
    print("未找到合适的圆形拟合")

三、实现效果