-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmain.py
243 lines (188 loc) · 6.5 KB
/
main.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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
import os
from PIL import Image
from utils import *
from rays import *
from tsdf import *
def ProcessDepthImage(file_name, depth_factor):
"""
Process Depth Image
Parameters
----------
filename : string
input depth file
depth_factor : float
normalized depth value
Returns
-------
depth_img : ndarray of shape (480, 640)
filtered depth image
"""
depth_img = Image.open(file_name).convert('F')
depth_img = np.array(depth_img) / depth_factor
scale = np.max(depth_img)
d_ = depth_img / scale
d_ = cv2.bilateralFilter(d_, 5, 3, 0.01)
depth_img = d_ * scale
return depth_img
def Get3D(depth, K):
"""
Inverse Projection - create point cloud from depth image
Parameters
----------
depth : ndarray of shape (H, W)
filtered depth image
K : ndarray of shape (3, 3)
Intrinsic parameters
Returns
-------
point : ndarray of shape (3, H, W)
Point cloud from depth image
normal : ndarray of shape (3, H, W)
Surface normal
"""
# TODO Your code goes here
return point, normal
def CreateTSDF(depth, T, voxel_param, K):
"""
CreateTSDF : VoxelParams class' member function
Compute distance of each voxel w.r.t a camera
Parameters
----------
depth : ndarray of shape (H, W)
Filtered depth image
T : ndarray of shape (4, 4)
Transformation that brings camera to world coordinate
voxel_param : an instance of voxel parameter VoxelParams
K : ndarray of shape (3, 3)
Intrinsic parameters
Returns
-------
tsdf : TSDF
An instance of TSDF with value computed as projective TSDF
"""
# TODO Your code goes here
return tsdf
def ComputeTSDFNormal(point, tsdf, voxel_param):
"""
ComputeTSDFNormal : Compute surface normal from tsdf
Parameters
----------
point : ndarray of shape (3, H, W)
Point cloud predicted by casting rays to tsdf
voxel_param : an instance of voxel parameter VoxelParams
tsdf : an instance of TSDF
Returns
-------
normal : ndarray of shape (3, H, W)
Surface normal at each 3D point indicated by 'point' variable
Note
-------
You can use scipy.ndimage.map_coordinates to interpolate ndarray
"""
# TODO Your code goes here
return normal
def FindCorrespondence(T, point_pred, normal_pred, point, normal, valid_rays, K, e_p, e_n):
"""
Find Correspondence between current tsdf and input image's depth/normal
Parameters
----------
T : ndarray of shape (4, 4)
Transformation of camera to world coordinate
point_pred : ndarray of shape (3, H, W)
Point cloud from ray casting the tsdf
normal_pred : ndarray of shape (3, H, W)
Surface normal from tsdf
point : ndarray of shape (3, H, W)
Point cloud extracted from depth image
normal : ndarray of shape (3, H, W)
Surface normal extracted from depth image
valid_rays : ndarray of shape (H, W)
Valid ray casting pixels
K : ndarray of shape (3, 3)
Intrinsic parameters
e_p : float
Threshold on distance error
e_n : float
Threshold on cosine angular error
Returns
-------
Correspondence point of 4 variables
p_pred, n_pred, p, n : ndarray of shape (3, m)
Inlier point_pred, normal_pred, point, normal
"""
# TODO Your code goes here
return p_pred, n_pred, p, n
def SolveForPose(p_pred, n_pred, p):
"""
Solve For Incremental Update Pose
Parameters
----------
p_pred : ndarray of shape (3, -1)
Inlier tsdf point
n_pred : ndarray of shape (3, -1)
Inlier tsdf surface normal
p : ndarray of shape (3, -1)
Inlier depth image's point
Returns
-------
deltaT : ndarray of shape (4, 4)
Incremental updated pose matrix
"""
# TODO Your code goes here
return deltaT
def FuseTSDF(tsdf, tsdf_new):
"""
FuseTSDF : Fusing 2 tsdfs
Parameters
----------
tsdf, tsdf_new : TSDFs
Returns
-------
tsdf : TSDF
Fused of tsdf and tsdf_new
"""
# TODO Your code goes here
return tsdf
if __name__ == '__main__':
DEPTH_FOLDER = 'depth_images'
OUTPUT_FOLDER = 'results'
if not os.path.exists(OUTPUT_FOLDER):
os.mkdir(OUTPUT_FOLDER)
voxel_param = VoxelParams(3, 256)
fx = 525.0
fy = 525.0
cx = 319.5
cy = 239.5
K = np.array([[fx, 0., cx], [0., fy, cy], [0., 0., 1]])
depth_factor = 5000.
n_iters = 3
e_p = voxel_param.vox_size * 10.0
e_n = np.cos(np.pi / 3.0)
T_cur = np.eye(4)
depth_file_list = open(os.path.join(DEPTH_FOLDER, 'filelist.list'), 'r').read().split('\n')
depth_img = ProcessDepthImage(os.path.join(DEPTH_FOLDER, depth_file_list[0]), depth_factor)
tsdf = CreateTSDF(depth_img, T_cur, voxel_param, K)
SaveTSDFtoMesh('%s/mesh_initial.ply' % OUTPUT_FOLDER, tsdf)
rays = ImageRays(K, voxel_param, depth_img.shape)
for i_frame in range(1, len(depth_file_list)-1):
print('process frame ', i_frame)
point_pred, valid_rays = rays.cast(T_cur, voxel_param, tsdf)
SavePointDepth('%s/pd_%02d.ply' % (OUTPUT_FOLDER, i_frame), point_pred, valid_rays)
normal_pred = -ComputeTSDFNormal(point_pred, tsdf, voxel_param)
SavePointNormal('%s/pn_%02d.ply' % (OUTPUT_FOLDER, i_frame), point_pred, normal_pred, valid_rays)
depth_img = ProcessDepthImage(os.path.join(DEPTH_FOLDER, depth_file_list[i_frame]), depth_factor)
point, normal = Get3D(depth_img, K)
for i in range(n_iters):
p_pred, n_pred, p, n = FindCorrespondence(T_cur, point_pred, normal_pred,
point, normal, valid_rays, K, e_p, e_n)
deltaT = SolveForPose(p_pred, n_pred, p)
# Update pose
T_cur = deltaT @ T_cur
u, s, vh = np.linalg.svd(T_cur[:3, :3])
R = u @ vh
R *= np.linalg.det(R)
T_cur[:3, :3] = R
tsdf_new = CreateTSDF(depth_img, T_cur, voxel_param, K)
tsdf = FuseTSDF(tsdf, tsdf_new)
SaveTSDFtoMesh('%s/mesh_%02d.ply' % (OUTPUT_FOLDER, i_frame), tsdf)
SaveTSDFtoMesh('%s/mesh_final.ply' % OUTPUT_FOLDER, tsdf, viz=True)