-
Notifications
You must be signed in to change notification settings - Fork 0
/
visualizer.py
50 lines (38 loc) · 5.21 KB
/
visualizer.py
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
import numpy as np
import open3d as o3d
import json
with open("data/r_t_timestamps_zed.json") as f:
R_T_dic = json.load(f)["R_T_DICT"]
# R_T_dic = {"0": [[[0.999881386756897, -0.015189755707979202, -0.00194762391038239], [0.009991499595344067, 0.7434438467025757, -0.6687236428260803], [0.01160570327192545, 0.6686257123947144, 0.7435088157653809]], [-0.00012604006042238325, 6.076310819480568e-05, 8.275053551187739e-05]], "1": [[[0.999880850315094, -0.01523471251130104, -0.0018577614100649953], [0.010082590393722057, 0.7433027625083923, -0.6688791513442993], [0.01157106552273035, 0.668781578540802, 0.7433691620826721]], [-0.00011593867384362966, 7.310221553780138e-05, 5.814027099404484e-05]], "2": [[[0.999880850315094, -0.01523471251130104, -0.0018577614100649953], [0.010082590393722057, 0.7433027625083923, -0.6688791513442993], [0.01157106552273035, 0.668781578540802, 0.7433691620826721]], [-0.00010843577911145985, 8.290143159683794e-05, 4.265885945642367e-05]], "3": [[[0.9993689060211182, -0.030283628031611443, -0.0184954646974802], [0.010146091692149639, 0.7433222532272339, -0.6688563823699951], [0.03400352969765663, 0.6682475209236145, 0.7431617379188538]], [-0.0007476437604054809, -0.00014476639626082033, 0.0008399677462875843]], "4": [[[0.9977330565452576, -0.05173853412270546, -0.04300107806921005], [0.00977784302085638, 0.7439153790473938, -0.6682022213935852], [0.06656105071306229, 0.6662678718566895, 0.7427362203598022]], [-0.001881624455563724, -0.0016572041204199195, 0.0012452759547159076]], "5": [[[0.9928941130638123, -0.08614709228277206, -0.08208024501800537], [0.009392757900059223, 0.7444025874137878, -0.6676648259162903], [0.11861831694841385, 0.6621503829956055, 0.7399234771728516]], [-0.006026501767337322, -0.003710590535774827, 0.002732448512688279]], "6": [[[0.9885755181312561, -0.10717130452394485, -0.1059718206524849], [0.00914453063160181, 0.7444713711738586, -0.6675915718078613], [0.15043988823890686, 0.6589965224266052, 0.7369475364685059]], [-0.008640835992991924, -0.004498380236327648, 0.00292348675429821]], "7": [[[0.9842954277992249, -0.12414906173944473, -0.1254856139421463], [0.008881825022399426, 0.7448140382766724, -0.6672127842903137], [0.17629757523536682, 0.6556208729743958, 0.7342211008071899]], [-0.011199848726391792, -0.005956065375357866, 0.0030975155532360077]], "8": [[[0.9794574975967407, -0.14076001942157745, -0.1443844884634018], [0.008656717836856842, 0.7447294592857361, -0.6673101186752319], [0.20145829021930695, 0.6523528695106506, 0.7306506633758545]], [-0.01432793214917183, -0.006793368142098188, 0.0037044319324195385]], "9": [[[0.9685923457145691, -0.17146824300289154, -0.18006914854049683], [0.008255688473582268, 0.7459682822227478, -0.6659300327301025], [0.2485121488571167, 0.6435289978981018, 0.7239560484886169]], [-0.018564525991678238, -0.010650238953530788, 0.00277972430922091]], "10": [[[0.9551836252212524, -0.2024911791086197, -0.2159142643213272], [0.00784297101199627, 0.7464730739593506, -0.6653692722320557], [0.2959060072898865, 0.6338571906089783, 0.7146081924438477]], [-0.025313641875982285, -0.013390068896114826, 0.0007428587996400893]], "11": [[[0.941616415977478, -0.22908522188663483, -0.24673022329807281], [0.007463390007615089, 0.7468475103378296, -0.6649531722068787], [0.3366013169288635, 0.6242902278900146, 0.7049548625946045]], [-0.03052765689790249, -0.015382397919893265, -0.0016597277717664838]], "12": [[[0.9299376010894775, -0.24923273921012878, -0.27036339044570923], [0.006731989327818155, 0.7466712594032288, -0.6651588678359985], [0.3676525056362152, 0.616736888885498, 0.6960367560386658]], [-0.03602632135152817, -0.01721910759806633, -0.0018034223467111588]]}
print("Load a ply point cloud, print it, and render it")
print(R_T_dic["0"][1])
#final_pcd = o3d.io.read_point_cloud("mesh_gen.ply")
#final_pcd = o3d.io.read_point_cloud("data/calib_image_zed_02.ply")
final_pcd = None
mesh = o3d.geometry.TriangleMesh.create_coordinate_frame()
for i in range(5):
path = "data/calib_image_zed_0" + str(i) + ".ply"
temp_pcd = o3d.io.read_point_cloud(path)
#cent = np.add(R_T_dic[str(i)][1],R_T_dic[str(i)][1]) / 2
#diff_rotate = np.subtract(R_T_dic[str(i)][0], R_T_dic[str(i)][0])
#temp_pcd = temp_pcd.rotate(temp_pcd.get_rotation_matrix_from_xyz([np.pi/2,0,0]))
final_pcd = temp_pcd if final_pcd == None else final_pcd + temp_pcd
#final_pcd = final_pcd.rotate(np.asarray([90, 0, 90], dtype=np.float64))
trans = np.array(R_T_dic[str(i)][1])
#final_pcd = final_pcd.translate(trans)
#final_pcd = final_pcd.rotate(np.linalg.inv(R_T_dic[str(i)][0]), center=[0,0,0])
#trans = np.zeros((4,4))
#trans[-1][-1] = 1
#trans[:3,:3] = R_T_dic[str(i)][0]
#print(trans)
#final_pcd = final_pcd.transform(trans)
o3d.visualization.draw_geometries([final_pcd, mesh])
#o3d.visualization.draw_geometries([final_pcd],
# zoom=0.3412,
# front=[0.7, -0.2125, -0.8795],
# lookat=[2.6172, 2.0475, 1.532],
# up=[-0.0694, -0.9768, 0.2024])
#pcd.points = o3d.utility.Vector3dVector(your_pointCloud)
#pcd.colors = o3d.utility.Vector3dVector(np_colors)
#o3d.visualization.draw_geometries([pcd])
#