Open3D (C++ API)  0.15.1
GeometryIndexer.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 <unordered_map>
30
32#include "open3d/core/Tensor.h"
37
38namespace open3d {
39namespace t {
40namespace geometry {
41namespace kernel {
42
45public:
48 TransformIndexer(const core::Tensor& intrinsics,
49 const core::Tensor& extrinsics,
50 float scale = 1.0f) {
51 core::AssertTensorShape(intrinsics, {3, 3});
53 core::AssertTensorDevice(intrinsics, core::Device("CPU:0"));
54 if (!intrinsics.IsContiguous()) {
55 utility::LogError("Intrinsics is not contiguous");
56 }
57
58 core::AssertTensorShape(extrinsics, {4, 4});
60 core::AssertTensorDevice(extrinsics, core::Device("CPU:0"));
61 if (!extrinsics.IsContiguous()) {
62 utility::LogError("Extrinsics is not contiguous");
63 }
64
65 const double* intrinsic_ptr = intrinsics.GetDataPtr<double>();
66 const double* extrinsic_ptr = extrinsics.GetDataPtr<double>();
67 for (int i = 0; i < 3; ++i) {
68 for (int j = 0; j < 4; ++j) {
69 extrinsic_[i][j] = extrinsic_ptr[i * 4 + j];
70 }
71 }
72
73 fx_ = intrinsic_ptr[0 * 3 + 0];
74 fy_ = intrinsic_ptr[1 * 3 + 1];
75 cx_ = intrinsic_ptr[0 * 3 + 2];
76 cy_ = intrinsic_ptr[1 * 3 + 2];
77 scale_ = scale;
78 }
79
82 float y_in,
83 float z_in,
84 float* x_out,
85 float* y_out,
86 float* z_out) const {
87 x_in *= scale_;
88 y_in *= scale_;
89 z_in *= scale_;
90
91 *x_out = x_in * extrinsic_[0][0] + y_in * extrinsic_[0][1] +
92 z_in * extrinsic_[0][2] + extrinsic_[0][3];
93 *y_out = x_in * extrinsic_[1][0] + y_in * extrinsic_[1][1] +
94 z_in * extrinsic_[1][2] + extrinsic_[1][3];
95 *z_out = x_in * extrinsic_[2][0] + y_in * extrinsic_[2][1] +
96 z_in * extrinsic_[2][2] + extrinsic_[2][3];
97 }
98
100 OPEN3D_HOST_DEVICE void Rotate(float x_in,
101 float y_in,
102 float z_in,
103 float* x_out,
104 float* y_out,
105 float* z_out) const {
106 x_in *= scale_;
107 y_in *= scale_;
108 z_in *= scale_;
109
110 *x_out = x_in * extrinsic_[0][0] + y_in * extrinsic_[0][1] +
111 z_in * extrinsic_[0][2];
112 *y_out = x_in * extrinsic_[1][0] + y_in * extrinsic_[1][1] +
113 z_in * extrinsic_[1][2];
114 *z_out = x_in * extrinsic_[2][0] + y_in * extrinsic_[2][1] +
115 z_in * extrinsic_[2][2];
116 }
117
120 float y_in,
121 float z_in,
122 float* u_out,
123 float* v_out) const {
124 float inv_z = 1.0f / z_in;
125 *u_out = fx_ * x_in * inv_z + cx_;
126 *v_out = fy_ * y_in * inv_z + cy_;
127 }
128
131 float v_in,
132 float d_in,
133 float* x_out,
134 float* y_out,
135 float* z_out) const {
136 *x_out = (u_in - cx_) * d_in / fx_;
137 *y_out = (v_in - cy_) * d_in / fy_;
138 *z_out = d_in;
139 }
140
141 OPEN3D_HOST_DEVICE void GetFocalLength(float* fx, float* fy) const {
142 *fx = fx_;
143 *fy = fy_;
144 }
145
147 float* y,
148 float* z) const {
149 *x = extrinsic_[0][3];
150 *y = extrinsic_[1][3];
151 *z = extrinsic_[2][3];
152 }
153
154private:
155 float extrinsic_[3][4];
156
157 float fx_;
158 float fy_;
159 float cx_;
160 float cy_;
161
162 float scale_;
163};
164
177const int64_t MAX_RESOLUTION_DIMS = 4;
178
179template <typename index_t>
181public:
182 TArrayIndexer() : ptr_(nullptr), element_byte_size_(0), active_dims_(0) {
183 for (index_t i = 0; i < MAX_RESOLUTION_DIMS; ++i) {
184 shape_[i] = 0;
185 }
186 }
187
188 TArrayIndexer(const core::Tensor& ndarray, index_t active_dims) {
189 if (!ndarray.IsContiguous()) {
191 "[TArrayIndexer] Only support contiguous tensors for "
192 "general operations.");
193 }
194
195 core::SizeVector shape = ndarray.GetShape();
196 index_t n = ndarray.NumDims();
197 if (active_dims > MAX_RESOLUTION_DIMS || active_dims > n) {
199 "[TArrayIndexer] Tensor shape too large, only <= {} and "
200 "<= {} array dim is supported, but received {}.",
201 MAX_RESOLUTION_DIMS, n, active_dims);
202 }
203
204 // Leading dimensions are coordinates
205 active_dims_ = active_dims;
206 for (index_t i = 0; i < active_dims_; ++i) {
207 shape_[i] = shape[i];
208 }
209 // Trailing dimensions are channels
210 element_byte_size_ = ndarray.GetDtype().ByteSize();
211 for (index_t i = active_dims_; i < n; ++i) {
212 element_byte_size_ *= shape[i];
213 }
214
215 // Fill-in rest to make compiler happy, not actually used.
216 for (index_t i = active_dims_; i < MAX_RESOLUTION_DIMS; ++i) {
217 shape_[i] = 0;
218 }
219 ptr_ = const_cast<void*>(ndarray.GetDataPtr());
220 }
221
224 index_t n = static_cast<index_t>(shape.size());
225 if (n > MAX_RESOLUTION_DIMS) {
227 "[TArrayIndexer] SizeVector too large, only <= {} is "
228 "supported, but received {}.",
230 }
231 active_dims_ = n;
232 for (index_t i = 0; i < active_dims_; ++i) {
233 shape_[i] = shape[i];
234 }
235
236 // Fill-in rest to make compiler happy, not actually used.
237 for (index_t i = active_dims_; i < MAX_RESOLUTION_DIMS; ++i) {
238 shape_[i] = 0;
239 }
240
241 // Reserved
242 element_byte_size_ = 0;
243 ptr_ = nullptr;
244 }
245
246 OPEN3D_HOST_DEVICE index_t ElementByteSize() { return element_byte_size_; }
247
249 index_t num_elems = 1;
250 for (index_t i = 0; i < active_dims_; ++i) {
251 num_elems *= shape_[i];
252 }
253 return num_elems;
254 }
255
258 index_t y_in,
259 index_t* workload) const {
260 *workload = y_in * shape_[1] + x_in;
261 }
262
265 index_t y_in,
266 index_t z_in,
267 index_t* workload) const {
268 *workload = (z_in * shape_[1] + y_in) * shape_[2] + x_in;
269 }
270
273 index_t y_in,
274 index_t z_in,
275 index_t t_in,
276 index_t* workload) const {
277 *workload = ((t_in * shape_[1] + z_in) * shape_[2] + y_in) * shape_[3] +
278 x_in;
279 }
280
283 index_t* x_out,
284 index_t* y_out) const {
285 *x_out = workload % shape_[1];
286 *y_out = workload / shape_[1];
287 }
288
291 index_t* x_out,
292 index_t* y_out,
293 index_t* z_out) const {
294 *x_out = workload % shape_[2];
295 workload = (workload - *x_out) / shape_[2];
296 *y_out = workload % shape_[1];
297 *z_out = workload / shape_[1];
298 }
299
302 index_t* x_out,
303 index_t* y_out,
304 index_t* z_out,
305 index_t* t_out) const {
306 *x_out = workload % shape_[3];
307 workload = (workload - *x_out) / shape_[3];
308 *y_out = workload % shape_[2];
309 workload = (workload - *y_out) / shape_[2];
310 *z_out = workload % shape_[1];
311 *t_out = workload / shape_[1];
312 }
313
314 inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const {
315 return y >= 0 && x >= 0 && y <= shape_[0] - 1.0f &&
316 x <= shape_[1] - 1.0f;
317 }
318 inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const {
319 return z >= 0 && y >= 0 && x >= 0 && z <= shape_[0] - 1.0f &&
320 y <= shape_[1] - 1.0f && x <= shape_[2] - 1.0f;
321 }
322 inline OPEN3D_HOST_DEVICE bool InBoundary(float x,
323 float y,
324 float z,
325 float t) const {
326 return t >= 0 && z >= 0 && y >= 0 && x >= 0 && t <= shape_[0] - 1.0f &&
327 z <= shape_[1] - 1.0f && y <= shape_[2] - 1.0f &&
328 x <= shape_[3] - 1.0f;
329 }
330
331 inline OPEN3D_HOST_DEVICE index_t GetShape(int i) const {
332 return shape_[i];
333 }
334
335 inline OPEN3D_HOST_DEVICE void* GetDataPtr() const { return ptr_; }
336
337 template <typename T>
339 return static_cast<T*>(static_cast<void*>(static_cast<uint8_t*>(ptr_) +
340 x * element_byte_size_));
341 }
342
343 template <typename T>
345 index_t workload;
346 CoordToWorkload(x, y, &workload);
347 return static_cast<T*>(static_cast<void*>(
348 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
349 }
350
351 template <typename T>
353 index_t y,
354 index_t z) const {
355 index_t workload;
356 CoordToWorkload(x, y, z, &workload);
357 return static_cast<T*>(static_cast<void*>(
358 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
359 }
360
361 template <typename T>
363 index_t y,
364 index_t z,
365 index_t t) const {
366 index_t workload;
367 CoordToWorkload(x, y, z, t, &workload);
368 return static_cast<T*>(static_cast<void*>(
369 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
370 }
371
372private:
373 void* ptr_;
374 index_t element_byte_size_;
375 index_t active_dims_;
376
378};
379
381
382} // namespace kernel
383} // namespace geometry
384} // namespace t
385} // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:63
#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:39
int64_t ByteSize() const
Definition: Dtype.h:77
Definition: SizeVector.h:79
Definition: Tensor.h:51
SizeVector GetShape() const
Definition: Tensor.h:1091
T * GetDataPtr()
Definition: Tensor.h:1108
int64_t NumDims() const
Definition: Tensor.h:1136
bool IsContiguous() const
Definition: Tensor.h:1000
Dtype GetDtype() const
Definition: Tensor.h:1128
Definition: GeometryIndexer.h:180
OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload, index_t *x_out, index_t *y_out, index_t *z_out, index_t *t_out) const
Workload => 4D coordinate.
Definition: GeometryIndexer.h:301
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const
Definition: GeometryIndexer.h:314
TArrayIndexer()
Definition: GeometryIndexer.h:182
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y, index_t z, index_t t) const
Definition: GeometryIndexer.h:362
OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in, index_t y_in, index_t z_in, index_t t_in, index_t *workload) const
4D coordinate => workload
Definition: GeometryIndexer.h:272
TArrayIndexer(const core::Tensor &ndarray, index_t active_dims)
Definition: GeometryIndexer.h:188
OPEN3D_HOST_DEVICE void * GetDataPtr() const
Definition: GeometryIndexer.h:335
OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in, index_t y_in, index_t *workload) const
2D coordinate => workload
Definition: GeometryIndexer.h:257
OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload, index_t *x_out, index_t *y_out) const
Workload => 2D coordinate.
Definition: GeometryIndexer.h:282
OPEN3D_HOST_DEVICE index_t ElementByteSize()
Definition: GeometryIndexer.h:246
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const
Definition: GeometryIndexer.h:318
OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in, index_t y_in, index_t z_in, index_t *workload) const
3D coordinate => workload
Definition: GeometryIndexer.h:264
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x) const
Definition: GeometryIndexer.h:338
OPEN3D_HOST_DEVICE index_t GetShape(int i) const
Definition: GeometryIndexer.h:331
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y, index_t z) const
Definition: GeometryIndexer.h:352
OPEN3D_HOST_DEVICE index_t NumElements()
Definition: GeometryIndexer.h:248
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y) const
Definition: GeometryIndexer.h:344
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z, float t) const
Definition: GeometryIndexer.h:322
OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload, index_t *x_out, index_t *y_out, index_t *z_out) const
Workload => 3D coordinate.
Definition: GeometryIndexer.h:290
TArrayIndexer(const core::SizeVector &shape)
Only used for simple shapes.
Definition: GeometryIndexer.h:223
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition: GeometryIndexer.h:44
OPEN3D_HOST_DEVICE void Project(float x_in, float y_in, float z_in, float *u_out, float *v_out) const
Project a 3D coordinate in camera coordinate to a 2D uv coordinate.
Definition: GeometryIndexer.h:119
OPEN3D_HOST_DEVICE void Rotate(float x_in, float y_in, float z_in, float *x_out, float *y_out, float *z_out) const
Transform a 3D coordinate in camera coordinate to world coordinate.
Definition: GeometryIndexer.h:100
TransformIndexer(const core::Tensor &intrinsics, const core::Tensor &extrinsics, float scale=1.0f)
Definition: GeometryIndexer.h:48
OPEN3D_HOST_DEVICE void GetCameraPosition(float *x, float *y, float *z) const
Definition: GeometryIndexer.h:146
OPEN3D_HOST_DEVICE void RigidTransform(float x_in, float y_in, float z_in, float *x_out, float *y_out, float *z_out) const
Transform a 3D coordinate in camera coordinate to world coordinate.
Definition: GeometryIndexer.h:81
OPEN3D_HOST_DEVICE void Unproject(float u_in, float v_in, float d_in, float *x_out, float *y_out, float *z_out) const
Unproject a 2D uv coordinate with depth to 3D in camera coordinate.
Definition: GeometryIndexer.h:130
OPEN3D_HOST_DEVICE void GetFocalLength(float *fx, float *fy) const
Definition: GeometryIndexer.h:141
int32_t index_t
Definition: NanoFlannImpl.h:43
const Dtype Float64
Definition: Dtype.cpp:62
const int64_t MAX_RESOLUTION_DIMS
Definition: GeometryIndexer.h:177
Definition: PinholeCameraIntrinsic.cpp:35