29 #include <Eigen/Geometry> 39 template <
class T,
int VECSIZE>
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();
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))));
53 z(i) = std::copysign(norm(i), z(i));
55 T s = norm(i) / std::sqrt(x(i) * x(i) + y(i) * y(i));
65 template <
class T,
int VECSIZE>
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();
72 for (
int i = 0; i < VECSIZE; ++i) {
73 if (sq_norm_xy(i) < T(1e-12)) {
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));
81 T tmp = std::copysign(norm_xy(i), y(i));
82 x(i) = tmp * T(4 / M_PI) * std::atan(x(i) / y(i));
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) {
135 x *= 2 * inv_extents.col(0);
136 y *= 2 * inv_extents.col(1);
137 z *= 2 * inv_extents.col(2);
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)) {
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;
156 x *= 2 * inv_extents.col(0);
157 y *= 2 * inv_extents.col(1);
158 z *= 2 * inv_extents.col(2);
168 x *= inv_extents.col(0);
169 y *= inv_extents.col(1);
170 z *= inv_extents.col(2);
178 x *= filter_size.x() - 1;
179 y *= filter_size.y() - 1;
180 z *= filter_size.z() - 1;
182 x *= filter_size.x();
183 y *= filter_size.y();
184 z *= filter_size.z();
191 x += filter_size.x() / 2;
192 y += filter_size.y() / 2;
193 z += filter_size.z() / 2;
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);
203 template <
class T,
int VECSIZE, InterpolationMode INTERPOLATION>
207 template <
class T,
int VECSIZE>
210 typedef Eigen::Array<int, 1, VECSIZE>
Idx_t;
214 static constexpr
int Size() {
return 1; };
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;
242 xi = x.round().template cast<int>();
243 yi = y.round().template cast<int>();
244 zi = z.round().template cast<int>();
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);
257 template <
class T,
int VECSIZE>
260 typedef Eigen::Array<int, 8, VECSIZE>
Idx_t;
262 static constexpr
int Size() {
return 8; };
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));
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));
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));
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)));
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);
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;
312 template <
class T,
int VECSIZE>
315 typedef Eigen::Array<int, 8, VECSIZE>
Idx_t;
317 static constexpr
int Size() {
return 8; };
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) {
340 if (zi0 < 0 || yi0 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
341 yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
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);
350 if (zi0 < 0 || yi0 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
351 yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
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);
360 if (zi0 < 0 || yi1 < 0 || xi0 < 0 || zi0 >= filter_size.z() ||
361 yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
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);
370 if (zi0 < 0 || yi1 < 0 || xi1 < 0 || zi0 >= filter_size.z() ||
371 yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
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);
380 if (zi1 < 0 || yi0 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
381 yi0 >= filter_size.y() || xi0 >= filter_size.x()) {
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);
390 if (zi1 < 0 || yi0 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
391 yi0 >= filter_size.y() || xi1 >= filter_size.x()) {
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);
400 if (zi1 < 0 || yi1 < 0 || xi0 < 0 || zi1 >= filter_size.z() ||
401 yi1 >= filter_size.y() || xi0 >= filter_size.x()) {
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);
410 if (zi1 < 0 || yi1 < 0 || xi1 < 0 || zi1 >= filter_size.z() ||
411 yi1 >= filter_size.y() || xi1 >= filter_size.x()) {
415 idx(7, i) = zi1 * filter_size.y() * filter_size.x() +
416 yi1 * filter_size.x() + xi1;
417 w(7, i) = (a) * (b) * (c);
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