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
|
|
|
|