Open3D (C++ API)  0.16.0
Utility.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// The MIT License (MIT)
5//
6// Copyright (c) 2018-2021 www.open3d.org
7//
8// Permission is hereby granted, free of charge, to any person obtaining a copy
9// of this software and associated documentation files (the "Software"), to deal
10// in the Software without restriction, including without limitation the rights
11// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12// copies of the Software, and to permit persons to whom the Software is
13// furnished to do so, subject to the following conditions:
14//
15// The above copyright notice and this permission notice shall be included in
16// all copies or substantial portions of the Software.
17//
18// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24// IN THE SOFTWARE.
25// ----------------------------------------------------------------------------
26
27#pragma once
28
29#include "open3d/core/Tensor.h"
31
32namespace open3d {
33namespace t {
34namespace geometry {
35
36inline void CheckDepthTensor(const core::Tensor& depth) {
37 if (depth.NumElements() == 0) {
38 utility::LogError("Input depth is empty.");
39 }
40 if (depth.GetDtype() != core::UInt16 && depth.GetDtype() != core::Float32) {
41 utility::LogError("Unsupported depth image dtype {}.",
42 depth.GetDtype().ToString());
43 }
44}
45
46inline void CheckColorTensor(const core::Tensor& color) {
47 if (color.NumElements() == 0) {
48 utility::LogError("Input color is empty.");
49 }
50 if (color.GetDtype() != core::UInt8 && color.GetDtype() != core::Float32) {
51 utility::LogError("Unsupported color image dtype {}.",
52 color.GetDtype().ToString());
53 }
54}
55
56inline void CheckIntrinsicTensor(const core::Tensor& intrinsic) {
57 if (intrinsic.GetShape() != core::SizeVector{3, 3}) {
58 utility::LogError("Unsupported intrinsic matrix shape {}",
59 intrinsic.GetShape());
60 }
61
62 if (intrinsic.GetDtype() != core::Dtype::Float64) {
63 utility::LogError("Unsupported intrinsic matrix dtype {}",
64 intrinsic.GetDtype().ToString());
65 }
66
67 if (!intrinsic.IsContiguous()) {
68 utility::LogError("Intrinsic matrix must be contiguous.");
69 }
70}
71
72inline void CheckExtrinsicTensor(const core::Tensor& extrinsic) {
73 if (extrinsic.GetShape() != core::SizeVector{4, 4}) {
74 utility::LogError("Unsupported extrinsic matrix shape {}",
75 extrinsic.GetShape());
76 }
77
78 if (extrinsic.GetDtype() != core::Dtype::Float64) {
79 utility::LogError("Unsupported extrinsic matrix dtype {}",
80 extrinsic.GetDtype().ToString());
81 }
82
83 if (!extrinsic.IsContiguous()) {
84 utility::LogError("Extrinsic matrix must be contiguous.");
85 }
86}
87
88inline void CheckBlockCoorinates(const core::Tensor& block_coords) {
89 if (block_coords.GetDtype() != core::Dtype::Int32) {
90 utility::LogError("Unsupported block coordinate dtype {}",
91 block_coords.GetDtype().ToString());
92 }
93}
94
97 core::AssertTensorShape(T, {4, 4});
100 if (!T.IsContiguous()) {
101 utility::LogError("T is expected to be contiguous");
102 }
103
104 core::Tensor Tinv({4, 4}, core::Float64, core::Device("CPU:0"));
105 const double* T_ptr = T.GetDataPtr<double>();
106 double* Tinv_ptr = Tinv.GetDataPtr<double>();
107
108 // R' = R.T
109 Tinv_ptr[0 * 4 + 0] = T_ptr[0 * 4 + 0];
110 Tinv_ptr[0 * 4 + 1] = T_ptr[1 * 4 + 0];
111 Tinv_ptr[0 * 4 + 2] = T_ptr[2 * 4 + 0];
112
113 Tinv_ptr[1 * 4 + 0] = T_ptr[0 * 4 + 1];
114 Tinv_ptr[1 * 4 + 1] = T_ptr[1 * 4 + 1];
115 Tinv_ptr[1 * 4 + 2] = T_ptr[2 * 4 + 1];
116
117 Tinv_ptr[2 * 4 + 0] = T_ptr[0 * 4 + 2];
118 Tinv_ptr[2 * 4 + 1] = T_ptr[1 * 4 + 2];
119 Tinv_ptr[2 * 4 + 2] = T_ptr[2 * 4 + 2];
120
121 // t' = -R.T @ t = -R' @ t
122 Tinv_ptr[0 * 4 + 3] = -(Tinv_ptr[0 * 4 + 0] * T_ptr[0 * 4 + 3] +
123 Tinv_ptr[0 * 4 + 1] * T_ptr[1 * 4 + 3] +
124 Tinv_ptr[0 * 4 + 2] * T_ptr[2 * 4 + 3]);
125 Tinv_ptr[1 * 4 + 3] = -(Tinv_ptr[1 * 4 + 0] * T_ptr[0 * 4 + 3] +
126 Tinv_ptr[1 * 4 + 1] * T_ptr[1 * 4 + 3] +
127 Tinv_ptr[1 * 4 + 2] * T_ptr[2 * 4 + 3]);
128 Tinv_ptr[2 * 4 + 3] = -(Tinv_ptr[2 * 4 + 0] * T_ptr[0 * 4 + 3] +
129 Tinv_ptr[2 * 4 + 1] * T_ptr[1 * 4 + 3] +
130 Tinv_ptr[2 * 4 + 2] * T_ptr[2 * 4 + 3]);
131
132 // Remaining part
133 Tinv_ptr[3 * 4 + 0] = 0;
134 Tinv_ptr[3 * 4 + 1] = 0;
135 Tinv_ptr[3 * 4 + 2] = 0;
136 Tinv_ptr[3 * 4 + 3] = 1;
137
138 return Tinv;
139}
140} // namespace geometry
141} // namespace t
142} // namespace open3d
math::float4 color
Definition: LineSetBuffers.cpp:64
#define LogError(...)
Definition: Logging.h:67
#define AssertTensorDevice(tensor,...)
Definition: TensorCheck.h:62
#define AssertTensorDtype(tensor,...)
Definition: TensorCheck.h:40
#define AssertTensorShape(tensor,...)
Definition: TensorCheck.h:77
Definition: Device.h:37
static const Dtype Float64
Definition: Dtype.h:43
static const Dtype Int32
Definition: Dtype.h:46
std::string ToString() const
Definition: Dtype.h:83
Definition: SizeVector.h:88
Definition: Tensor.h:51
SizeVector GetShape() const
Definition: Tensor.h:1132
T * GetDataPtr()
Definition: Tensor.h:1149
bool IsContiguous() const
Definition: Tensor.h:1041
int64_t NumElements() const
Definition: Tensor.h:1175
Dtype GetDtype() const
Definition: Tensor.h:1169
const Dtype UInt16
Definition: Dtype.cpp:68
const Dtype UInt8
Definition: Dtype.cpp:67
const Dtype Float64
Definition: Dtype.cpp:62
const Dtype Float32
Definition: Dtype.cpp:61
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:96
void CheckBlockCoorinates(const core::Tensor &block_coords)
Definition: Utility.h:88
void CheckIntrinsicTensor(const core::Tensor &intrinsic)
Definition: Utility.h:56
void CheckColorTensor(const core::Tensor &color)
Definition: Utility.h:46
void CheckDepthTensor(const core::Tensor &depth)
Definition: Utility.h:36
void CheckExtrinsicTensor(const core::Tensor &extrinsic)
Definition: Utility.h:72
Definition: PinholeCameraIntrinsic.cpp:35