|
1
|
|
|
#!/usr/bin/env python |
|
2
|
|
|
|
|
3
|
1 |
|
""" |
|
4
|
|
|
Registration algorithms and utility functions |
|
5
|
|
|
""" |
|
6
|
|
|
|
|
7
|
1 |
|
from __future__ import print_function |
|
8
|
1 |
|
import numpy as np |
|
9
|
1 |
|
from .. import BoundingBox, force_srs, extract_mask, clone |
|
10
|
1 |
|
from .stickscale import get_stick_scale |
|
11
|
1 |
|
from pcl.registration import gicp |
|
12
|
|
|
|
|
13
|
1 |
|
from patty.utils import ( |
|
14
|
|
|
log, |
|
15
|
|
|
save, |
|
16
|
|
|
downsample_voxel, |
|
17
|
|
|
) |
|
18
|
|
|
|
|
19
|
1 |
|
from patty.segmentation import ( |
|
20
|
|
|
boundary_of_center_object, |
|
21
|
|
|
boundary_of_drivemap, |
|
22
|
|
|
boundary_of_lowest_points, |
|
23
|
|
|
) |
|
24
|
|
|
|
|
25
|
1 |
|
from sklearn.decomposition import PCA |
|
26
|
|
|
|
|
27
|
|
|
|
|
28
|
1 |
|
def align_footprints(loose_pc, fixed_pc, |
|
29
|
|
|
allow_scaling=True, |
|
30
|
|
|
allow_rotation=True, |
|
31
|
|
|
allow_translation=True): |
|
32
|
|
|
''' |
|
33
|
|
|
Align a pointcloud 'loose_pc' by placing it on top of |
|
34
|
|
|
'fixed_pc' as good as poosible. Done by aligning the |
|
35
|
|
|
principle axis of both pointclouds. |
|
36
|
|
|
|
|
37
|
|
|
NOTE: Both pointclouds are assumed to be the footprint (or projection) |
|
38
|
|
|
on the xy plane, with basically zero extent along the z-axis. |
|
39
|
|
|
|
|
40
|
|
|
(allow_rotation=True) |
|
41
|
|
|
The pointcloud boundary is alinged with the footprint |
|
42
|
|
|
by rotating its pricipal axis in the (x,y) plane. |
|
43
|
|
|
|
|
44
|
|
|
(allow_translation=True) |
|
45
|
|
|
Then, it is translated so the centers of mass coincide. |
|
46
|
|
|
|
|
47
|
|
|
(allow_scaling=True) |
|
48
|
|
|
Finally, the pointcloud is scaled to have the same extent. |
|
49
|
|
|
|
|
50
|
|
|
Arguments: |
|
51
|
|
|
loose_pc : pcl.PointCloud |
|
52
|
|
|
fixed_pc : pcl.PointCloud |
|
53
|
|
|
|
|
54
|
|
|
allow_scaling : Bolean |
|
55
|
|
|
allow_rotation : Bolean |
|
56
|
|
|
allow_translation : Bolean |
|
57
|
|
|
|
|
58
|
|
|
Returns: |
|
59
|
|
|
rot_matrix, rot_center, scale, translation : np.array() |
|
60
|
|
|
|
|
61
|
|
|
''' |
|
62
|
|
|
|
|
63
|
|
|
rot_center = loose_pc.center() |
|
64
|
|
|
|
|
65
|
|
|
if allow_rotation: |
|
66
|
|
|
log(" - Finding rotation") |
|
67
|
|
|
rot_matrix = find_rotation_xy(loose_pc, fixed_pc) |
|
68
|
|
|
loose_pc.rotate(rot_matrix, origin=rot_center) |
|
69
|
|
|
else: |
|
70
|
|
|
log(" - Skipping rotation") |
|
71
|
|
|
rot_matrix = np.eye(3) |
|
72
|
|
|
|
|
73
|
|
|
if allow_scaling: |
|
74
|
|
|
fixed_bb = BoundingBox(fixed_pc) # used 2x below |
|
75
|
|
|
loose_bb = BoundingBox(loose_pc) |
|
76
|
|
|
scale = fixed_bb.size[0:2] / loose_bb.size[0:2] |
|
77
|
|
|
|
|
78
|
|
|
# take the average scale factor for the x and y dimensions |
|
79
|
|
|
scale = np.mean(scale) |
|
80
|
|
|
loose_pc.scale(scale, origin=rot_center) |
|
81
|
|
|
log(" - Scale: %s" % scale) |
|
82
|
|
|
else: |
|
83
|
|
|
log(" - Skipping scale") |
|
84
|
|
|
scale = 1.0 |
|
85
|
|
|
|
|
86
|
|
|
if allow_translation: |
|
87
|
|
|
translation = fixed_pc.center() - rot_center |
|
88
|
|
|
loose_pc.translate(translation) |
|
89
|
|
|
log(" - Translation: %s" % translation) |
|
90
|
|
|
else: |
|
91
|
|
|
log(" - Skipping translation") |
|
92
|
|
|
translation = np.array([0.0, 0.0, 0.0]) |
|
93
|
|
|
|
|
94
|
|
|
return rot_matrix, rot_center, scale, translation |
|
95
|
|
|
|
|
96
|
|
|
|
|
97
|
1 |
|
def estimate_pancake_up(pointcloud): |
|
98
|
|
|
''' |
|
99
|
|
|
Assuming a pancake like pointcloud, the up direction is the third PCA. |
|
100
|
|
|
''' |
|
101
|
|
|
pca = PCA(n_components=3) |
|
102
|
|
|
|
|
103
|
|
|
points = np.asarray(pointcloud) |
|
104
|
|
|
pca.fit(points[:, 0:3]) |
|
105
|
|
|
|
|
106
|
|
|
return pca.components_[2] |
|
107
|
|
|
|
|
108
|
|
|
|
|
109
|
1 |
|
def _find_rotation_xy_helper(pointcloud): |
|
110
|
1 |
|
pca = PCA(n_components=2) |
|
111
|
|
|
|
|
112
|
1 |
|
points = np.asarray(pointcloud) |
|
113
|
1 |
|
pca.fit(points[:, 0:2]) |
|
114
|
|
|
|
|
115
|
1 |
|
rotxy = np.array(pca.components_) |
|
116
|
|
|
|
|
117
|
|
|
# make sure the rotation is a proper rotation, ie det = +1 |
|
118
|
1 |
|
if np.linalg.det(rotxy) < 0: |
|
119
|
1 |
|
rotxy[:, 1] *= -1.0 |
|
120
|
|
|
|
|
121
|
|
|
# create a 3D rotation around the z-axis |
|
122
|
1 |
|
rotation = np.eye(3) |
|
123
|
1 |
|
rotation[0:2, 0:2] = rotxy |
|
124
|
|
|
|
|
125
|
1 |
|
return rotation |
|
126
|
|
|
|
|
127
|
|
|
|
|
128
|
1 |
|
def find_rotation_xy(pc, ref): |
|
129
|
|
|
'''Find the transformation that rotates the principal axis of the |
|
130
|
|
|
pointcloud onto those of the reference. |
|
131
|
|
|
Keep the z-axis pointing upwards. |
|
132
|
|
|
|
|
133
|
|
|
Arguments: |
|
134
|
|
|
pc: pcl.PointCloud |
|
135
|
|
|
|
|
136
|
|
|
ref: pcl.PointCloud |
|
137
|
|
|
|
|
138
|
|
|
Returns: |
|
139
|
|
|
numpy array of shape [3,3], can be used to rotate pointclouds |
|
140
|
|
|
with pc.rotate() |
|
141
|
|
|
''' |
|
142
|
|
|
|
|
143
|
1 |
|
pc_transform = _find_rotation_xy_helper(pc) |
|
144
|
1 |
|
ref_transform = _find_rotation_xy_helper(ref) |
|
145
|
|
|
|
|
146
|
1 |
|
return np.dot(np.linalg.inv(ref_transform), pc_transform) |
|
147
|
|
|
|
|
148
|
|
|
|
|
149
|
1 |
|
def rotate_upwards(pc, up): |
|
150
|
|
|
''' |
|
151
|
|
|
Rotate the pointcloud in-place around its center, such that the |
|
152
|
|
|
'up' vector points along [0,0,1] |
|
153
|
|
|
|
|
154
|
|
|
Arguments: |
|
155
|
|
|
pc : pcl.PointCloud |
|
156
|
|
|
up : np.array([3]) |
|
157
|
|
|
|
|
158
|
|
|
Returns: |
|
159
|
|
|
pc : pcl.PointCloud the input pointcloud, for convenience. |
|
160
|
|
|
|
|
161
|
|
|
''' |
|
162
|
|
|
|
|
163
|
|
|
newz = np.array(up) |
|
164
|
|
|
|
|
165
|
|
|
# Right-handed coordiante system: |
|
166
|
|
|
# np.cross(x,y) = z |
|
167
|
|
|
# np.cross(y,z) = x |
|
168
|
|
|
# np.cross(z,x) = y |
|
169
|
|
|
|
|
170
|
|
|
# normalize |
|
171
|
|
|
newz /= (np.dot(newz, newz)) ** 0.5 |
|
172
|
|
|
|
|
173
|
|
|
# find two orthogonal vectors to represent x and y, |
|
174
|
|
|
# randomly choose a vector, and take cross product. If we're unlucky, |
|
175
|
|
|
# this ones is parallel to z, so cross pruduct is zero. |
|
176
|
|
|
# In that case, try another one |
|
177
|
|
|
try: |
|
178
|
|
|
newx = np.cross(np.array([0, 1, 0]), newz) |
|
179
|
|
|
newx /= (np.dot(newx, newx)) ** 0.5 |
|
180
|
|
|
|
|
181
|
|
|
newy = np.cross(newz, newx) |
|
182
|
|
|
newy /= (np.dot(newy, newy)) ** 0.5 |
|
183
|
|
|
except: |
|
184
|
|
|
print("Alternative") |
|
185
|
|
|
newy = np.cross(newz, np.array([1, 0, 0])) |
|
186
|
|
|
newy /= (np.dot(newy, newy)) ** 0.5 |
|
187
|
|
|
|
|
188
|
|
|
newx = np.cross(newy, newz) |
|
189
|
|
|
newx /= (np.dot(newx, newx)) ** 0.5 |
|
190
|
|
|
|
|
191
|
|
|
rotation = np.zeros([3, 3]) |
|
192
|
|
|
rotation[0, 0] = newx[0] |
|
193
|
|
|
rotation[1, 0] = newx[1] |
|
194
|
|
|
rotation[2, 0] = newx[2] |
|
195
|
|
|
|
|
196
|
|
|
rotation[0, 1] = newy[0] |
|
197
|
|
|
rotation[1, 1] = newy[1] |
|
198
|
|
|
rotation[2, 1] = newy[2] |
|
199
|
|
|
|
|
200
|
|
|
rotation[0, 2] = newz[0] |
|
201
|
|
|
rotation[1, 2] = newz[1] |
|
202
|
|
|
rotation[2, 2] = newz[2] |
|
203
|
|
|
|
|
204
|
|
|
rotation = np.linalg.inv(rotation) |
|
205
|
|
|
pc.rotate(rotation, origin=pc.center()) |
|
206
|
|
|
|
|
207
|
|
|
return pc |
|
208
|
|
|
|
|
209
|
|
|
|
|
210
|
1 |
|
def initial_registration(pointcloud, up, drivemap, |
|
211
|
|
|
initial_scale=None, trust_up=True): |
|
212
|
|
|
""" |
|
213
|
|
|
Initial registration adds an spatial reference system to the pointcloud, |
|
214
|
|
|
and place the pointlcoud on top of the drivemap. The pointcloud is rotated |
|
215
|
|
|
so that the up vector points along [0,0,1], and scaled such that it has the |
|
216
|
|
|
right order of magnitude in size. |
|
217
|
|
|
|
|
218
|
|
|
Arguments: |
|
219
|
|
|
pointcloud : pcl.PointCloud |
|
220
|
|
|
The high-res object to register. |
|
221
|
|
|
|
|
222
|
|
|
up: np.array([3]) |
|
223
|
|
|
Up direction for the pointcloud. |
|
224
|
|
|
If None, assume the object is pancake shaped, and chose the |
|
225
|
|
|
upvector such that it is perpendicullar to the pancake. |
|
226
|
|
|
|
|
227
|
|
|
drivemap : pcl.PointCloud |
|
228
|
|
|
A small part of the low-res drivemap on which to register. |
|
229
|
|
|
|
|
230
|
|
|
initial_scale : float |
|
231
|
|
|
if given, scale pointcloud using this value; estimate scale factor |
|
232
|
|
|
from bounding boxes. |
|
233
|
|
|
|
|
234
|
|
|
trust_up : Boolean, default to True |
|
235
|
|
|
True: Assume the up vector is exact. |
|
236
|
|
|
False: Calculate 'up' as if it was None, but orient it such that |
|
237
|
|
|
np.dot( up, pancake_up ) > 0 |
|
238
|
|
|
|
|
239
|
|
|
NOTE: Modifies the input pointcloud in-place, and leaves |
|
240
|
|
|
it in a undefined state. |
|
241
|
|
|
|
|
242
|
|
|
""" |
|
243
|
|
|
log("Starting initial registration") |
|
244
|
|
|
|
|
245
|
|
|
##### |
|
246
|
|
|
# set scale and offset of pointcloud, drivemap, and footprint |
|
247
|
|
|
# as the pointcloud is unregisterd, the coordinate system is undefined, |
|
248
|
|
|
# and we lose nothing if we just copy it |
|
249
|
|
|
|
|
250
|
|
|
if(hasattr(pointcloud, "offset")): |
|
251
|
|
|
log(" - Dropping initial offset, was: %s" % pointcloud.offset) |
|
252
|
|
|
else: |
|
253
|
|
|
log(" - No initial offset") |
|
254
|
|
|
force_srs(pointcloud, same_as=drivemap) |
|
255
|
|
|
log(" - New offset forced to: %s" % pointcloud.offset) |
|
256
|
|
|
|
|
257
|
|
|
if up is not None: |
|
258
|
|
|
log(" - Rotating the pointcloud so up points along [0,0,1]") |
|
259
|
|
|
|
|
260
|
|
|
if trust_up: |
|
261
|
|
|
rotate_upwards(pointcloud, up) |
|
262
|
|
|
log(" - Using trusted up: %s" % up) |
|
263
|
|
|
else: |
|
264
|
|
|
pancake_up = estimate_pancake_up(pointcloud) |
|
265
|
|
|
if np.dot(up, pancake_up) < 0.0: |
|
266
|
|
|
pancake_up *= -1.0 |
|
267
|
|
|
log(" - Using estimated up: %s" % pancake_up) |
|
268
|
|
|
rotate_upwards(pointcloud, pancake_up) |
|
269
|
|
|
|
|
270
|
|
|
else: |
|
271
|
|
|
log(" - No upvector, skipping") |
|
272
|
|
|
|
|
273
|
|
|
if initial_scale is None: |
|
274
|
|
|
bbDrivemap = BoundingBox(points=np.asarray(drivemap)) |
|
|
|
|
|
|
275
|
|
|
bbObject = BoundingBox(points=np.asarray(pointcloud)) |
|
|
|
|
|
|
276
|
|
|
scale = bbDrivemap.size[0:2] / bbObject.size[0:2] # ignore z-direction |
|
277
|
|
|
|
|
278
|
|
|
# take the average scale factor for x and y dimensions |
|
279
|
|
|
scale = np.mean(scale) |
|
280
|
|
|
else: |
|
281
|
|
|
# use user provided scale |
|
282
|
|
|
scale = initial_scale |
|
283
|
|
|
|
|
284
|
|
|
log(" - Applying rough estimation of scale factor", scale) |
|
285
|
|
|
pointcloud.scale(scale) # dont care about origin of scaling |
|
286
|
|
|
|
|
287
|
|
|
|
|
288
|
1 |
|
def coarse_registration(pointcloud, drivemap, footprint, downsample=None): |
|
289
|
|
|
""" |
|
290
|
|
|
Improve the initial registration. |
|
291
|
|
|
Find the proper scale by looking for the red meter sticks, and calculate |
|
292
|
|
|
and align the pointcloud's footprint. |
|
293
|
|
|
|
|
294
|
|
|
Arguments: |
|
295
|
|
|
pointcloud: pcl.PointCloud |
|
296
|
|
|
The high-res object to register. |
|
297
|
|
|
|
|
298
|
|
|
drivemap: pcl.PointCloud |
|
299
|
|
|
A small part of the low-res drivemap on which to register |
|
300
|
|
|
|
|
301
|
|
|
footprint: pcl.PointCloud |
|
302
|
|
|
Pointlcloud containing the objects footprint |
|
303
|
|
|
|
|
304
|
|
|
downsample: float, default=None, no resampling |
|
305
|
|
|
Downsample the high-res pointcloud before footprint |
|
306
|
|
|
calculation. |
|
307
|
|
|
""" |
|
308
|
|
|
log("Starting coarse registration") |
|
309
|
|
|
|
|
310
|
|
|
### |
|
311
|
|
|
# find redstick scale, and use it if possible |
|
312
|
|
|
log(" - Redstick scaling") |
|
313
|
|
|
|
|
314
|
|
|
allow_scaling = True |
|
315
|
|
|
|
|
316
|
|
|
scale, confidence = get_stick_scale(pointcloud) |
|
317
|
|
|
log(" - Red stick scale=%s confidence=%s" % (scale, confidence)) |
|
318
|
|
|
|
|
319
|
|
|
if (confidence > 0.5): |
|
320
|
|
|
log(" - Applying red stick scale") |
|
321
|
|
|
pointcloud.scale(1.0 / scale) # dont care about origin of scaling |
|
322
|
|
|
allow_scaling = False |
|
323
|
|
|
else: |
|
324
|
|
|
log(" - Not applying red stick scale, confidence too low") |
|
325
|
|
|
|
|
326
|
|
|
##### |
|
327
|
|
|
# find all the points in the drivemap along the footprint |
|
328
|
|
|
# use bottom two meters of drivemap (not trees) |
|
329
|
|
|
|
|
330
|
|
|
dm_boundary = boundary_of_drivemap(drivemap, footprint) |
|
331
|
|
|
dm_bb = BoundingBox(dm_boundary) |
|
332
|
|
|
|
|
333
|
|
|
# set footprint height from minimum value, |
|
334
|
|
|
# as trees, or high object make the pc.center() too high |
|
335
|
|
|
fixed_boundary = clone(footprint) |
|
336
|
|
|
fp_array = np.asarray(fixed_boundary) |
|
337
|
|
|
fp_array[:, 2] = dm_bb.min[2] |
|
338
|
|
|
|
|
339
|
|
|
save(fixed_boundary, "fixed_bound.las") |
|
340
|
|
|
|
|
341
|
|
|
##### |
|
342
|
|
|
# find all the boundary points of the pointcloud |
|
343
|
|
|
|
|
344
|
|
|
loose_boundary = boundary_of_center_object(pointcloud, downsample) |
|
345
|
|
|
if loose_boundary is None: |
|
346
|
|
|
log(" - boundary estimation failed, using lowest 30 percent of points") |
|
347
|
|
|
loose_boundary = boundary_of_lowest_points(pointcloud, |
|
348
|
|
|
height_fraction=0.3) |
|
349
|
|
|
|
|
350
|
|
|
#### |
|
351
|
|
|
# match the pointcloud boundary with the footprint boundary |
|
352
|
|
|
|
|
353
|
|
|
log(" - Aligning footprints:") |
|
354
|
|
|
rot_matrix, rot_center, scale, translation = align_footprints( |
|
355
|
|
|
loose_boundary, fixed_boundary, |
|
356
|
|
|
allow_scaling=allow_scaling, |
|
357
|
|
|
allow_rotation=True, |
|
358
|
|
|
allow_translation=True) |
|
359
|
|
|
|
|
360
|
|
|
save(loose_boundary, "aligned_bound.las") |
|
361
|
|
|
|
|
362
|
|
|
#### |
|
363
|
|
|
# Apply to the main pointcloud |
|
364
|
|
|
|
|
365
|
|
|
pointcloud.rotate(rot_matrix, origin=rot_center) |
|
366
|
|
|
pointcloud.scale(scale, origin=rot_center) |
|
367
|
|
|
pointcloud.translate(translation) |
|
368
|
|
|
rot_center += translation |
|
369
|
|
|
|
|
370
|
|
|
return rot_center |
|
371
|
|
|
|
|
372
|
|
|
|
|
373
|
1 |
|
def _fine_registration_helper(pointcloud, drivemap, voxelsize=0.05, attempt=0): |
|
374
|
|
|
""" |
|
375
|
|
|
Perform ICP on pointcloud with drivemap, and return convergence indicator. |
|
376
|
|
|
Reject large translatoins. |
|
377
|
|
|
|
|
378
|
|
|
Returns: |
|
379
|
|
|
transf : np.array([4,4]) |
|
380
|
|
|
transform |
|
381
|
|
|
success : Boolean |
|
382
|
|
|
if icp was successful |
|
383
|
|
|
fitness : float |
|
384
|
|
|
sort of sum of square differences, ie. smaller is better |
|
385
|
|
|
""" |
|
386
|
|
|
#### |
|
387
|
|
|
# Downsample to speed up |
|
388
|
|
|
# use voxel filter to keep evenly distributed spatial extent |
|
389
|
|
|
|
|
390
|
|
|
log(" - Downsampling with voxel filter: %s" % voxelsize) |
|
391
|
|
|
pc = downsample_voxel(pointcloud, voxelsize) |
|
392
|
|
|
|
|
393
|
|
|
#### |
|
394
|
|
|
# Clip to drivemap to prevent outliers confusing the ICP algorithm |
|
395
|
|
|
|
|
396
|
|
|
log(" - Clipping to drivemap") |
|
397
|
|
|
bb = BoundingBox(drivemap) |
|
398
|
|
|
z = bb.center[2] |
|
|
|
|
|
|
399
|
|
|
extracted = extract_mask(pc, [bb.contains([point[0], point[1], z]) |
|
400
|
|
|
for point in pc]) |
|
401
|
|
|
|
|
402
|
|
|
log(" - Remaining points: %s" % len(extracted)) |
|
403
|
|
|
|
|
404
|
|
|
#### |
|
405
|
|
|
# GICP |
|
406
|
|
|
|
|
407
|
|
|
converged, transf, estimate, fitness = gicp(extracted, drivemap) |
|
408
|
|
|
|
|
409
|
|
|
#### |
|
410
|
|
|
# Dont accept large translations |
|
411
|
|
|
|
|
412
|
|
|
translation = transf[0:3, 3] |
|
413
|
|
|
if np.dot(translation, translation) > 5 ** 2: |
|
414
|
|
|
log(" - Translation too large, considering it a failure.") |
|
415
|
|
|
converged = False |
|
416
|
|
|
fitness = 1e30 |
|
417
|
|
|
else: |
|
418
|
|
|
log(" - Success, fitness: ", converged, fitness) |
|
419
|
|
|
|
|
420
|
|
|
force_srs(estimate, same_as=pointcloud) |
|
421
|
|
|
save(estimate, "attempt%s.las" % attempt) |
|
422
|
|
|
|
|
423
|
|
|
return transf, converged, fitness |
|
424
|
|
|
|
|
425
|
|
|
|
|
426
|
1 |
|
def fine_registration(pointcloud, drivemap, center, voxelsize=0.05): |
|
427
|
|
|
""" |
|
428
|
|
|
Final registration step using ICP. |
|
429
|
|
|
|
|
430
|
|
|
Find the local optimal postion of the pointcloud on the drivemap; due to |
|
431
|
|
|
our coarse_registration algorithm, we have to try two orientations: |
|
432
|
|
|
original, and rotated by 180 degrees around the z-axis. |
|
433
|
|
|
|
|
434
|
|
|
Arguments: |
|
435
|
|
|
pointcloud: pcl.PointCloud |
|
436
|
|
|
The high-res object to register. |
|
437
|
|
|
|
|
438
|
|
|
drivemap: pcl.PointCloud |
|
439
|
|
|
A small part of the low-res drivemap on which to register |
|
440
|
|
|
|
|
441
|
|
|
center: np.array([3]) |
|
442
|
|
|
Vector giving the centerpoint of the pointcloud, used to do |
|
443
|
|
|
the 180 degree rotations. |
|
444
|
|
|
|
|
445
|
|
|
voxelsize: float default : 0.05 |
|
446
|
|
|
Size in [m] of the voxel grid used for downsampling |
|
447
|
|
|
""" |
|
448
|
|
|
log("Starting fine registration") |
|
449
|
|
|
|
|
450
|
|
|
# for rotation around z-axis |
|
451
|
|
|
rot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) |
|
452
|
|
|
|
|
453
|
|
|
#### |
|
454
|
|
|
# do a ICP step for 4 orientations |
|
455
|
|
|
|
|
456
|
|
|
transf = {} |
|
457
|
|
|
success = {} |
|
458
|
|
|
fitness = {} |
|
459
|
|
|
for i in range(4): |
|
460
|
|
|
log(" - attempt: %s" % i) |
|
461
|
|
|
transf[i], success[i], fitness[i] = _fine_registration_helper( |
|
462
|
|
|
pointcloud, |
|
463
|
|
|
drivemap, |
|
464
|
|
|
attempt=i, |
|
465
|
|
|
voxelsize=voxelsize) |
|
466
|
|
|
pointcloud.rotate(rot, origin=center) |
|
467
|
|
|
|
|
468
|
|
|
#### |
|
469
|
|
|
# pick best |
|
470
|
|
|
|
|
471
|
|
|
best, value = min(fitness.iteritems(), key=lambda x: x[1]) |
|
472
|
|
|
if success[best]: |
|
473
|
|
|
log(" - Best attempt: %s" % best) |
|
474
|
|
|
if best > 0: # np.array()**0 does weird things |
|
475
|
|
|
pointcloud.rotate(rot**best, origin=center) |
|
476
|
|
|
pointcloud.transform(transf[best]) |
|
477
|
|
|
return |
|
478
|
|
|
|
|
479
|
|
|
# ICP failed: |
|
480
|
|
|
# return the pointcloud with just footprints aligned |
|
481
|
|
|
# no use to undo a rotation, as any orientationi is equally likely. |
|
482
|
|
|
log(" - Unable to do fine registration") |
|
483
|
|
|
|
|
484
|
|
|
return |
|
485
|
|
|
|
This check looks for invalid names for a range of different identifiers.
You can set regular expressions to which the identifiers must conform if the defaults do not match your requirements.
If your project includes a Pylint configuration file, the settings contained in that file take precedence.
To find out more about Pylint, please refer to their site.