1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
| import math from math import sqrt import pandas as pd import open3d as o3d import numpy as np
pcd = o3d.io.read_point_cloud('/home/zpp/depth_image/models/data0200.ply') print(pcd) all_points = np.asarray(pcd.points) print(all_points) obb = pcd.get_oriented_bounding_box() obb.color = (0,1,0) eight_points = np.asarray(obb.get_box_points())
o3d.visualization.draw_geometries([pcd, obb]) pcd_points = np.asarray(pcd.points) min_x = np.min(eight_points[:, 0]) max_x = np.max(eight_points[:, 0]) min_y = np.min(eight_points[:, 1]) max_y = np.max(eight_points[:, 1]) min_z = np.min(eight_points[:, 2]) max_z = np.max(eight_points[:, 2])
""" 法向量n 三个点确定一个平面
""" point1 = np.array([min_x, min_y, max_z]) point2 = np.array([min_x, max_y, max_z]) point3 = np.array([max_x, min_y, max_z]) point4 = np.array([max_x, max_y, max_z]) r = 100 point_d = [] def point_face_d(point1, point2, point3, point4, r, all_points): AB = np.asmatrix(point2-point1) AC = np.asmatrix(point3-point1) N = np.cross(AB, AC) A = N[0, 0] B = N[0, 1] C = N[0, 2] D = -(A * point4[0] + B * point4[1] + C * point4[2]-r) for p in all_points: d = (A*p[0]+B*p[1]+C*p[2]+D)/sqrt(A**2+B**2+C**2) point_d.append(d) return r/C
translation_z = point_face_d(point1, point2, point3, point4, r, all_points) all_points[:, 2] = point_d print(all_points)
max_d = np.max(point_d) max_x = np.max(all_points[:, 0]) max_y = np.max(all_points[:, 1]) print(max_x, max_y)
t = 2.54 nor_point_d = [] nor_point_x = [] nor_point_y = [] result = [] for d in point_d: nor_point_d.append(round(d * 255 / max_d)) for x in all_points[:, 0]: nor_point_x.append(round(x * 100 * t / max_x)) for y in all_points[:, 1]: y = y + abs(min_y) nor_point_y.append(round(y * 100 / max_y)) img_depth = np.zeros((int(140*t), 140), np.uint8) for idx, d in enumerate(nor_point_d): a = nor_point_x[idx] b = nor_point_y[idx] tmp = (nor_point_x[idx], nor_point_y[idx], nor_point_d[idx]) result.append(tmp) img_depth[a, b] = d tmp = np.array(result) h = abs(max_z - min_z) + translation_z ori_z = [] for z in tmp: ori_value = h - z[2]/255*max_d ori_z.append(ori_value) ori_z = np.array(ori_z) tmp[:, 2] = ori_z from PIL import Image im = Image.fromarray(img_depth) im.save("out1.jpg")
point_cloud = o3d.geometry.PointCloud() point_cloud.points = o3d.utility.Vector3dVector(tmp)
o3d.io.write_point_cloud('/home/zpp/depth_image/models/data.ply', point_cloud) o3d.visualization.draw_geometries([point_cloud])
|