-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathMerge.py
157 lines (127 loc) · 6.06 KB
/
Merge.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
"""
cBLUE (comprehensive Bathymetric Lidar Uncertainty Estimator)
Copyright (C) 2019
Oregon State University (OSU)
Center for Coastal and Ocean Mapping/Joint Hydrographic Center, University of New Hampshire (CCOM/JHC, UNH)
NOAA Remote Sensing Division (NOAA RSD)
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Contact:
Christopher Parrish, PhD
School of Construction and Civil Engineering
101 Kearney Hall
Oregon State University
Corvallis, OR 97331
(541) 737-5688
"""
import numpy as np
import numexpr as ne
from math import radians
import logging
import matplotlib.pyplot as plt
class Merge:
max_allowable_dt = 1.0 # second
def __init__(self):
self.a_std_dev = 0.02 # degrees
self.b_std_dev = 0.02 # degrees
self.std_rho = 0.025
"""returns sbet & las data merged based on timestamps
The cBLUE TPU calculations require the sbet and las data to be in
a single array, so the sbet and lidar data, which are contained in
separate files, are 'merged', based on time. cBLUE uses the numpy
searchsorted function to match lidar datapoints with the nearest-in-time
(with sorter='left') sbet datapoints.
If the delta_t between any of the matched datapoints is > 1, TPU is
not calculated. Future versions might be smart enough to exclude only
the offending datapoints rather than discarding the whole file, but for
the time being, if a single datapoint's delta_t exceeds the allowable
maximum dt, the entire line is ignored.
:param str las:
:param ? fl:
:param ? sbet_data:
:param Tuple(ndarray) # TODO: are the parentheses necessary?
:return: List[ndarray]
The following table lists the contents of the returned list of ndarrays:
===== ========= ======================== =======
Index ndarray description units
===== ========= ======================== =======
0 t_sbet sbet timestamps
1 t_las las timestamps
2 x_las las x coordinates
3 y_las las y coordinates
4 z_las las z coordinates
5 x_sbet sbet x coordinates
6 y_sbet sbet y coordinates
7 z_sbet sbet z coordinates
8 r sbet roll
9 p sbet pitch
10 h sbet heading
11 std_a a uncertainty
12 std_b b uncertainty
13 std_r sbet roll uncertainty
14 std_p sbet pitch uncertainty
15 std_h sbet heading uncertainty
16 stdx_sbet sbet x uncertainty
17 stdy_sbet sbet y uncertainty
18 stdz_sbet sbet z uncertainty
19 std_rho ?
===== ========= ======================== =======
"""
def merge(self, las, fl, sbet_data, fl_las_data, fl_t_idx):
num_sbet_pts = sbet_data.shape[0]
# sort xyzt array based on t_idx column
idx = fl_t_idx.argsort()
fl_las_data = fl_las_data[idx]
fl_t_idx = fl_t_idx[idx]
# match sbet and las dfs based on timestamps
idx = np.searchsorted(sbet_data[:, 0], fl_las_data[:, 3])
# don't use las points outside range of sbet points
mask = ne.evaluate('0 < idx') & ne.evaluate('idx < num_sbet_pts')
t_sbet_masked = sbet_data[:, 0][idx[mask]]
t_las_masked = fl_las_data[:, 3][mask]
dt = ne.evaluate('t_sbet_masked - t_las_masked')
max_dt = ne.evaluate('max(dt)') # may be empty
if max_dt > self.max_allowable_dt or max_dt.size == 0:
data = False
stddev = False
logging.warning('trajectory and LAS data NOT MERGED')
logging.warning('({} FL {}) max_dt: {}'.format(las, fl, max_dt))
else:
data = np.asarray([
sbet_data[:, 0][idx[mask]],
fl_las_data[:, 3][mask], # t
fl_las_data[:, 0][mask], # x
fl_las_data[:, 1][mask], # y
fl_las_data[:, 2][mask], # z
sbet_data[:, 3][idx[mask]],
sbet_data[:, 4][idx[mask]],
sbet_data[:, 5][idx[mask]],
np.radians(sbet_data[:, 6][idx[mask]]),
np.radians(sbet_data[:, 7][idx[mask]]),
np.radians(sbet_data[:, 8][idx[mask]])
])
num_points = data[0].shape
stddev = np.vstack([
np.full(num_points, radians(self.a_std_dev)), # std_a
np.full(num_points, radians(self.b_std_dev)), # std_b
np.radians(sbet_data[:, 12][idx[mask]]), # std_r
np.radians(sbet_data[:, 13][idx[mask]]), # std_p
np.radians(sbet_data[:, 14][idx[mask]]), # std_h
sbet_data[:, 9][idx[mask]], # stdx_sbet
sbet_data[:, 10][idx[mask]], # stdy_sbet
sbet_data[:, 11][idx[mask]], # stdz_sbet
np.full(num_points, self.std_rho) # std_rho
])
return data, stddev, fl_t_idx[mask] # last array is masked t_idx
if __name__ == '__main__':
pass