-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtransform_cube.py
157 lines (129 loc) · 4.8 KB
/
transform_cube.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
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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
import open3d as o3d
import cv2 as cv
import numpy as np
from scipy.spatial.transform import Rotation as R
import sys, os
import pandas as pd
def load_point_cloud(points3D_df):
xyz = np.vstack(points3D_df['XYZ'])
rgb = np.vstack(points3D_df['RGB'])/255
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
pcd.colors = o3d.utility.Vector3dVector(rgb)
return pcd
def load_axes():
axes = o3d.geometry.LineSet()
axes.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]])
axes.lines = o3d.utility.Vector2iVector([[0, 1], [0, 2], [0, 3]]) # X, Y, Z
axes.colors = o3d.utility.Vector3dVector([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # R, G, B
return axes
def get_transform_mat(rotation, translation, scale):
r_mat = R.from_euler('xyz', rotation, degrees=True).as_matrix()
scale_mat = np.eye(3) * scale
transform_mat = np.concatenate([scale_mat @ r_mat, translation.reshape(3, 1)], axis=1)
return transform_mat
def update_cube():
global cube, cube_vertices, R_euler, t, scale
print(cube_vertices)
transform_mat = get_transform_mat(R_euler, t, scale)
transform_vertices = (transform_mat @ np.concatenate([
cube_vertices.transpose(),
np.ones([1, cube_vertices.shape[0]])
], axis=0)).transpose()
print(transform_vertices)
cube.vertices = o3d.utility.Vector3dVector(transform_vertices)
print(np.asarray(cube.vertices))
cube.compute_vertex_normals()
cube.paint_uniform_color([1, 0.706, 0])
vis.update_geometry(cube)
def toggle_key_shift(vis, action, mods):
global shift_pressed
if action == 1: # key down
shift_pressed = True
elif action == 0: # key up
shift_pressed = False
return True
def update_tx(vis):
global t, shift_pressed
t[0] += -0.01 if shift_pressed else 0.01
update_cube()
def update_ty(vis):
global t, shift_pressed
t[1] += -0.01 if shift_pressed else 0.01
update_cube()
def update_tz(vis):
global t, shift_pressed
t[2] += -0.01 if shift_pressed else 0.01
update_cube()
def update_rx(vis):
global R_euler, shift_pressed
R_euler[0] += -1 if shift_pressed else 1
update_cube()
def update_ry(vis):
global R_euler, shift_pressed
R_euler[1] += -1 if shift_pressed else 1
update_cube()
def update_rz(vis):
global R_euler, shift_pressed
R_euler[2] += -1 if shift_pressed else 1
update_cube()
def update_scale(vis):
global scale, shift_pressed
scale += -0.05 if shift_pressed else 0.05
update_cube()
# if len(sys.argv) != 2:
# print('[Usage] python3 transform_cube.py /PATH/TO/points3D.txt')
# sys.exit(1)
vis = o3d.visualization.VisualizerWithKeyCallback()
vis.create_window()
# load point cloud
points3D_df = pd.read_pickle("data/points3D.pkl")
pcd = load_point_cloud(points3D_df)
vis.add_geometry(pcd)
# load axes
axes = load_axes()
vis.add_geometry(axes)
# load cube
cube = o3d.geometry.TriangleMesh.create_box(width=1.0, height=1.0, depth=1.0)
cube_vertices = np.asarray(cube.vertices).copy()
vis.add_geometry(cube)
R_euler = np.array([0, 0, 0]).astype(float)
t = np.array([0, 0, 0]).astype(float)
scale = 1.0
update_cube()
# just set a proper initial camera view
vc = vis.get_view_control()
vc_cam = vc.convert_to_pinhole_camera_parameters()
initial_cam = get_transform_mat(np.array([7.227, -16.950, -14.868]), np.array([-0.351, 1.036, 5.132]), 1)
initial_cam = np.concatenate([initial_cam, np.zeros([1, 4])], 0)
initial_cam[-1, -1] = 1.
setattr(vc_cam, 'extrinsic', initial_cam)
vc.convert_from_pinhole_camera_parameters(vc_cam)
# set key callback
shift_pressed = False
vis.register_key_action_callback(340, toggle_key_shift)
vis.register_key_action_callback(344, toggle_key_shift)
vis.register_key_callback(ord('A'), update_tx)
vis.register_key_callback(ord('S'), update_ty)
vis.register_key_callback(ord('D'), update_tz)
vis.register_key_callback(ord('Z'), update_rx)
vis.register_key_callback(ord('X'), update_ry)
vis.register_key_callback(ord('C'), update_rz)
vis.register_key_callback(ord('V'), update_scale)
print('[Keyboard usage]')
print('Translate along X-axis\tA / Shift+A')
print('Translate along Y-axis\tS / Shift+S')
print('Translate along Z-axis\tD / Shift+D')
print('Rotate along X-axis\tZ / Shift+Z')
print('Rotate along Y-axis\tX / Shift+X')
print('Rotate along Z-axis\tC / Shift+C')
print('Scale \tV / Shift+V')
vis.run()
vis.destroy_window()
'''
print('Rotation matrix:\n{}'.format(R.from_euler('xyz', R_euler, degrees=True).as_matrix()))
print('Translation vector:\n{}'.format(t))
print('Scale factor: {}'.format(scale))
'''
np.save('./cube/cube_transform_mat.npy', get_transform_mat(R_euler, t, scale))
np.save('./cube/cube_vertices.npy', np.asarray(cube.vertices))