Open3D (C++ API)  0.18.0+0cf605f
Utility.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 "open3d/core/Tensor.h"
12 
13 namespace open3d {
14 namespace t {
15 namespace geometry {
16 
17 inline void CheckDepthTensor(const core::Tensor& depth) {
18  if (depth.NumElements() == 0) {
19  utility::LogError("Input depth is empty.");
20  }
21  if (depth.GetDtype() != core::UInt16 && depth.GetDtype() != core::Float32) {
22  utility::LogError("Unsupported depth image dtype {}.",
23  depth.GetDtype().ToString());
24  }
25 }
26 
27 inline void CheckColorTensor(const core::Tensor& color) {
28  if (color.NumElements() == 0) {
29  utility::LogError("Input color is empty.");
30  }
31  if (color.GetDtype() != core::UInt8 && color.GetDtype() != core::Float32) {
32  utility::LogError("Unsupported color image dtype {}.",
33  color.GetDtype().ToString());
34  }
35 }
36 
37 inline void CheckIntrinsicTensor(const core::Tensor& intrinsic) {
38  if (intrinsic.GetShape() != core::SizeVector{3, 3}) {
39  utility::LogError("Unsupported intrinsic matrix shape {}",
40  intrinsic.GetShape());
41  }
42 
43  if (intrinsic.GetDtype() != core::Dtype::Float64) {
44  utility::LogError("Unsupported intrinsic matrix dtype {}",
45  intrinsic.GetDtype().ToString());
46  }
47 
48  if (!intrinsic.IsContiguous()) {
49  utility::LogError("Intrinsic matrix must be contiguous.");
50  }
51 }
52 
53 inline void CheckExtrinsicTensor(const core::Tensor& extrinsic) {
54  if (extrinsic.GetShape() != core::SizeVector{4, 4}) {
55  utility::LogError("Unsupported extrinsic matrix shape {}",
56  extrinsic.GetShape());
57  }
58 
59  if (extrinsic.GetDtype() != core::Dtype::Float64) {
60  utility::LogError("Unsupported extrinsic matrix dtype {}",
61  extrinsic.GetDtype().ToString());
62  }
63 
64  if (!extrinsic.IsContiguous()) {
65  utility::LogError("Extrinsic matrix must be contiguous.");
66  }
67 }
68 
69 inline void CheckBlockCoorinates(const core::Tensor& block_coords) {
70  if (block_coords.GetDtype() != core::Dtype::Int32) {
71  utility::LogError("Unsupported block coordinate dtype {}",
72  block_coords.GetDtype().ToString());
73  }
74 }
75 
78  core::AssertTensorShape(T, {4, 4});
81  if (!T.IsContiguous()) {
82  utility::LogError("T is expected to be contiguous");
83  }
84 
85  core::Tensor Tinv({4, 4}, core::Float64, core::Device("CPU:0"));
86  const double* T_ptr = T.GetDataPtr<double>();
87  double* Tinv_ptr = Tinv.GetDataPtr<double>();
88 
89  // R' = R.T
90  Tinv_ptr[0 * 4 + 0] = T_ptr[0 * 4 + 0];
91  Tinv_ptr[0 * 4 + 1] = T_ptr[1 * 4 + 0];
92  Tinv_ptr[0 * 4 + 2] = T_ptr[2 * 4 + 0];
93 
94  Tinv_ptr[1 * 4 + 0] = T_ptr[0 * 4 + 1];
95  Tinv_ptr[1 * 4 + 1] = T_ptr[1 * 4 + 1];
96  Tinv_ptr[1 * 4 + 2] = T_ptr[2 * 4 + 1];
97 
98  Tinv_ptr[2 * 4 + 0] = T_ptr[0 * 4 + 2];
99  Tinv_ptr[2 * 4 + 1] = T_ptr[1 * 4 + 2];
100  Tinv_ptr[2 * 4 + 2] = T_ptr[2 * 4 + 2];
101 
102  // t' = -R.T @ t = -R' @ t
103  Tinv_ptr[0 * 4 + 3] = -(Tinv_ptr[0 * 4 + 0] * T_ptr[0 * 4 + 3] +
104  Tinv_ptr[0 * 4 + 1] * T_ptr[1 * 4 + 3] +
105  Tinv_ptr[0 * 4 + 2] * T_ptr[2 * 4 + 3]);
106  Tinv_ptr[1 * 4 + 3] = -(Tinv_ptr[1 * 4 + 0] * T_ptr[0 * 4 + 3] +
107  Tinv_ptr[1 * 4 + 1] * T_ptr[1 * 4 + 3] +
108  Tinv_ptr[1 * 4 + 2] * T_ptr[2 * 4 + 3]);
109  Tinv_ptr[2 * 4 + 3] = -(Tinv_ptr[2 * 4 + 0] * T_ptr[0 * 4 + 3] +
110  Tinv_ptr[2 * 4 + 1] * T_ptr[1 * 4 + 3] +
111  Tinv_ptr[2 * 4 + 2] * T_ptr[2 * 4 + 3]);
112 
113  // Remaining part
114  Tinv_ptr[3 * 4 + 0] = 0;
115  Tinv_ptr[3 * 4 + 1] = 0;
116  Tinv_ptr[3 * 4 + 2] = 0;
117  Tinv_ptr[3 * 4 + 3] = 1;
118 
119  return Tinv;
120 }
121 } // namespace geometry
122 } // namespace t
123 } // namespace open3d
math::float4 color
Definition: LineSetBuffers.cpp:45
#define LogError(...)
Definition: Logging.h:48
#define AssertTensorDevice(tensor,...)
Definition: TensorCheck.h:43
#define AssertTensorDtype(tensor,...)
Definition: TensorCheck.h:21
#define AssertTensorShape(tensor,...)
Definition: TensorCheck.h:58
Definition: Device.h:18
static const Dtype Float64
Definition: Dtype.h:24
static const Dtype Int32
Definition: Dtype.h:27
std::string ToString() const
Definition: Dtype.h:64
Definition: SizeVector.h:69
Definition: Tensor.h:32
T * GetDataPtr()
Definition: Tensor.h:1143
SizeVector GetShape() const
Definition: Tensor.h:1126
bool IsContiguous() const
Definition: Tensor.h:1035
int64_t NumElements() const
Definition: Tensor.h:1169
Dtype GetDtype() const
Definition: Tensor.h:1163
const Dtype UInt16
Definition: Dtype.cpp:49
const Dtype UInt8
Definition: Dtype.cpp:48
const Dtype Float64
Definition: Dtype.cpp:43
const Dtype Float32
Definition: Dtype.cpp:42
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:77
void CheckBlockCoorinates(const core::Tensor &block_coords)
Definition: Utility.h:69
void CheckIntrinsicTensor(const core::Tensor &intrinsic)
Definition: Utility.h:37
void CheckColorTensor(const core::Tensor &color)
Definition: Utility.h:27
void CheckDepthTensor(const core::Tensor &depth)
Definition: Utility.h:17
void CheckExtrinsicTensor(const core::Tensor &extrinsic)
Definition: Utility.h:53
Definition: PinholeCameraIntrinsic.cpp:16