Open3D (C++ API)  0.18.0+252c867
CoordinateTransformation.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2023 www.open3d.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
8 #pragma once
9 
10 #include <Eigen/Geometry>
11 
13 
14 namespace open3d {
15 namespace ml {
16 namespace impl {
17 
20 template <class T, int VECSIZE>
21 inline void MapSphereToCylinder(Eigen::Array<T, VECSIZE, 1>& x,
22  Eigen::Array<T, VECSIZE, 1>& y,
23  Eigen::Array<T, VECSIZE, 1>& z) {
24  Eigen::Array<T, VECSIZE, 1> sq_norm = x * x + y * y + z * z;
25  Eigen::Array<T, VECSIZE, 1> norm = sq_norm.sqrt();
26 
27  for (int i = 0; i < VECSIZE; ++i) {
28  if (sq_norm(i) < T(1e-12)) {
29  x(i) = y(i) = z(i) = T(0);
30  } else if (T(5.0 / 4) * z(i) * z(i) > (x(i) * x(i) + y(i) * y(i))) {
31  T s = std::sqrt(3 * norm(i) / (norm(i) + std::abs(z(i))));
32  x(i) *= s;
33  y(i) *= s;
34  z(i) = std::copysign(norm(i), z(i));
35  } else {
36  T s = norm(i) / std::sqrt(x(i) * x(i) + y(i) * y(i));
37  x(i) *= s;
38  y(i) *= s;
39  z(i) *= T(3.0 / 2);
40  }
41  }
42 }
43 
46 template <class T, int VECSIZE>
47 inline void MapCylinderToCube(Eigen::Array<T, VECSIZE, 1>& x,
48  Eigen::Array<T, VECSIZE, 1>& y,
49  Eigen::Array<T, VECSIZE, 1>& z) {
50  Eigen::Array<T, VECSIZE, 1> sq_norm_xy = x * x + y * y;
51  Eigen::Array<T, VECSIZE, 1> norm_xy = sq_norm_xy.sqrt();
52 
53  for (int i = 0; i < VECSIZE; ++i) {
54  if (sq_norm_xy(i) < T(1e-12)) {
55  x(i) = y(i) = T(0);
56  } else if (std::abs(y(i)) <= std::abs(x(i))) {
57  T tmp = std::copysign(norm_xy(i), x(i));
58  y(i) = tmp * T(4 / M_PI) * std::atan(y(i) / x(i));
59  x(i) = tmp;
60  } else // if( std::abs(x(i)) <= y(i) )
61  {
62  T tmp = std::copysign(norm_xy(i), y(i));
63  x(i) = tmp * T(4 / M_PI) * std::atan(x(i) / y(i));
64  y(i) = tmp;
65  }
66  }
67 }
68 
106 template <bool ALIGN_CORNERS, CoordinateMapping MAPPING, class T, int VECSIZE>
108  Eigen::Array<T, VECSIZE, 1>& x,
109  Eigen::Array<T, VECSIZE, 1>& y,
110  Eigen::Array<T, VECSIZE, 1>& z,
111  const Eigen::Array<int, 3, 1>& filter_size,
112  const Eigen::Array<T, VECSIZE, 3>& inv_extents,
113  const Eigen::Array<T, 3, 1>& offset) {
115  // x,y,z is now in the range [-1,1]
116  x *= 2 * inv_extents.col(0);
117  y *= 2 * inv_extents.col(1);
118  z *= 2 * inv_extents.col(2);
119 
120  Eigen::Array<T, VECSIZE, 1> radius = (x * x + y * y + z * z).sqrt();
121  for (int i = 0; i < VECSIZE; ++i) {
122  T abs_max = std::max(std::abs(x(i)),
123  std::max(std::abs(y(i)), std::abs(z(i))));
124  if (abs_max < T(1e-8)) {
125  x(i) = 0;
126  y(i) = 0;
127  z(i) = 0;
128  } else {
129  // map to the unit cube with edge length 1 and range [-0.5,0.5]
130  x(i) *= T(0.5) * radius(i) / abs_max;
131  y(i) *= T(0.5) * radius(i) / abs_max;
132  z(i) *= T(0.5) * radius(i) / abs_max;
133  }
134  }
136  // x,y,z is now in the range [-1,1]
137  x *= 2 * inv_extents.col(0);
138  y *= 2 * inv_extents.col(1);
139  z *= 2 * inv_extents.col(2);
140 
141  MapSphereToCylinder(x, y, z);
142  MapCylinderToCube(x, y, z);
143 
144  x *= T(0.5);
145  y *= T(0.5);
146  z *= T(0.5);
147  } else {
148  // map to the unit cube with edge length 1 and range [-0.5,0.5]
149  x *= inv_extents.col(0);
150  y *= inv_extents.col(1);
151  z *= inv_extents.col(2);
152  }
153 
154  if (ALIGN_CORNERS) {
155  x += T(0.5);
156  y += T(0.5);
157  z += T(0.5);
158 
159  x *= filter_size.x() - 1;
160  y *= filter_size.y() - 1;
161  z *= filter_size.z() - 1;
162  } else {
163  x *= filter_size.x();
164  y *= filter_size.y();
165  z *= filter_size.z();
166 
167  x += offset.x();
168  y += offset.y();
169  z += offset.z();
170 
171  // integer div
172  x += filter_size.x() / 2;
173  y += filter_size.y() / 2;
174  z += filter_size.z() / 2;
175 
176  // shift if the filter size is even
177  if (filter_size.x() % 2 == 0) x -= T(0.5);
178  if (filter_size.y() % 2 == 0) y -= T(0.5);
179  if (filter_size.z() % 2 == 0) z -= T(0.5);
180  }
181 }
182 
184 template <class T, int VECSIZE, InterpolationMode INTERPOLATION>
186 
188 template <class T, int VECSIZE>
190  typedef Eigen::Array<T, 1, VECSIZE> Weight_t;
191  typedef Eigen::Array<int, 1, VECSIZE> Idx_t;
192 
195  static constexpr int Size() { return 1; };
196 
214  inline void Interpolate(Eigen::Array<T, 1, VECSIZE>& w,
215  Eigen::Array<int, 1, VECSIZE>& idx,
216  const Eigen::Array<T, VECSIZE, 1>& x,
217  const Eigen::Array<T, VECSIZE, 1>& y,
218  const Eigen::Array<T, VECSIZE, 1>& z,
219  const Eigen::Array<int, 3, 1>& filter_size,
220  int num_channels = 1) const {
221  Eigen::Array<int, VECSIZE, 1> xi, yi, zi;
222 
223  xi = x.round().template cast<int>();
224  yi = y.round().template cast<int>();
225  zi = z.round().template cast<int>();
226 
227  // clamp to the valid range
228  xi = xi.min(filter_size.x() - 1).max(0);
229  yi = yi.min(filter_size.y() - 1).max(0);
230  zi = zi.min(filter_size.z() - 1).max(0);
231  idx = num_channels * (zi * filter_size.y() * filter_size.x() +
232  yi * filter_size.x() + xi);
233  w = 1;
234  }
235 };
236 
238 template <class T, int VECSIZE>
240  typedef Eigen::Array<T, 8, VECSIZE> Weight_t;
241  typedef Eigen::Array<int, 8, VECSIZE> Idx_t;
242 
243  static constexpr int Size() { return 8; };
244 
245  inline void Interpolate(Eigen::Array<T, 8, VECSIZE>& w,
246  Eigen::Array<int, 8, VECSIZE>& idx,
247  const Eigen::Array<T, VECSIZE, 1>& x,
248  const Eigen::Array<T, VECSIZE, 1>& y,
249  const Eigen::Array<T, VECSIZE, 1>& z,
250  const Eigen::Array<int, 3, 1>& filter_size,
251  int num_channels = 1) const {
252  for (int i = 0; i < VECSIZE; ++i) {
253  int xi0 = std::max(0, std::min(int(x(i)), filter_size.x() - 1));
254  int xi1 = std::max(0, std::min(xi0 + 1, filter_size.x() - 1));
255 
256  int yi0 = std::max(0, std::min(int(y(i)), filter_size.y() - 1));
257  int yi1 = std::max(0, std::min(yi0 + 1, filter_size.y() - 1));
258 
259  int zi0 = std::max(0, std::min(int(z(i)), filter_size.z() - 1));
260  int zi1 = std::max(0, std::min(zi0 + 1, filter_size.z() - 1));
261 
262  T a = std::max(T(0), std::min(x(i) - xi0, T(1)));
263  T b = std::max(T(0), std::min(y(i) - yi0, T(1)));
264  T c = std::max(T(0), std::min(z(i) - zi0, T(1)));
265 
266  w.col(i) << (1 - a) * (1 - b) * (1 - c), (a) * (1 - b) * (1 - c),
267  (1 - a) * (b) * (1 - c), (a) * (b) * (1 - c),
268  (1 - a) * (1 - b) * (c), (a) * (1 - b) * (c),
269  (1 - a) * (b) * (c), (a) * (b) * (c);
270 
271  idx.col(i) << zi0 * filter_size.y() * filter_size.x() +
272  yi0 * filter_size.x() + xi0,
273  zi0 * filter_size.y() * filter_size.x() +
274  yi0 * filter_size.x() + xi1,
275  zi0 * filter_size.y() * filter_size.x() +
276  yi1 * filter_size.x() + xi0,
277  zi0 * filter_size.y() * filter_size.x() +
278  yi1 * filter_size.x() + xi1,
279  zi1 * filter_size.y() * filter_size.x() +
280  yi0 * filter_size.x() + xi0,
281  zi1 * filter_size.y() * filter_size.x() +
282  yi0 * filter_size.x() + xi1,
283  zi1 * filter_size.y() * filter_size.x() +
284  yi1 * filter_size.x() + xi0,
285  zi1 * filter_size.y() * filter_size.x() +
286  yi1 * filter_size.x() + xi1;
287  }
288  idx *= num_channels;
289  }
290 };
291 
293 template <class T, int VECSIZE>
295  typedef Eigen::Array<T, 8, VECSIZE> Weight_t;
296  typedef Eigen::Array<int, 8, VECSIZE> Idx_t;
297 
298  static constexpr int Size() { return 8; };
299 
300  inline void Interpolate(Eigen::Array<T, 8, VECSIZE>& w,
301  Eigen::Array<int, 8, VECSIZE>& idx,
302  const Eigen::Array<T, VECSIZE, 1>& x,
303  const Eigen::Array<T, VECSIZE, 1>& y,
304  const Eigen::Array<T, VECSIZE, 1>& z,
305  const Eigen::Array<int, 3, 1>& filter_size,
306  int num_channels = 1) const {
307  for (int i = 0; i < VECSIZE; ++i) {
308  int xi0 = int(std::floor(x(i)));
309  int xi1 = xi0 + 1;
310 
311  int yi0 = int(std::floor(y(i)));
312  int yi1 = yi0 + 1;
313 
314  int zi0 = int(std::floor(z(i)));
315  int zi1 = zi0 + 1;
316 
317  T a = x(i) - xi0;
318  T b = y(i) - yi0;
319  T c = z(i) - zi0;
320 
321  if (zi0 < 0 || yi0 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
322  yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
323  idx(0, i) = 0;
324  w(0, i) = 0;
325  } else {
326  idx(0, i) = zi0 * filter_size.y() * filter_size.x() +
327  yi0 * filter_size.x() + xi0;
328  w(0, i) = (1 - a) * (1 - b) * (1 - c);
329  }
330 
331  if (zi0 < 0 || yi0 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
332  yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
333  idx(1, i) = 0;
334  w(1, i) = 0;
335  } else {
336  idx(1, i) = zi0 * filter_size.y() * filter_size.x() +
337  yi0 * filter_size.x() + xi1;
338  w(1, i) = (a) * (1 - b) * (1 - c);
339  }
340 
341  if (zi0 < 0 || yi1 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
342  yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
343  idx(2, i) = 0;
344  w(2, i) = 0;
345  } else {
346  idx(2, i) = zi0 * filter_size.y() * filter_size.x() +
347  yi1 * filter_size.x() + xi0;
348  w(2, i) = (1 - a) * (b) * (1 - c);
349  }
350 
351  if (zi0 < 0 || yi1 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
352  yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
353  idx(3, i) = 0;
354  w(3, i) = 0;
355  } else {
356  idx(3, i) = zi0 * filter_size.y() * filter_size.x() +
357  yi1 * filter_size.x() + xi1;
358  w(3, i) = (a) * (b) * (1 - c);
359  }
360 
361  if (zi1 < 0 || yi0 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
362  yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
363  idx(4, i) = 0;
364  w(4, i) = 0;
365  } else {
366  idx(4, i) = zi1 * filter_size.y() * filter_size.x() +
367  yi0 * filter_size.x() + xi0;
368  w(4, i) = (1 - a) * (1 - b) * (c);
369  }
370 
371  if (zi1 < 0 || yi0 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
372  yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
373  idx(5, i) = 0;
374  w(5, i) = 0;
375  } else {
376  idx(5, i) = zi1 * filter_size.y() * filter_size.x() +
377  yi0 * filter_size.x() + xi1;
378  w(5, i) = (a) * (1 - b) * (c);
379  }
380 
381  if (zi1 < 0 || yi1 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
382  yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
383  idx(6, i) = 0;
384  w(6, i) = 0;
385  } else {
386  idx(6, i) = zi1 * filter_size.y() * filter_size.x() +
387  yi1 * filter_size.x() + xi0;
388  w(6, i) = (1 - a) * (b) * (c);
389  }
390 
391  if (zi1 < 0 || yi1 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
392  yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
393  idx(7, i) = 0;
394  w(7, i) = 0;
395  } else {
396  idx(7, i) = zi1 * filter_size.y() * filter_size.x() +
397  yi1 * filter_size.x() + xi1;
398  w(7, i) = (a) * (b) * (c);
399  }
400  }
401  idx *= num_channels;
402  }
403 };
404 
405 } // namespace impl
406 } // namespace ml
407 } // namespace open3d
#define VECSIZE
int offset
Definition: FilePCD.cpp:45
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:474
InterpolationMode
Definition: ContinuousConvTypes.h:18
@ NEAREST_NEIGHBOR
Definition: VoxelPooling.h:21
void MapSphereToCylinder(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z)
Definition: CoordinateTransformation.h:21
void MapCylinderToCube(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z)
Definition: CoordinateTransformation.h:47
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:107
FN_SPECIFIERS MiniVec< float, N > floor(const MiniVec< float, N > &a)
Definition: MiniVec.h:75
Definition: PinholeCameraIntrinsic.cpp:16
static constexpr int Size()
Definition: CoordinateTransformation.h:298
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:295
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:296
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:300
Eigen::Array< T, 1, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:190
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:214
static constexpr int Size()
Definition: CoordinateTransformation.h:195
Eigen::Array< int, 1, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:191
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:240
static constexpr int Size()
Definition: CoordinateTransformation.h:243
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:241
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:245
Class for computing interpolation weights.
Definition: CoordinateTransformation.h:185