Open3D (C++ API)  0.19.0
Loading...
Searching...
No Matches
GeometryIndexer.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// Copyright (c) 2018-2024 www.open3d.org
5// SPDX-License-Identifier: MIT
6// ----------------------------------------------------------------------------
7
8#pragma once
9
10#include <unordered_map>
11
13#include "open3d/core/Tensor.h"
18
19namespace open3d {
20namespace t {
21namespace geometry {
22namespace kernel {
23
26public:
29 TransformIndexer(const core::Tensor& intrinsics,
30 const core::Tensor& extrinsics,
31 float scale = 1.0f) {
32 core::AssertTensorShape(intrinsics, {3, 3});
33 core::AssertTensorDtype(intrinsics, core::Float64);
34 core::AssertTensorDevice(intrinsics, core::Device("CPU:0"));
35 if (!intrinsics.IsContiguous()) {
36 utility::LogError("Intrinsics is not contiguous");
37 }
38
39 core::AssertTensorShape(extrinsics, {4, 4});
40 core::AssertTensorDtype(extrinsics, core::Float64);
41 core::AssertTensorDevice(extrinsics, core::Device("CPU:0"));
42 if (!extrinsics.IsContiguous()) {
43 utility::LogError("Extrinsics is not contiguous");
44 }
45
46 const double* intrinsic_ptr = intrinsics.GetDataPtr<double>();
47 const double* extrinsic_ptr = extrinsics.GetDataPtr<double>();
48 for (int i = 0; i < 3; ++i) {
49 for (int j = 0; j < 4; ++j) {
50 extrinsic_[i][j] = extrinsic_ptr[i * 4 + j];
51 }
52 }
53
54 fx_ = intrinsic_ptr[0 * 3 + 0];
55 fy_ = intrinsic_ptr[1 * 3 + 1];
56 cx_ = intrinsic_ptr[0 * 3 + 2];
57 cy_ = intrinsic_ptr[1 * 3 + 2];
58 scale_ = scale;
59 }
60
63 float y_in,
64 float z_in,
65 float* x_out,
66 float* y_out,
67 float* z_out) const {
68 x_in *= scale_;
69 y_in *= scale_;
70 z_in *= scale_;
71
72 *x_out = x_in * extrinsic_[0][0] + y_in * extrinsic_[0][1] +
73 z_in * extrinsic_[0][2] + extrinsic_[0][3];
74 *y_out = x_in * extrinsic_[1][0] + y_in * extrinsic_[1][1] +
75 z_in * extrinsic_[1][2] + extrinsic_[1][3];
76 *z_out = x_in * extrinsic_[2][0] + y_in * extrinsic_[2][1] +
77 z_in * extrinsic_[2][2] + extrinsic_[2][3];
78 }
79
81 OPEN3D_HOST_DEVICE void Rotate(float x_in,
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];
93 *y_out = x_in * extrinsic_[1][0] + y_in * extrinsic_[1][1] +
94 z_in * extrinsic_[1][2];
95 *z_out = x_in * extrinsic_[2][0] + y_in * extrinsic_[2][1] +
96 z_in * extrinsic_[2][2];
97 }
98
101 float y_in,
102 float z_in,
103 float* u_out,
104 float* v_out) const {
105 float inv_z = 1.0f / z_in;
106 *u_out = fx_ * x_in * inv_z + cx_;
107 *v_out = fy_ * y_in * inv_z + cy_;
108 }
109
112 float v_in,
113 float d_in,
114 float* x_out,
115 float* y_out,
116 float* z_out) const {
117 *x_out = (u_in - cx_) * d_in / fx_;
118 *y_out = (v_in - cy_) * d_in / fy_;
119 *z_out = d_in;
120 }
121
122 OPEN3D_HOST_DEVICE void GetFocalLength(float* fx, float* fy) const {
123 *fx = fx_;
124 *fy = fy_;
125 }
126
128 float* y,
129 float* z) const {
130 *x = extrinsic_[0][3];
131 *y = extrinsic_[1][3];
132 *z = extrinsic_[2][3];
133 }
134
135private:
136 float extrinsic_[3][4];
137
138 float fx_;
139 float fy_;
140 float cx_;
141 float cy_;
142
143 float scale_;
144};
145
158const int64_t MAX_RESOLUTION_DIMS = 4;
159
160template <typename index_t>
162public:
163 TArrayIndexer() : ptr_(nullptr), element_byte_size_(0), active_dims_(0) {
164 for (index_t i = 0; i < MAX_RESOLUTION_DIMS; ++i) {
165 shape_[i] = 0;
166 }
167 }
168
169 TArrayIndexer(const core::Tensor& ndarray, index_t active_dims) {
170 if (!ndarray.IsContiguous()) {
171 utility::LogError(
172 "Only support contiguous tensors for general operations.");
173 }
174
175 core::SizeVector shape = ndarray.GetShape();
176 index_t n = ndarray.NumDims();
177 if (active_dims > MAX_RESOLUTION_DIMS || active_dims > n) {
178 utility::LogError(
179 "Tensor shape too large, only <= {} and <= {} array dim is "
180 "supported, but received {}.",
181 MAX_RESOLUTION_DIMS, n, active_dims);
182 }
183
184 // Leading dimensions are coordinates
185 active_dims_ = active_dims;
186 for (index_t i = 0; i < active_dims_; ++i) {
187 shape_[i] = shape[i];
188 }
189 // Trailing dimensions are channels
190 element_byte_size_ = ndarray.GetDtype().ByteSize();
191 for (index_t i = active_dims_; i < n; ++i) {
192 element_byte_size_ *= shape[i];
193 }
194
195 // Fill-in rest to make compiler happy, not actually used.
196 for (index_t i = active_dims_; i < MAX_RESOLUTION_DIMS; ++i) {
197 shape_[i] = 0;
198 }
199 ptr_ = const_cast<void*>(ndarray.GetDataPtr());
200 }
201
204 index_t n = static_cast<index_t>(shape.size());
205 if (n > MAX_RESOLUTION_DIMS) {
206 utility::LogError(
207 "SizeVector too large, only <= {} is supported, but "
208 "received {}.",
210 }
211 active_dims_ = n;
212 for (index_t i = 0; i < active_dims_; ++i) {
213 shape_[i] = shape[i];
214 }
215
216 // Fill-in rest to make compiler happy, not actually used.
217 for (index_t i = active_dims_; i < MAX_RESOLUTION_DIMS; ++i) {
218 shape_[i] = 0;
219 }
220
221 // Reserved
222 element_byte_size_ = 0;
223 ptr_ = nullptr;
224 }
225
226 OPEN3D_HOST_DEVICE index_t ElementByteSize() { return element_byte_size_; }
227
229 index_t num_elems = 1;
230 for (index_t i = 0; i < active_dims_; ++i) {
231 num_elems *= shape_[i];
232 }
233 return num_elems;
234 }
235
237 inline OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in,
238 index_t y_in,
239 index_t* workload) const {
240 *workload = y_in * shape_[1] + x_in;
241 }
242
244 inline OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in,
245 index_t y_in,
246 index_t z_in,
247 index_t* workload) const {
248 *workload = (z_in * shape_[1] + y_in) * shape_[2] + x_in;
249 }
250
252 inline OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in,
253 index_t y_in,
254 index_t z_in,
255 index_t t_in,
256 index_t* workload) const {
257 *workload = ((t_in * shape_[1] + z_in) * shape_[2] + y_in) * shape_[3] +
258 x_in;
259 }
260
262 inline OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload,
263 index_t* x_out,
264 index_t* y_out) const {
265 *x_out = workload % shape_[1];
266 *y_out = workload / shape_[1];
267 }
268
270 inline OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload,
271 index_t* x_out,
272 index_t* y_out,
273 index_t* z_out) const {
274 *x_out = workload % shape_[2];
275 workload = (workload - *x_out) / shape_[2];
276 *y_out = workload % shape_[1];
277 *z_out = workload / shape_[1];
278 }
279
281 inline OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload,
282 index_t* x_out,
283 index_t* y_out,
284 index_t* z_out,
285 index_t* t_out) const {
286 *x_out = workload % shape_[3];
287 workload = (workload - *x_out) / shape_[3];
288 *y_out = workload % shape_[2];
289 workload = (workload - *y_out) / shape_[2];
290 *z_out = workload % shape_[1];
291 *t_out = workload / shape_[1];
292 }
293
294 inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const {
295 return y >= 0 && x >= 0 && y <= shape_[0] - 1.0f &&
296 x <= shape_[1] - 1.0f;
297 }
298 inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const {
299 return z >= 0 && y >= 0 && x >= 0 && z <= shape_[0] - 1.0f &&
300 y <= shape_[1] - 1.0f && x <= shape_[2] - 1.0f;
301 }
302 inline OPEN3D_HOST_DEVICE bool InBoundary(float x,
303 float y,
304 float z,
305 float t) const {
306 return t >= 0 && z >= 0 && y >= 0 && x >= 0 && t <= shape_[0] - 1.0f &&
307 z <= shape_[1] - 1.0f && y <= shape_[2] - 1.0f &&
308 x <= shape_[3] - 1.0f;
309 }
310
311 inline OPEN3D_HOST_DEVICE index_t GetShape(int i) const {
312 return shape_[i];
313 }
314
315 inline OPEN3D_HOST_DEVICE void* GetDataPtr() const { return ptr_; }
316
317 template <typename T>
318 inline OPEN3D_HOST_DEVICE T* GetDataPtr(index_t x) const {
319 return static_cast<T*>(static_cast<void*>(static_cast<uint8_t*>(ptr_) +
320 x * element_byte_size_));
321 }
322
323 template <typename T>
324 inline OPEN3D_HOST_DEVICE T* GetDataPtr(index_t x, index_t y) const {
325 index_t workload;
326 CoordToWorkload(x, y, &workload);
327 return static_cast<T*>(static_cast<void*>(
328 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
329 }
330
331 template <typename T>
332 inline OPEN3D_HOST_DEVICE T* GetDataPtr(index_t x,
333 index_t y,
334 index_t z) const {
335 index_t workload;
336 CoordToWorkload(x, y, z, &workload);
337 return static_cast<T*>(static_cast<void*>(
338 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
339 }
340
341 template <typename T>
342 inline OPEN3D_HOST_DEVICE T* GetDataPtr(index_t x,
343 index_t y,
344 index_t z,
345 index_t t) const {
346 index_t workload;
347 CoordToWorkload(x, y, z, t, &workload);
348 return static_cast<T*>(static_cast<void*>(
349 static_cast<uint8_t*>(ptr_) + workload * element_byte_size_));
350 }
351
352private:
353 void* ptr_;
354 index_t element_byte_size_;
355 index_t active_dims_;
356
357 index_t shape_[MAX_RESOLUTION_DIMS];
358};
359
361
362} // namespace kernel
363} // namespace geometry
364} // namespace t
365} // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition CUDAUtils.h:44
double t
Definition SurfaceReconstructionPoisson.cpp:172
Definition Device.h:18
int64_t ByteSize() const
Definition Dtype.h:58
Definition SizeVector.h:69
size_t size() const
Definition SmallVector.h:119
Definition Tensor.h:32
SizeVector GetShape() const
Definition Tensor.h:1126
T * GetDataPtr()
Definition Tensor.h:1143
int64_t NumDims() const
Definition Tensor.h:1171
bool IsContiguous() const
Definition Tensor.h:1035
Dtype GetDtype() const
Definition Tensor.h:1163
Definition GeometryIndexer.h:161
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:281
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const
Definition GeometryIndexer.h:294
TArrayIndexer()
Definition GeometryIndexer.h:163
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y, index_t z, index_t t) const
Definition GeometryIndexer.h:342
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:252
TArrayIndexer(const core::Tensor &ndarray, index_t active_dims)
Definition GeometryIndexer.h:169
OPEN3D_HOST_DEVICE void * GetDataPtr() const
Definition GeometryIndexer.h:315
OPEN3D_HOST_DEVICE void CoordToWorkload(index_t x_in, index_t y_in, index_t *workload) const
2D coordinate => workload
Definition GeometryIndexer.h:237
OPEN3D_HOST_DEVICE void WorkloadToCoord(index_t workload, index_t *x_out, index_t *y_out) const
Workload => 2D coordinate.
Definition GeometryIndexer.h:262
OPEN3D_HOST_DEVICE index_t ElementByteSize()
Definition GeometryIndexer.h:226
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const
Definition GeometryIndexer.h:298
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:244
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x) const
Definition GeometryIndexer.h:318
OPEN3D_HOST_DEVICE index_t GetShape(int i) const
Definition GeometryIndexer.h:311
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y, index_t z) const
Definition GeometryIndexer.h:332
OPEN3D_HOST_DEVICE index_t NumElements()
Definition GeometryIndexer.h:228
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y) const
Definition GeometryIndexer.h:324
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z, float t) const
Definition GeometryIndexer.h:302
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:270
TArrayIndexer(const core::SizeVector &shape)
Only used for simple shapes.
Definition GeometryIndexer.h:203
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition GeometryIndexer.h:25
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:100
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:81
TransformIndexer(const core::Tensor &intrinsics, const core::Tensor &extrinsics, float scale=1.0f)
Definition GeometryIndexer.h:29
OPEN3D_HOST_DEVICE void GetCameraPosition(float *x, float *y, float *z) const
Definition GeometryIndexer.h:127
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:62
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:111
OPEN3D_HOST_DEVICE void GetFocalLength(float *fx, float *fy) const
Definition GeometryIndexer.h:122
const Dtype Float64
Definition Dtype.cpp:43
const int64_t MAX_RESOLUTION_DIMS
Definition GeometryIndexer.h:158
Definition PinholeCameraIntrinsic.cpp:16