|
1
|
|
|
import os.path |
|
2
|
|
|
import shutil |
|
3
|
|
|
from tempfile import mkdtemp |
|
4
|
|
|
|
|
5
|
|
|
import numpy as np |
|
6
|
|
|
import pcl |
|
7
|
|
|
|
|
8
|
|
|
from patty import utils |
|
9
|
|
|
from patty.utils import downsample_voxel |
|
10
|
|
|
|
|
11
|
|
|
from helpers import make_tri_pyramid_with_base |
|
12
|
|
|
|
|
13
|
|
|
from sklearn.utils.extmath import cartesian |
|
14
|
|
|
import unittest |
|
15
|
|
|
|
|
16
|
|
|
|
|
17
|
|
|
class TestRegistrationPipeline(unittest.TestCase): |
|
18
|
|
|
|
|
19
|
|
|
def setUp(self): |
|
20
|
|
|
self.useLocal = False |
|
21
|
|
|
|
|
22
|
|
|
if self.useLocal: |
|
23
|
|
|
self.tempdir = tempdir = '.' |
|
24
|
|
|
else: |
|
25
|
|
|
self.tempdir = tempdir = mkdtemp(prefix='patty-analytics') |
|
26
|
|
|
|
|
27
|
|
|
self.drivemapLas = os.path.join(tempdir, 'testDriveMap.las') |
|
28
|
|
|
self.sourcelas = os.path.join(tempdir, 'testSource.las') |
|
29
|
|
|
self.footprint_csv = os.path.join(tempdir, 'testFootprint.csv') |
|
30
|
|
|
self.foutlas = os.path.join(tempdir, 'testOutput.las') |
|
31
|
|
|
|
|
32
|
|
|
self.min = -10 |
|
33
|
|
|
self.max = 10 |
|
34
|
|
|
self.num_rows = 1000 |
|
35
|
|
|
|
|
36
|
|
|
# Create plane with a pyramid |
|
37
|
|
|
dm_pct = 0.5 |
|
38
|
|
|
dm_rows = np.round(self.num_rows * dm_pct) |
|
39
|
|
|
dm_min = self.min * dm_pct |
|
40
|
|
|
dm_max = self.max * dm_pct |
|
41
|
|
|
|
|
42
|
|
|
delta = dm_max / dm_rows |
|
43
|
|
|
shape_side = dm_max - dm_min |
|
44
|
|
|
|
|
45
|
|
|
dm_offset = [0, 0, 0] |
|
46
|
|
|
self.dense_obj_offset = [3, 2, -(1 + shape_side / 2)] |
|
47
|
|
|
|
|
48
|
|
|
# make drivemap |
|
49
|
|
|
plane_row = np.linspace( |
|
50
|
|
|
start=self.min, stop=self.max, num=self.num_rows) |
|
51
|
|
|
plane_points = cartesian((plane_row, plane_row, [0])) |
|
52
|
|
|
|
|
53
|
|
|
shape_points, footprint = make_tri_pyramid_with_base( |
|
54
|
|
|
shape_side, delta, dm_offset) |
|
55
|
|
|
np.savetxt(self.footprint_csv, footprint, fmt='%.3f', delimiter=',') |
|
56
|
|
|
|
|
57
|
|
|
dm_points = np.vstack([plane_points, shape_points]) |
|
58
|
|
|
plane_grid = np.zeros((dm_points.shape[0], 6), dtype=np.float32) |
|
59
|
|
|
plane_grid[:, 0:3] = dm_points |
|
60
|
|
|
|
|
61
|
|
|
self.drivemap_pc = pcl.PointCloudXYZRGB(plane_grid) |
|
62
|
|
|
self.drivemap_pc = downsample_voxel(self.drivemap_pc, |
|
63
|
|
|
voxel_size=delta * 20) |
|
64
|
|
|
# utils.set_registration(self.drivemap_pc) |
|
65
|
|
|
utils.save(self.drivemap_pc, self.drivemapLas) |
|
66
|
|
|
|
|
67
|
|
|
# Create a simple pyramid |
|
68
|
|
|
dense_grid = np.zeros((shape_points.shape[0], 6), dtype=np.float32) |
|
69
|
|
|
dense_grid[:, 0:3] = shape_points + self.dense_obj_offset |
|
70
|
|
|
|
|
71
|
|
|
self.source_pc = pcl.PointCloudXYZRGB(dense_grid) |
|
72
|
|
|
self.source_pc = downsample_voxel(self.source_pc, voxel_size=delta * 5) |
|
73
|
|
|
utils.save(self.source_pc, self.sourcelas) |
|
74
|
|
|
|
|
75
|
|
|
def tearDown(self): |
|
76
|
|
|
if not self.useLocal: |
|
77
|
|
|
shutil.rmtree(self.tempdir, ignore_errors=True) |
|
78
|
|
|
|
|
79
|
|
|
def test_pipeline(self): |
|
80
|
|
|
pass |
|
81
|
|
|
# # TODO: should just use shutil to run the registration.py script, and |
|
82
|
|
|
# # load the result |
|
83
|
|
|
|
|
84
|
|
|
# os.system( './scripts/registration.py -u testupfile.json' |
|
85
|
|
|
# " " + self.sourcelas + |
|
86
|
|
|
# " " + self.drivemapLas + |
|
87
|
|
|
# " " + self.footprint_csv + |
|
88
|
|
|
# " " + self.foutlas ) |
|
89
|
|
|
|
|
90
|
|
|
# goal = utils.load( self.sourcelas) |
|
91
|
|
|
# actual = np.asarray( start ) |
|
92
|
|
|
|
|
93
|
|
|
# result = utils.load( self.foutlas ) |
|
94
|
|
|
# target = np.asarray( result ) |
|
95
|
|
|
|
|
96
|
|
|
# array_in_margin(target.min(axis=0), actual.min(axis=0), [1, 1, 1], |
|
97
|
|
|
# "Lower bound of registered cloud does not" |
|
98
|
|
|
# " match expectation") |
|
99
|
|
|
# array_in_margin(target.max(axis=0), actual.max(axis=0), [2.5, 5.5, 2], |
|
100
|
|
|
# "Upper bound of registered cloud does not" |
|
101
|
|
|
# " match expectation") |
|
102
|
|
|
# array_in_margin(target.mean(axis=0), actual.mean(axis=0), [1, 1, 1], |
|
103
|
|
|
# "Middle point of registered cloud does not" |
|
104
|
|
|
# " match expectation") |
|
105
|
|
|
|