RANSAC算法拟合平面 python pcd文件 matplotlib

以下是使用Python和matplotlib库实现RANSAC算法拟合平面的代码：

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 读取点云数据
data = np.genfromtxt('point_cloud.pcd', delimiter=' ', skip_header=11)
points = data[:, 0:3]

# 定义RANSAC算法
def ransac(points, n_iterations, threshold):
    best_inliers = None
    best_plane = None
    for i in range(n_iterations):
        # 随机选择3个点
        sample = points[np.random.choice(points.shape[0], 3, replace=False), :]
        # 计算平面法向量
        normal = np.cross(sample[1]-sample[0], sample[2]-sample[0])
        normal /= np.linalg.norm(normal)
        # 计算所有点到平面的距离
        distances = np.abs(np.dot(points - sample[0], normal))
        # 找到距离小于阈值的点
        inliers = points[distances < threshold, :]
        # 如果当前内点数量大于最好的内点数量，更新最好的内点集合和平面
        if best_inliers is None or len(inliers) > len(best_inliers):
            best_inliers = inliers
            best_plane = normal, sample[0]
    # 用最好的内点集合重新计算平面
    normal, point = best_plane
    distances = np.abs(np.dot(points - point, normal))
    inliers = points[distances < threshold, :]
    normal = np.cross(inliers[1]-inliers[0], inliers[2]-inliers[0])
    normal /= np.linalg.norm(normal)
    return normal, point

# 运行RANSAC算法
normal, point = ransac(points, n_iterations=100, threshold=0.01)

# 可视化结果
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(points[:,0], points[:,1], points[:,2], s=2, c='b')
x, y = np.meshgrid(np.linspace(-1, 1, 10), np.linspace(-1, 1, 10))
z = (-normal[0] * x - normal[1] * y + point[2]) / normal[2]
ax.plot_surface(x, y, z, alpha=0.5)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()

其中，点云数据存储在名为point_cloud.pcd的文件中，格式为：

# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 1000
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 1000
DATA ascii
0.352222 0.356944 0.02
0.343056 0.368056 0.02
0.343056 0.368056 0.02
...

ransac()函数实现了RANSAC算法，其中参数points为点云数据，n_iterations为迭代次数，threshold为距离阈值。该函数返回一个包含平面法向量和平面上任意一点的元组。

最后，通过plt.scatter()和plt.plot_surface()函数可视化点云和拟合的平面