平面拟合
1. 平面拟合
- 输入：
输入一系列三维点\((x_i, y_i, z_i)\) - 求解：
平面方程 \(ax+by+cz+d=0\)
2. 求解方法
2.1 基于最小二乘法与奇异值分解（SVD）
一个平面一般由法向量\(n=[a,b,c]^T\)和距离d来描述
\[ax+by+cz+d=0
\]
约束条件为：
\[a^2+b^2+c^2=1
\]
为了使得拟合的平面是最佳的（即，求解最优的平面参数 abcd），对于 n个点云数据，就是使得所有点云到该平面的距离平方和最小，即满足：
\[e=\sum_{i=1}^nd_i^2\to\min
\]
式中，\(d_i=|ax_i+by_i+cz_i+d|\)是点云数据中任意一点\(p_i=[x_i,y_i,z_i]^T\)到这个平面的距离，要使得\(e\to min\)，可以使用SVD矩阵分解得到。
设所有点的平均坐标为\(\bar{p} = [\bar{x},\bar{y},\bar{z}]^T\)，则：
\[a\bar{x}+b\bar{y}+c\bar{z}+d=0
\]
得：
\[a(x_i - \bar{x}) + b(y_i - \bar{y}) + c(z_i - \bar{z})=0
\]
整合得：
\[\begin{bmatrix}
x_1 - \bar{x} & y_1 - \bar{y} & z_1 - \bar{z}\\
x_2 - \bar{x} & y_2 - \bar{y} & z_2 - \bar{z}\\
\vdots & \vdots & \vdots \\
x_n - \bar{x} & y_n - \bar{y} & z_n - \bar{z}\\
\end{bmatrix}
\begin{bmatrix}
a\\b\\c
\end{bmatrix}
=0
\to AX=0
\]
理想情况下所有点都在平面上,式
成立，实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为：
\[ min|||AX|| \space \space \space \space s.t.||X||=1
\]
对A做奇异值分解：
\[ A=UDV^T
\]
则：
\[ |||AX||=|| UDV^T X ||=||DV^TX||
\]
其中，\(V^TX\)为列矩阵，并且
\[ ||V^TX||=||X||=1
\]
因为 D 的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当：
\[ V^TX=\begin{bmatrix}
0\\0\\\dots\\1
\end{bmatrix}
\]
时，式 可以取得最小值，即式 成立了。此时有：
\[X=V\begin{bmatrix}
0\\0\\\dots\\1
\end{bmatrix}
=\begin{bmatrix}
v_1 & v_2 &\dots&v_n
\end{bmatrix}
\begin{bmatrix}
0\\0\\\dots\\1
\end{bmatrix}
\]
因此，\(X=(a,b,c)=(v_{n,1}, v_{n,2}, v_{n,3})\)所以，e的最小值就是矩阵A的最小特征值，对应的特征向量为平面参数 \(a,b,c\)，利用质心可求得 d 。
2.2 基于随机抽样一致性（RANSAC）
- 原理：通过迭代随机采样，剔除离群点，拟合鲁棒平面模型。
- 步骤：
- 随机选择3个点（不共线），计算初始平面方程 \(Ax+By + Cz + D=0\)
- 计算所有点到平面的距离：
\(d_i = \frac{|ax_i+by_i+cz_i - d|}{\sqrt{a^2+b^2+c^2}}\)` - 设定距离阈值 \(t\)(可基于标准偏差\(\sigma\)，取\(t=2\sigma\)），统计内点\(d_i<t\)
- 重复迭代，选择内点最多的平面模型。
- 用内点通过最小二乘法重新拟合平面
2.3 多个平面的拟合
-
RANSAC算法：通过随机采样点云拟合平面模型，根据距离阈值区分内点（属于平面）和外点（噪声或其它平面）。迭代选择内点最多的平面，移除内点后继续拟合其他平面。此方法对噪声和离群点鲁棒性强，适合多平面场景 。
-
法向量聚类：先计算点云法向量，再通过法向量方向（如夹角阈值）聚类点云，将属于同一平面的点归为一组。可结合RANSAC提升精度
3. 实现过程
生成一个倾斜立方体，然后拟合，查看拟合效果
import os
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math
import random
def generate_tilted_cube(angle=45):
"""生成一个倾斜指定角度的立方体"""
# 定义立方体的8个顶点
vertices = np.array([
[1, 1, 1], [1, 1, -1], [1, -1, -1], [1, -1, 1],
[-1, 1, 1], [-1, -1, 1], [-1, -1, -1], [-1, 1, -1]
])
# 将角度转换为弧度
theta = angle * np.pi / 180
# 创建旋转矩阵（绕y轴旋转45°）
rotation_matrix = np.array([
[np.cos(theta), 0, np.sin(theta)],
[0, 1, 0],
[-np.sin(theta), 0, np.cos(theta)]
])
# 应用旋转变换
rotated_vertices = np.dot(vertices, rotation_matrix)
return rotated_vertices
def generate_face_samples(face_vertices, num_samples=50):
"""在单个平面上生成均匀采样点"""
# 创建参数网格 (u, v ∈ [0,1])
u = np.linspace(0, 1, num_samples)
v = np.linspace(0, 1, num_samples)
u_grid, v_grid = np.meshgrid(u, v)
u_grid = u_grid.flatten()
v_grid = v_grid.flatten()
# 双线性插值生成平面上的点
v0, v1, v2, v3 = face_vertices
samples = (1 - u_grid[:, None]) * (1 - v_grid[:, None]) * v0 + \
u_grid[:, None] * (1 - v_grid[:, None]) * v1 + \
u_grid[:, None] * v_grid[:, None] * v2 + \
(1 - u_grid[:, None]) * v_grid[:, None] * v3
return samples
def plot_tilted_cube():
"""绘制倾斜45°的立方体及其平面采样点"""
cube_vertices = generate_tilted_cube(45)
# 定义立方体的12条边
edges = [
[0, 1], [1, 2], [2, 3], [3, 0], # 前面
[4, 5], [5, 6], [6, 7], [7, 4], # 后面
[0, 4], [1, 7], [2, 6], [3, 5] # 连接前后
]
# 定义立方体的6个平面（每个平面由4个顶点索引组成）
faces = [
[0, 1, 2, 3], # 右面 (x=1)
[4, 5, 6, 7], # 左面 (x=-1)
[0, 3, 5, 4], # 前面 (z=1)
[1, 2, 6, 7], # 后面 (z=-1)
[0, 1, 7, 4], # 顶面 (y=1)
[3, 2, 6, 5] # 底面 (y=-1)
]
# 在每个平面上生成并绘制均匀采样点
num_samples = 50 # 每个平面的采样点数（沿边方向）
sample_list = []
for i, face in enumerate(faces):
# 获取平面的4个顶点
face_verts = cube_vertices[face]
# 生成均匀采样点
samples = generate_face_samples(face_verts, num_samples)
sample_list += samples.tolist()
return sample_list
def SVD(points):
# 二维，三维均适用
# 二维直线，三维平面
pts = points.copy()
# 奇异值分解
c = np.mean(pts, axis=0)
A = pts - c # shift the points
A = A.T # 3*n
u, s, vh = np.linalg.svd(A, full_matrices=False, compute_uv=True) # A=u*s*vh
normal = u[:, -1]
# 法向量归一化
nlen = np.sqrt(np.dot(normal, normal))
normal = normal / nlen
# normal 是主方向的方向向量 与PCA最小特征值对应的特征向量是垂直关系
# u 每一列是一个方向
# s 是对应的特征值
# c >>> 点的中心
# normal >>> 拟合的方向向量
return u, s, c, normal
class plane_model(object):
def __init__(self):
self.parameters = None
def calc_inliers(self, points, dst_threshold):
c = self.parameters[0:3]
n = self.parameters[3:6]
dst = abs(np.dot(points - c, n))
ind = dst < dst_threshold
return ind
def estimate_parameters(self, pts):
num = pts.shape[0]
if num == 3:
c = np.mean(pts, axis=0)
l1 = pts[1] - pts[0]
l2 = pts[2] - pts[0]
n = np.cross(l1, l2)
scale = [n[i] ** 2 for i in range(n.shape[0])]
# print(scale)
n = n / np.sqrt(np.sum(scale))
else:
_, _, c, n = SVD(pts)
params = np.hstack((c.reshape(1, -1), n.reshape(1, -1)))[0, :]
self.parameters = params
return params
def set_parameters(self, parameters):
self.parameters = parameters
def ransac_planefit(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None):
# RANSAC 平面拟合
pts = points.copy()
num = pts.shape[0]
cc = np.mean(pts, axis=0)
iter_max = max_trials
best_inliers_ratio = 0 # 符合拟合模型的数据的比例
best_plane_params = None
best_inliers = None
best_remains = None
for i in range(iter_max):
sample_index = random.sample(range(num), ransac_n)
sample_points = pts[sample_index, :]
plane = plane_model()
plane_params = plane.estimate_parameters(sample_points)
# 计算内点 外点
index = plane.calc_inliers(points, max_dst)
inliers_ratio = pts[index].shape[0] / num
if inliers_ratio > best_inliers_ratio:
best_inliers_ratio = inliers_ratio
best_plane_params = plane_params
bset_inliers = pts[index]
bset_remains = pts[index == False]
if best_inliers_ratio > stop_inliers_ratio:
# 检查是否达到最大的比例
print("iter: %d\n" % i)
print("best_inliers_ratio: %f\n" % best_inliers_ratio)
break
return best_plane_params, bset_inliers, bset_remains
def ransac_plane_detection(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None,
out_layer_inliers_threshold=230, out_layer_remains_threshold=230):
inliers_num = out_layer_inliers_threshold + 1
remains_num = out_layer_remains_threshold + 1
plane_set = []
plane_inliers_set = []
plane_inliers_num_set = []
data_remains = np.copy(points)
i = 0
while inliers_num > out_layer_inliers_threshold and remains_num > out_layer_remains_threshold:
# robustly fit line only using inlier data with RANSAC algorithm
best_plane_params, pts_inliers, pts_outliers = ransac_planefit(data_remains, ransac_n, max_dst,
max_trials=max_trials,
stop_inliers_ratio=stop_inliers_ratio)
inliers_num = pts_inliers.shape[0]
remains_num = pts_outliers.shape[0]
if inliers_num > out_layer_inliers_threshold:
plane_set.append(best_plane_params)
plane_inliers_set.append(pts_inliers)
plane_inliers_num_set.append(inliers_num)
i = i + 1
print('------------> %d <--------------' % i)
print(best_plane_params)
data_remains = pts_outliers
# sorting
plane_set = [x for _, x in sorted(zip(plane_inliers_num_set, plane_set), key=lambda s: s[0], reverse=True)]
plane_inliers_set = [x for _, x in
sorted(zip(plane_inliers_num_set, plane_inliers_set), key=lambda s: s[0], reverse=True)]
return plane_set, plane_inliers_set, data_remains
def show_3dpoints(pointcluster, s=None, colors=None, quiver=None, q_length=10, tri_face_index=None):
# pointcluster should be a list of numpy ndarray
# This functions would show a list of pint cloud in different colors
n = len(pointcluster)
if colors is None:
colors = ['r', 'g', 'b', 'c', 'm', 'y', 'k', 'tomato', 'gold']
if n < 10:
colors = np.array(colors[0:n])
else:
colors = np.random.rand(n, 3)
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
if s is None:
s = np.ones(n) * 2
for i in range(n):
ax.scatter(pointcluster[i][:, 0], pointcluster[i][:, 1], pointcluster[i][:, 2], s=s[i], c=[colors[i]],
alpha=0.6)
if not (quiver is None):
c1 = [random.random() for _ in range(len(quiver))]
c2 = [random.random() for _ in range(len(quiver))]
c3 = [random.random() for _ in range(len(quiver))]
c = []
for i in range(len(quiver)):
c.append((c1[i], c2[i], c3[i]))
cp = []
for i in range(len(quiver)):
cp.append(c[i])
cp.append(c[i])
c = c + cp
ax.quiver(quiver[:, 0], quiver[:, 1], quiver[:, 2], quiver[:, 3], quiver[:, 4], quiver[:, 5], length=q_length,
arrow_length_ratio=.2, pivot='tail', normalize=False, color=c)
if not (tri_face_index is None):
for i in range(len(tri_face_index)):
for j in range(tri_face_index[i].shape[0]):
index = tri_face_index[i][j].tolist()
index = index + [index[0]]
ax.plot(*zip(*pointcluster[i][index]))
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# ax.set_ylim([-20,60])
plt.show()
return 0
# 如果直接运行该文件，则显示立方体
if __name__ == "__main__":
sample_list = plot_tilted_cube()
plane_set, plane_inliers_set, data_remains = ransac_plane_detection(sample_list, 3, 0.01, max_trials=1000,
stop_inliers_ratio=1.0 / 6.0, # 每个面约占总点数的1/6
initial_inliers=None,
out_layer_inliers_threshold=2300,
# 降低内点数量阈值（每个面有2500个点）
out_layer_remains_threshold=1000)
plane_set = np.array(plane_set)
print("================= 平面参数 ====================")
print(plane_set)
# 绘图
show_3dpoints(plane_inliers_set)
print("over!!!")
拟合的六个平面效果如下：
浙公网安备 33010602011771号