Open3D (C++ API)  0.18.0
Loading...
Searching...
No Matches
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
15
16namespace open3d {
17namespace t {
18namespace pipelines {
19namespace kernel {
20
22template <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
46template <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
67template <typename scalar_t>
68void PoseToTransformationCUDA(scalar_t *transformation_ptr,
69 const scalar_t *pose_ptr);
70
75template <typename scalar_t>
76void 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