一、算法原理
拟合出的平面方程为:
a
x
+
b
y
+
c
z
+
d
=
0
ax+by+cz+d=0
ax+by+cz+d=0
约束条件:
a
2
+
b
2
+
c
2
=
1
a^2+b^2+c^2 = 1
a2+b2+c2=1
可以得到平面参数a
,b
,c
,d
。此时,要使获得的拟合平面是最佳的,就是使得点到该平面的距离的平方和最小,即满足:
e
=
∑
i
=
1
N
a
i
2
−
>
m
i
n
e = \sum_{i=1}^{N}{a_i^2}->min
e=i=1∑Nai2−>min
d i 是点云数据中的任一点 p i p i ( x i , y i , z i ) 到这个平面距离 d i = ∣ a x i + b y i + c z i + d ∣ 要使 e − > m i n ,可以使用 S V D 矩阵分解得到 d_i是点云数据中的任一点p_ip_i(x_i, y_i, z_i)到这个平面距离d_i = |ax_i + by_i + cz_i + d|要使e->min,可以使用\\SVD矩阵分解得到 di是点云数据中的任一点pipi(xi,yi,zi)到这个平面距离di=∣axi+byi+czi+d∣要使e−>min,可以使用SVD矩阵分解得到
推导过程如下:
所以点平均坐标
(
x
,
y
,
z
)
带入到方程中
a
x
+
b
y
+
c
z
+
d
=
0
所以点平均坐标(\frac{}{x}, \frac{}{y}, \frac{}{z})\\ \\带入到方程中 a\frac{}{x}+b\frac{}{y}+c\frac{}{z}+d = 0
所以点平均坐标(x,y,z)带入到方程中ax+by+cz+d=0
则可得函数
a
(
x
i
−
x
)
+
b
(
y
i
−
y
)
+
c
(
z
i
−
z
)
=
0
a(x_i - \frac{}{x}) + b(y_i - \frac{}{y}) + c(z_i - \frac{}{z}) = 0
a(xi−x)+b(yi−y)+c(zi−z)=0
理想情况下所有点都在平面上;实际情况下有部分点在平面外,拟合的目的为平面距离所有点的距离之和尽量小,所以目标函数为:
所以, e的最小值就是矩阵 A 的最小特征值,对应的特征向量为平面参数a
,b
,c
,d
,利用质心可求得 d
。
二、代码
import open3d as o3d
import numpy as np
def plane(pcd, normal_vector):
"""
Args:将点云投影到平面
pcd: 点云数据
normal_vector: 方程法向量 mx + ny + sz + d = 0 传入[m , n, s, d]
Returns: 投影后的点云数据
"""
plane_seeds = [] # 保存投影后的点云数据
# 获取平面系数
m = normal_vector[0]
n = normal_vector[1]
s = normal_vector[2]
d = normal_vector[3]
# 将点云转换为数组
points = np.asarray(pcd.points)
for xyz in points:
x, y, z = xyz
"""
t = -(m*x + n*y + s*z + d) / (m*m + n*n + s*s) # 计算参数方程参数
"""
t = -(m * x + n * y + s * z + d) / (m * m + n * n + s * s) # 计算参数方程参数
"""
xi = m*t + x # 计算x的投影
yi = b*t + y # 计算y的投影
zi = s*t + z # 计算z的投影
"""
xi = m * t + x # 计算x的投影
yi = n * t + y # 计算y的投影
zi = s * t + z # 计算z的投影
plane_seeds.append([xi, yi, zi]) # 将投影后的点云添加到数组中
plane_cloud = o3d.geometry.PointCloud() # 使用numpy生成点云
plane_cloud.points = o3d.utility.Vector3dVector(plane_seeds) # points numpy数组
return plane_cloud
if __name__ == '__main__':
""" 拟合出的平面方程为: ax + by +cz + d = 0"""
"""约束条件 a^2 + b^2 + c^2 = 1"""
pcd = o3d.io.read_point_cloud('res/bunny.pcd')
points = np.asarray(pcd.points) # 将点云文件转换为数组
# 获得平均值
avg_x = np.mean(points[:, 0])
avg_y = np.mean(points[:, 1])
avg_z = np.mean(points[:, 2])
# 去重心化
x = points[:, 0] - avg_x
y = points[:, 1] - avg_y
z = points[:, 2] - avg_z
# 生成矩阵
A = np.transpose(np.asarray([x, y, z])) # 将数据矩阵化
"""AX = 0"""
# 计算协方差矩阵
cov_matrix = np.cov(A, rowvar=False)
# 使用 SVD 分解求解协方差矩阵的特征值和特征向量
U, S, Vt = np.linalg.svd(cov_matrix)
# 特征值和特征向量
eigenvalues = S
eigenvectors = Vt.T # 转置后得到正确的特征向量 一行代表一个特征向量 最小的特征向量在最后一行
print("特征值:", eigenvalues)
print("特征向量:", eigenvectors)
"""如果想要找到最小的特征向量,可以直接从特征向量矩阵中取最后一列(没有转置前),
因为特征向量是按特征值从大到小排列的,所以最后一列就是对应最小特征值的特征向量。"""
X = eigenvectors[2]
center = pcd.get_center() # 获得中心(质心)质心是一个几何概念,通常用于描述几何图形(如点集、线段、多边形等)的中心位置。
print('平面拟合结果为:0 = %.3f * x + %.3f * y + %.3f + %3.f' % (X[0], X[1], X[2], (-X[0]*center[0] - X[1]*center[1] - X[2]*center[2])))
plane_cloud = plane(pcd, (X[0], X[1], X[2], (-X[0]*center[0] - X[1]*center[1] - X[2]*center[2]))) # 获得投影后的点云数据
# ------------------ 可视化点云 -----------------
plane_cloud.paint_uniform_color([1, 0, 0.0]) # 渲染颜色
o3d.visualization.draw_geometries([plane_cloud])
o3d.visualization.draw_geometries([pcd])
三、结果
1.原点云数据
2.svd分解后投影平面点云数据
四、相关数据
svd
分解法:最小二乘拟合平面(python/C++版) - 知乎 (zhihu.com)
奇异值分解(SVD):奇异值分解(SVD) - 知乎 (zhihu.com)