Open3D (C++ API)  0.15.1
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) 2018-2021 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
33namespace open3d {
34namespace ml {
35namespace impl {
36
39template <class T, int VECSIZE>
40inline 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
65template <class T, int VECSIZE>
66inline 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
125template <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
203template <class T, int VECSIZE, InterpolationMode INTERPOLATION>
205
207template <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
257template <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
312template <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
#define VECSIZE
int offset
Definition: FilePCD.cpp:64
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:493
InterpolationMode
Definition: ContinuousConvTypes.h:37
@ NEAREST_NEIGHBOR
Definition: VoxelPooling.h:40
void MapSphereToCylinder(Eigen::Array< T, VECSIZE, 1 > &x, Eigen::Array< T, VECSIZE, 1 > &y, Eigen::Array< T, VECSIZE, 1 > &z)
Definition: CoordinateTransformation.h:40
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 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
FN_SPECIFIERS MiniVec< float, N > floor(const MiniVec< float, N > &a)
Definition: MiniVec.h:94
Definition: PinholeCameraIntrinsic.cpp:35
static constexpr int Size()
Definition: CoordinateTransformation.h:317
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:314
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:315
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
Eigen::Array< T, 1, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:209
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
static constexpr int Size()
Definition: CoordinateTransformation.h:214
Eigen::Array< int, 1, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:210
Eigen::Array< T, 8, VECSIZE > Weight_t
Definition: CoordinateTransformation.h:259
static constexpr int Size()
Definition: CoordinateTransformation.h:262
Eigen::Array< int, 8, VECSIZE > Idx_t
Definition: CoordinateTransformation.h:260
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
Class for computing interpolation weights.
Definition: CoordinateTransformation.h:204