Open3D (C++ API)  0.18.0+252c867
TransformationConverterImpl.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 // Private header. Do not include in Open3d.h.
9 
10 #pragma once
11 
12 #include <cmath>
13 
14 #include "open3d/core/CUDAUtils.h"
15 
16 namespace open3d {
17 namespace t {
18 namespace pipelines {
19 namespace kernel {
20 
22 template <typename scalar_t>
24  scalar_t *transformation_ptr, const scalar_t *pose_ptr) {
25  transformation_ptr[0] = cos(pose_ptr[2]) * cos(pose_ptr[1]);
26  transformation_ptr[1] =
27  -1 * sin(pose_ptr[2]) * cos(pose_ptr[0]) +
28  cos(pose_ptr[2]) * sin(pose_ptr[1]) * sin(pose_ptr[0]);
29  transformation_ptr[2] =
30  sin(pose_ptr[2]) * sin(pose_ptr[0]) +
31  cos(pose_ptr[2]) * sin(pose_ptr[1]) * cos(pose_ptr[0]);
32  transformation_ptr[4] = sin(pose_ptr[2]) * cos(pose_ptr[1]);
33  transformation_ptr[5] =
34  cos(pose_ptr[2]) * cos(pose_ptr[0]) +
35  sin(pose_ptr[2]) * sin(pose_ptr[1]) * sin(pose_ptr[0]);
36  transformation_ptr[6] =
37  -1 * cos(pose_ptr[2]) * sin(pose_ptr[0]) +
38  sin(pose_ptr[2]) * sin(pose_ptr[1]) * cos(pose_ptr[0]);
39  transformation_ptr[8] = -1 * sin(pose_ptr[1]);
40  transformation_ptr[9] = cos(pose_ptr[1]) * sin(pose_ptr[0]);
41  transformation_ptr[10] = cos(pose_ptr[1]) * cos(pose_ptr[0]);
42 }
43 
46 template <typename scalar_t>
48  scalar_t *pose_ptr, const scalar_t *transformation_ptr) {
49  const scalar_t sy = sqrt(transformation_ptr[0] * transformation_ptr[0] +
50  transformation_ptr[4] * transformation_ptr[4]);
51  if (!(sy < 1e-6)) {
52  pose_ptr[0] = atan2(transformation_ptr[9], transformation_ptr[10]);
53  pose_ptr[1] = atan2(-transformation_ptr[8], sy);
54  pose_ptr[2] = atan2(transformation_ptr[4], transformation_ptr[0]);
55  } else {
56  pose_ptr[0] = atan2(-transformation_ptr[6], transformation_ptr[5]);
57  pose_ptr[1] = atan2(-transformation_ptr[8], sy);
58  pose_ptr[2] = 0;
59  }
60 }
61 
62 #ifdef BUILD_CUDA_MODULE
67 template <typename scalar_t>
68 void PoseToTransformationCUDA(scalar_t *transformation_ptr,
69  const scalar_t *pose_ptr);
70 
75 template <typename scalar_t>
76 void TransformationToPoseCUDA(scalar_t *pose_ptr,
77  const scalar_t *transformation_ptr);
78 #endif
79 
80 } // namespace kernel
81 } // namespace pipelines
82 } // namespace t
83 } // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:44
OPEN3D_HOST_DEVICE void TransformationToPoseImpl(scalar_t *pose_ptr, const scalar_t *transformation_ptr)
Definition: TransformationConverterImpl.h:47
OPEN3D_HOST_DEVICE void PoseToTransformationImpl(scalar_t *transformation_ptr, const scalar_t *pose_ptr)
Shared implementation for PoseToTransformation function.
Definition: TransformationConverterImpl.h:23
Definition: PinholeCameraIntrinsic.cpp:16