Open3D (C++ API)  0.18.0
Loading...
Searching...
No Matches
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
14namespace open3d {
15namespace ml {
16namespace impl {
17
20template <class T, int VECSIZE>
21inline 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
46template <class T, int VECSIZE>
47inline 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
106template <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
184template <class T, int VECSIZE, InterpolationMode INTERPOLATION>
186
188template <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
238template <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
293template <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
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
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