open3d 将点云投影到平面
一、算法原理
假设点（x0, y0, z0）, 平面方程为 mx + ny + sz + d = 0
过点（x0, y0, z0），且垂直平面的直线方程为
x − x 0 m = y − y 0 n = z − z 0 s \frac{x- x0}{m} = \frac{y- y0}{n}=\frac{z- z0}{s} mx−x0=ny−y0=sz−z0
点到平面的投影就是上述直线与平面的交点 ，注意到直线的参数方程为
x = m t + x 0 , y = n t + y 0 , z = s t + z 0 x = mt + x0,\quad y = nt + y0,\quad z = st + z0 x=mt+x0,y=nt+y0,z=st+z0
代入平面方程求解t
t = − m x + n y + s z + d m ∗ m + n ∗ n + s ∗ s t=-\frac{mx + ny + sz + d}{m*m + n*n + s*s} t=−m∗m+n∗n+s∗smx+ny+sz+d
将t带入到平面方程得到投影。。结合代码看易于理解。
二、代码
python
import numpy as np
import open3d as o3d
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__':
# -------------------读取点云数据------------------------
cloud_size = 1000
a = np.random.ranf(cloud_size * 3).reshape(-1, 3) * 1024
pcd = o3d.geometry.PointCloud() # 使用numpy生成点云
pcd.points = o3d.utility.Vector3dVector(a) # points numpy数组
# pcd = o3d.io.read_point_cloud('res/bunny.pcd') # 读取兔子点云
plane_cloud = plane(pcd, [1, 0, 1, 0]) # 获得投影后的点云数据
# ------------------ 可视化点云 -----------------
plane_cloud.paint_uniform_color([1, 0, 0.0]) # 渲染颜色
o3d.visualization.draw_geometries([pcd, plane_cloud])三、结果展示
1.原点云
2.点云数据向x + z = 0投影
四、相关数据
参考文章
pclpy 参数模型投影：pclpy 参数模型投影-CSDN博客
点到平面的投影：点到平面的投影 - 知乎 (zhihu.com)