Open3D (C++ API)  0.12.0
CoordinateTransformation.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2020 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <Eigen/Geometry>
30 
32 
33 namespace open3d {
34 namespace ml {
35 namespace impl {
36 
39 template <class T, int VECSIZE>
40 inline void MapSphereToCylinder(Eigen::Array<T, VECSIZE, 1>& x,
41  Eigen::Array<T, VECSIZE, 1>& y,
42  Eigen::Array<T, VECSIZE, 1>& z) {
43  Eigen::Array<T, VECSIZE, 1> sq_norm = x * x + y * y + z * z;
44  Eigen::Array<T, VECSIZE, 1> norm = sq_norm.sqrt();
45 
46  for (int i = 0; i < VECSIZE; ++i) {
47  if (sq_norm(i) < T(1e-12)) {
48  x(i) = y(i) = z(i) = T(0);
49  } else if (T(5.0 / 4) * z(i) * z(i) > (x(i) * x(i) + y(i) * y(i))) {
50  T s = std::sqrt(3 * norm(i) / (norm(i) + std::abs(z(i))));
51  x(i) *= s;
52  y(i) *= s;
53  z(i) = std::copysign(norm(i), z(i));
54  } else {
55  T s = norm(i) / std::sqrt(x(i) * x(i) + y(i) * y(i));
56  x(i) *= s;
57  y(i) *= s;
58  z(i) *= T(3.0 / 2);
59  }
60  }
61 }
62 
65 template <class T, int VECSIZE>
66 inline void MapCylinderToCube(Eigen::Array<T, VECSIZE, 1>& x,
67  Eigen::Array<T, VECSIZE, 1>& y,
68  Eigen::Array<T, VECSIZE, 1>& z) {
69  Eigen::Array<T, VECSIZE, 1> sq_norm_xy = x * x + y * y;
70  Eigen::Array<T, VECSIZE, 1> norm_xy = sq_norm_xy.sqrt();
71 
72  for (int i = 0; i < VECSIZE; ++i) {
73  if (sq_norm_xy(i) < T(1e-12)) {
74  x(i) = y(i) = T(0);
75  } else if (std::abs(y(i)) <= std::abs(x(i))) {
76  T tmp = std::copysign(norm_xy(i), x(i));
77  y(i) = tmp * T(4 / M_PI) * std::atan(y(i) / x(i));
78  x(i) = tmp;
79  } else // if( std::abs(x(i)) <= y(i) )
80  {
81  T tmp = std::copysign(norm_xy(i), y(i));
82  x(i) = tmp * T(4 / M_PI) * std::atan(x(i) / y(i));
83  y(i) = tmp;
84  }
85  }
86 }
87 
125 template <bool ALIGN_CORNERS, CoordinateMapping MAPPING, class T, int VECSIZE>
127  Eigen::Array<T, VECSIZE, 1>& x,
128  Eigen::Array<T, VECSIZE, 1>& y,
129  Eigen::Array<T, VECSIZE, 1>& z,
130  const Eigen::Array<int, 3, 1>& filter_size,
131  const Eigen::Array<T, VECSIZE, 3>& inv_extents,
132  const Eigen::Array<T, 3, 1>& offset) {
134  // x,y,z is now in the range [-1,1]
135  x *= 2 * inv_extents.col(0);
136  y *= 2 * inv_extents.col(1);
137  z *= 2 * inv_extents.col(2);
138 
139  Eigen::Array<T, VECSIZE, 1> radius = (x * x + y * y + z * z).sqrt();
140  for (int i = 0; i < VECSIZE; ++i) {
141  T abs_max = std::max(std::abs(x(i)),
142  std::max(std::abs(y(i)), std::abs(z(i))));
143  if (abs_max < T(1e-8)) {
144  x(i) = 0;
145  y(i) = 0;
146  z(i) = 0;
147  } else {
148  // map to the unit cube with edge length 1 and range [-0.5,0.5]
149  x(i) *= T(0.5) * radius(i) / abs_max;
150  y(i) *= T(0.5) * radius(i) / abs_max;
151  z(i) *= T(0.5) * radius(i) / abs_max;
152  }
153  }
155  // x,y,z is now in the range [-1,1]
156  x *= 2 * inv_extents.col(0);
157  y *= 2 * inv_extents.col(1);
158  z *= 2 * inv_extents.col(2);
159 
160  MapSphereToCylinder(x, y, z);
161  MapCylinderToCube(x, y, z);
162 
163  x *= T(0.5);
164  y *= T(0.5);
165  z *= T(0.5);
166  } else {
167  // map to the unit cube with edge length 1 and range [-0.5,0.5]
168  x *= inv_extents.col(0);
169  y *= inv_extents.col(1);
170  z *= inv_extents.col(2);
171  }
172 
173  if (ALIGN_CORNERS) {
174  x += T(0.5);
175  y += T(0.5);
176  z += T(0.5);
177 
178  x *= filter_size.x() - 1;
179  y *= filter_size.y() - 1;
180  z *= filter_size.z() - 1;
181  } else {
182  x *= filter_size.x();
183  y *= filter_size.y();
184  z *= filter_size.z();
185 
186  x += offset.x();
187  y += offset.y();
188  z += offset.z();
189 
190  // integer div
191  x += filter_size.x() / 2;
192  y += filter_size.y() / 2;
193  z += filter_size.z() / 2;
194 
195  // shift if the filter size is even
196  if (filter_size.x() % 2 == 0) x -= T(0.5);
197  if (filter_size.y() % 2 == 0) y -= T(0.5);
198  if (filter_size.z() % 2 == 0) z -= T(0.5);
199  }
200 }
201 
203 template <class T, int VECSIZE, InterpolationMode INTERPOLATION>
205 
207 template <class T, int VECSIZE>
209  typedef Eigen::Array<T, 1, VECSIZE> Weight_t;
210  typedef Eigen::Array<int, 1, VECSIZE> Idx_t;
211 
214  static constexpr int Size() { return 1; };
215 
233  inline void Interpolate(Eigen::Array<T, 1, VECSIZE>& w,
234  Eigen::Array<int, 1, VECSIZE>& idx,
235  const Eigen::Array<T, VECSIZE, 1>& x,
236  const Eigen::Array<T, VECSIZE, 1>& y,
237  const Eigen::Array<T, VECSIZE, 1>& z,
238  const Eigen::Array<int, 3, 1>& filter_size,
239  int num_channels = 1) const {
240  Eigen::Array<int, VECSIZE, 1> xi, yi, zi;
241 
242  xi = x.round().template cast<int>();
243  yi = y.round().template cast<int>();
244  zi = z.round().template cast<int>();
245 
246  // clamp to the valid range
247  xi = xi.min(filter_size.x() - 1).max(0);
248  yi = yi.min(filter_size.y() - 1).max(0);
249  zi = zi.min(filter_size.z() - 1).max(0);
250  idx = num_channels * (zi * filter_size.y() * filter_size.x() +
251  yi * filter_size.x() + xi);
252  w = 1;
253  }
254 };
255 
257 template <class T, int VECSIZE>
259  typedef Eigen::Array<T, 8, VECSIZE> Weight_t;
260  typedef Eigen::Array<int, 8, VECSIZE> Idx_t;
261 
262  static constexpr int Size() { return 8; };
263 
264  inline void Interpolate(Eigen::Array<T, 8, VECSIZE>& w,
265  Eigen::Array<int, 8, VECSIZE>& idx,
266  const Eigen::Array<T, VECSIZE, 1>& x,
267  const Eigen::Array<T, VECSIZE, 1>& y,
268  const Eigen::Array<T, VECSIZE, 1>& z,
269  const Eigen::Array<int, 3, 1>& filter_size,
270  int num_channels = 1) const {
271  for (int i = 0; i < VECSIZE; ++i) {
272  int xi0 = std::max(0, std::min(int(x(i)), filter_size.x() - 1));
273  int xi1 = std::max(0, std::min(xi0 + 1, filter_size.x() - 1));
274 
275  int yi0 = std::max(0, std::min(int(y(i)), filter_size.y() - 1));
276  int yi1 = std::max(0, std::min(yi0 + 1, filter_size.y() - 1));
277 
278  int zi0 = std::max(0, std::min(int(z(i)), filter_size.z() - 1));
279  int zi1 = std::max(0, std::min(zi0 + 1, filter_size.z() - 1));
280 
281  T a = std::max(T(0), std::min(x(i) - xi0, T(1)));
282  T b = std::max(T(0), std::min(y(i) - yi0, T(1)));
283  T c = std::max(T(0), std::min(z(i) - zi0, T(1)));
284 
285  w.col(i) << (1 - a) * (1 - b) * (1 - c), (a) * (1 - b) * (1 - c),
286  (1 - a) * (b) * (1 - c), (a) * (b) * (1 - c),
287  (1 - a) * (1 - b) * (c), (a) * (1 - b) * (c),
288  (1 - a) * (b) * (c), (a) * (b) * (c);
289 
290  idx.col(i) << zi0 * filter_size.y() * filter_size.x() +
291  yi0 * filter_size.x() + xi0,
292  zi0 * filter_size.y() * filter_size.x() +
293  yi0 * filter_size.x() + xi1,
294  zi0 * filter_size.y() * filter_size.x() +
295  yi1 * filter_size.x() + xi0,
296  zi0 * filter_size.y() * filter_size.x() +
297  yi1 * filter_size.x() + xi1,
298  zi1 * filter_size.y() * filter_size.x() +
299  yi0 * filter_size.x() + xi0,
300  zi1 * filter_size.y() * filter_size.x() +
301  yi0 * filter_size.x() + xi1,
302  zi1 * filter_size.y() * filter_size.x() +
303  yi1 * filter_size.x() + xi0,
304  zi1 * filter_size.y() * filter_size.x() +
305  yi1 * filter_size.x() + xi1;
306  }
307  idx *= num_channels;
308  }
309 };
310 
312 template <class T, int VECSIZE>
314  typedef Eigen::Array<T, 8, VECSIZE> Weight_t;
315  typedef Eigen::Array<int, 8, VECSIZE> Idx_t;
316 
317  static constexpr int Size() { return 8; };
318 
319  inline void Interpolate(Eigen::Array<T, 8, VECSIZE>& w,
320  Eigen::Array<int, 8, VECSIZE>& idx,
321  const Eigen::Array<T, VECSIZE, 1>& x,
322  const Eigen::Array<T, VECSIZE, 1>& y,
323  const Eigen::Array<T, VECSIZE, 1>& z,
324  const Eigen::Array<int, 3, 1>& filter_size,
325  int num_channels = 1) const {
326  for (int i = 0; i < VECSIZE; ++i) {
327  int xi0 = int(std::floor(x(i)));
328  int xi1 = xi0 + 1;
329 
330  int yi0 = int(std::floor(y(i)));
331  int yi1 = yi0 + 1;
332 
333  int zi0 = int(std::floor(z(i)));
334  int zi1 = zi0 + 1;
335 
336  T a = x(i) - xi0;
337  T b = y(i) - yi0;
338  T c = z(i) - zi0;
339 
340  if (zi0 < 0 || yi0 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
341  yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
342  idx(0, i) = 0;
343  w(0, i) = 0;
344  } else {
345  idx(0, i) = zi0 * filter_size.y() * filter_size.x() +
346  yi0 * filter_size.x() + xi0;
347  w(0, i) = (1 - a) * (1 - b) * (1 - c);
348  }
349 
350  if (zi0 < 0 || yi0 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
351  yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
352  idx(1, i) = 0;
353  w(1, i) = 0;
354  } else {
355  idx(1, i) = zi0 * filter_size.y() * filter_size.x() +
356  yi0 * filter_size.x() + xi1;
357  w(1, i) = (a) * (1 - b) * (1 - c);
358  }
359 
360  if (zi0 < 0 || yi1 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
361  yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
362  idx(2, i) = 0;
363  w(2, i) = 0;
364  } else {
365  idx(2, i) = zi0 * filter_size.y() * filter_size.x() +
366  yi1 * filter_size.x() + xi0;
367  w(2, i) = (1 - a) * (b) * (1 - c);
368  }
369 
370  if (zi0 < 0 || yi1 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
371  yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
372  idx(3, i) = 0;
373  w(3, i) = 0;
374  } else {
375  idx(3, i) = zi0 * filter_size.y() * filter_size.x() +
376  yi1 * filter_size.x() + xi1;
377  w(3, i) = (a) * (b) * (1 - c);
378  }
379 
380  if (zi1 < 0 || yi0 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
381  yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
382  idx(4, i) = 0;
383  w(4, i) = 0;
384  } else {
385  idx(4, i) = zi1 * filter_size.y() * filter_size.x() +
386  yi0 * filter_size.x() + xi0;
387  w(4, i) = (1 - a) * (1 - b) * (c);
388  }
389 
390  if (zi1 < 0 || yi0 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
391  yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
392  idx(5, i) = 0;
393  w(5, i) = 0;
394  } else {
395  idx(5, i) = zi1 * filter_size.y() * filter_size.x() +
396  yi0 * filter_size.x() + xi1;
397  w(5, i) = (a) * (1 - b) * (c);
398  }
399 
400  if (zi1 < 0 || yi1 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
401  yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
402  idx(6, i) = 0;
403  w(6, i) = 0;
404  } else {
405  idx(6, i) = zi1 * filter_size.y() * filter_size.x() +
406  yi1 * filter_size.x() + xi0;
407  w(6, i) = (1 - a) * (b) * (c);
408  }
409 
410  if (zi1 < 0 || yi1 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
411  yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
412  idx(7, i) = 0;
413  w(7, i) = 0;
414  } else {
415  idx(7, i) = zi1 * filter_size.y() * filter_size.x() +
416  yi1 * filter_size.x() + xi1;
417  w(7, i) = (a) * (b) * (c);
418  }
419  }
420  idx *= num_channels;
421  }
422 };
423 
424 } // namespace impl
425 } // namespace ml
426 } // namespace open3d
Definition: VoxelPooling.h:40
void Interpolate(Eigen::Array< T, 8, VECSIZE > &w, Eigen::Array< int, 8, VECSIZE > &idx, const Eigen::Array< T, VECSIZE, 1 > &x, const Eigen::Array< T, VECSIZE, 1 > &y, const Eigen::Array< T, VECSIZE, 1 > &z, const Eigen::Array< int, 3, 1 > &filter_size, int num_channels=1) const
Definition: CoordinateTransformation.h:319
int offset
Definition: FilePCD.cpp:64
static constexpr int Size()
Definition: CoordinateTransformation.h:317
void MapCylinderToCube(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z)
Definition: CoordinateTransformation.h:66
void Interpolate(Eigen::Array< T, 1, VECSIZE > &w, Eigen::Array< int, 1, VECSIZE > &idx, const Eigen::Array< T, VECSIZE, 1 > &x, const Eigen::Array< T, VECSIZE, 1 > &y, const Eigen::Array< T, VECSIZE, 1 > &z, const Eigen::Array< int, 3, 1 > &filter_size, int num_channels=1) const
Definition: CoordinateTransformation.h:233
InterpolationMode
Definition: ContinuousConvTypes.h:37
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:314
void ComputeFilterCoordinates(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z, const Eigen::Array< int, 3, 1 > &filter_size, const Eigen::Array< T, VECSIZE, 3 > &inv_extents, const Eigen::Array< T, 3, 1 > &offset)
Definition: CoordinateTransformation.h:126
static constexpr int Size()
Definition: CoordinateTransformation.h:262
FN_SPECIFIERS MiniVec< float, N > floor(const MiniVec< float, N > &a)
Definition: MiniVec.h:94
Class for computing interpolation weights.
Definition: CoordinateTransformation.h:204
static constexpr int Size()
Definition: CoordinateTransformation.h:214
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample playback_handle k4a_logging_message_cb_t void min_level device_handle k4a_imu_sample_t timeout_in_ms capture_handle capture_handle capture_handle image_handle temperature_c int
Definition: K4aPlugin.cpp:479
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:315
Definition: PinholeCameraIntrinsic.cpp:35
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:260
void MapSphereToCylinder(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z)
Definition: CoordinateTransformation.h:40
Eigen::Array< int, 1, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:210
void Interpolate(Eigen::Array< T, 8, VECSIZE > &w, Eigen::Array< int, 8, VECSIZE > &idx, const Eigen::Array< T, VECSIZE, 1 > &x, const Eigen::Array< T, VECSIZE, 1 > &y, const Eigen::Array< T, VECSIZE, 1 > &z, const Eigen::Array< int, 3, 1 > &filter_size, int num_channels=1) const
Definition: CoordinateTransformation.h:264
Eigen::Array< T, 1, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:209
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:259