Open3D (C++ API)  0.18.0
GeometryIndexer.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 <unordered_map>
11 
12 #include "open3d/core/CUDAUtils.h"
13 #include "open3d/core/Tensor.h"
15 #include "open3d/utility/Helper.h"
16 #include "open3d/utility/Logging.h"
17 #include "open3d/utility/Timer.h"
18 
19 namespace open3d {
20 namespace t {
21 namespace geometry {
22 namespace kernel {
23 
26 public:
29  TransformIndexer(const core::Tensor& intrinsics,
30  const core::Tensor& extrinsics,
31  float scale = 1.0f) {
32  core::AssertTensorShape(intrinsics, {3, 3});
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});
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 
100  OPEN3D_HOST_DEVICE void Project(float x_in,
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 
111  OPEN3D_HOST_DEVICE void Unproject(float u_in,
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 
135 private:
136  float extrinsic_[3][4];
137 
138  float fx_;
139  float fy_;
140  float cx_;
141  float cy_;
142 
143  float scale_;
144 };
145 
158 const int64_t MAX_RESOLUTION_DIMS = 4;
159 
160 template <typename index_t>
162 public:
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()) {
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) {
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) {
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 
238  index_t y_in,
239  index_t* workload) const {
240  *workload = y_in * shape_[1] + x_in;
241  }
242 
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 
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 
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 
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 
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>
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>
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>
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 
352 private:
353  void* ptr_;
354  index_t element_byte_size_;
355  index_t active_dims_;
356 
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
#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
int64_t ByteSize() const
Definition: Dtype.h:58
Definition: SizeVector.h:69
size_t size() const
Definition: SmallVector.h:119
Definition: Tensor.h:32
T * GetDataPtr()
Definition: Tensor.h:1143
SizeVector GetShape() const
Definition: Tensor.h:1126
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 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 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 T * GetDataPtr(index_t x, index_t y, index_t z, index_t t) const
Definition: GeometryIndexer.h:342
OPEN3D_HOST_DEVICE void * GetDataPtr() const
Definition: GeometryIndexer.h:315
OPEN3D_HOST_DEVICE index_t ElementByteSize()
Definition: GeometryIndexer.h:226
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) 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 index_t GetShape(int i) const
Definition: GeometryIndexer.h:311
OPEN3D_HOST_DEVICE index_t NumElements()
Definition: GeometryIndexer.h:228
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x, index_t y, index_t z) const
Definition: GeometryIndexer.h:332
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z, float t) const
Definition: GeometryIndexer.h:302
OPEN3D_HOST_DEVICE T * GetDataPtr(index_t x) const
Definition: GeometryIndexer.h:318
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
int index_t
Definition: VoxelBlockGrid.h:22
const int64_t MAX_RESOLUTION_DIMS
Definition: GeometryIndexer.h:158
Definition: PinholeCameraIntrinsic.cpp:16