三维点云数据投影到xy或者yz或者xy平面
- 示例如下：
"""
将3d点云投影到xyz等不同平面上
"""
import numpy as np
import open3d as o3d
import matplotlib.pyplot as plt
import mayavi.mlab as mlab
from PyQt5.QtWidgets import QApplication
desktop = QApplication.desktop()
width = desktop.width()
height = desktop.height()
def show_3Dpoint_cloud(pointcloud):
x = pointcloud[:, 0] # x position of point
y = pointcloud[:, 1] # y position of point
z = pointcloud[:, 2] # z position of point
fig = mlab.figure(figure="点云展示", bgcolor=(0, 0, 0), size=(width, height))
mlab.points3d(x, y, z,
mode="point",
colormap='spectral', # 'bone', 'copper', 'gnuplot'
# color=(0, 1, 0), # Used a fixed (r,g,b) instead
figure=fig,
)
# 添加坐标系
mlab.axes(xlabel='x', ylabel='y', zlabel='z')
"""将点云数据投影到xyz平面时的坐标计算
Args
points：三维点云数据
flat：由Ax+By+Cz+D=0表征的平面
xy平面：(0 0 1 0) | xz平面：(0 1 0 0) | yz平面：(1 0 0 0)
Returns
x,y,z：投影后的坐标
"""
def coordinate_caculate(points, flat):
A, B, C, D = flat
# 投影后的坐标计算
distance = A**2 + B**2 + C**2
t = -(A*points[:, 0] + B*points[:, 1] + C*points[:, 2] + D)/distance
x = A*t + points[:, 0]
y = B*t + points[:, 1]
z = C*t + points[:, 2]
return (x, y, z)
"""将点云数据投影到xy平面或者xz平面或者yz平面并展示
Args
points：三维点云数据
flat：由Ax+By+Cz+D=0表征的平面
xy平面：(0 0 1 0) | xz平面：(0 1 0 0) | yz平面：(1 0 0 0)
"""
def project_2d_any(points, flat):
x, y, z, _ = flat
x_transform, y_transform, z_transform = coordinate_caculate(points, flat)
project_point = np.array([x_transform, y_transform, z_transform]).T
print(project_point.shape)
mlab.figure(figure="投影到平面展示", bgcolor=(0, 0, 0), size=(width, height))
# 计算点云中的最大值和最小值点
min_point = np.min(project_point, axis=0)
max_point = np.max(project_point, axis=0)
print("min_point", min_point)
print("max_point", max_point)
# 第四参数This scalar value can be used to modulate the color and the size of the points.
if (x == 1):
fourth_param = y_transform
min_point_proj = [min_point[1], min_point[2], 0]
max_point_proj = [max_point[1], max_point[2], 0]
elif (y == 1):
fourth_param = x_transform
min_point_proj = [min_point[0], min_point[2], 0]
max_point_proj = [max_point[0], max_point[2], 0]
elif (z == 1):
fourth_param = y_transform
min_point_proj = [min_point[0], min_point[1], 0]
max_point_proj = [max_point[0], max_point[1], 0]
mlab.points3d(x_transform, y_transform, z_transform, fourth_param, mode='point', colormap='spectral')
# 添加坐标系
mlab.axes(xlabel='x', ylabel='y', zlabel='z')
# 绘制最小值点和最大值点
mlab.points3d(min_point_proj[0], min_point_proj[1], min_point_proj[2], scale_factor=0.1, color=(0, 1, 0))
mlab.points3d(max_point_proj[0], max_point_proj[1], max_point_proj[2], scale_factor=0.1, color=(1, 0, 0))
mlab.show()
def project_2d_all(points):
A1, B1, C1, D1 = (0, 1, 0, 0)
distance1 = A1**2 + B1**2 + C1**2
t1 = -(A1*points[:, 0] + B1*points[:, 1] + C1*points[:, 2] + D1)/distance1
x1 = A1*t1 + points[:, 0]
y1 = B1*t1 + points[:, 1]
z1 = C1*t1 + points[:, 2]
project_point_xz = np.array([x1, y1, z1]).T
# print(project_point_xz.shape)
# print(project_point_xz)
A2, B2, C2, D2 = (1, 0, 0, 0)
distance2 = A2**2 + B2**2 + C2**2
t2 = -(A2*points[:, 0] + B2*points[:, 1] + C2*points[:, 2] + D2)/distance2
x2 = A2*t2 + points[:, 0]
y2 = B2*t2 + points[:, 1]
z2 = C2*t2 + points[:, 2]
project_point_yz = np.array([x2, y2, z2]).T
# print(project_point_yz.shape)
A3, B3, C3, D3 = (0, 0, 1, 0)
distance3 = A3**2 + B3**2 + C3**2
t3 = -(A3*points[:, 0] + B3*points[:, 1] + C3*points[:, 2] + D3)/distance3
x3 = A3*t3 + points[:, 0]
y3 = B3*t3 + points[:, 1]
z3 = C3*t3 + points[:, 2]
project_point_xy = np.array([x3, y3, z3]).T
# print(project_point_xy.shape)
c = np.vstack((project_point_xy,project_point_xz,project_point_yz))
# print(c.shape)
# 投影展现
fig = plt.figure(figsize=(width, height))
# 第一个子图
# 121表示将图形分为 1 行 2 列的子图，当前操作的是第一个子图
ax = fig.add_subplot(121,projection='3d')
# 三维散点图，红色
ax.scatter(c.T[0], c.T[1],c.T[2], c='r')
ax.set_title('3D Scatter Plot')
# 二维散点图，蓝色
# 122表示将图形分为 1 行 2 列的子图，当前操作的是第一个子图
ax = fig.add_subplot(122)
ax.scatter(c.T[0], c.T[2], c='b')
ax.set_title('2D Scatter Plot')
# 设置整个图形的标题
plt.suptitle('Point Cloud Visualization', fontsize=16, fontweight='bold')
#plt.savefig(savepath)
plt.show()
# 将三维点云投影到xyz平面
if __name__ == '__main__':
# pcdpath = "datasets/surface_points.pcd"
pcdpath = "datasets/rabbit.pcd"
points = o3d.io.read_point_cloud(pcdpath)
#输出点云的个数
# print(points)
#输出点的三维坐标
# print(np.asarray(points.points))
pointcloud = np.asarray(points.points)
show_3Dpoint_cloud(pointcloud)
#xy平面： 0 0 1 0
#xz平面： 0 1 0 0
#yz平面： 1 0 0 0
# 投影到xy平面示例
# project_2d_any(pointcloud, (0, 0, 1, 0))
# project_2d_any(pointcloud, (0, 1, 0, 0))
# project_2d_any(pointcloud, (1, 0, 0, 0))
project_2d_all(pointcloud)
三维点云数据投影到任意xyz平面
- 示例如下：
"""
将3D点云投影到任意平面
"""
import numpy as np
import copy
import open3d as o3d
import matplotlib.pyplot as plt
import mayavi.mlab as mlab
from PyQt5.QtWidgets import QApplication
desktop = QApplication.desktop()
width = desktop.width()
height = desktop.height()
def project_2d_any(points, flat):
"""
Args:
points：三维点云数据
flat：任意平面
Returns:
投影到指定平面的三维点云数据
"""
A, B, C, D = flat
points = copy.deepcopy(points)
x = points[:, 0]
y = points[:, 1]
z = points[:, 2]
t = (A * x + B * y + C * z + D) / (A * A + B * B + C * C)
points[:, 0] = x - A * t
points[:, 1] = y - B * t
points[:, 2] = z - C * t
return np.unique(points, axis=0)
def show_3Dpoint_cloud(points_source, points):
fig = plt.figure(figsize=(width, height))
# 第一个子图
ax = fig.add_subplot(131,projection='3d')
# 绿色的原三维散点图
ax.scatter(points_source.T[0], points_source.T[1], points_source.T[2], c='g', marker='.')
ax.set_title('source 3D Scatter Plot')
# 第二个子图
# 132表示将图形分为 1 行 3 列的子图，当前操作的是第二个子图
ax = fig.add_subplot(132,projection='3d')
# 红色的三维散点图，表示投影到指定平面的三维表示
ax.scatter(points.T[0], points.T[1], points.T[2], c='r')
ax.set_title('3D Scatter Plot')
# 133表示将图形分为 1 行 3 列的子图，当前操作的是第三个子图
ax = fig.add_subplot(133)
# 蓝色的二维散点图，表示投影到指定平面的二维表示
ax.scatter(points.T[0], points.T[2], c='b')
ax.set_title('2D Scatter Plot')
# 设置整个图形的标题
plt.suptitle('Point Cloud Visualization', fontsize=16, fontweight='bold')
#plt.savefig(savepath)
plt.show()
if __name__ == '__main__':
pcdpath = "datasets/surface_points.pcd"
# pcdpath = "datasets/rabbit.pcd"
points = o3d.io.read_point_cloud(pcdpath)
#输出点云的个数
# print(points)
#输出点的三维坐标
# print(np.asarray(points.points))
pointcloud_source = np.asarray(points.points)
#xy平面： 0 0 1 0
#xz平面： 0 1 0 0
#yz平面： 1 0 0 0
# 投影到xy平面示例
# pointcloud = project_2d_any(pointcloud_source, (0, 0, 1, 0))
pointcloud = project_2d_any(pointcloud_source, (0, 1, 0, 0))
# pointcloud = project_2d_any(pointcloud_source, (1, 0, 0, 0))
# 可视化
show_3Dpoint_cloud(pointcloud_source, pointcloud)
浙公网安备 33010602011771号