Open3D (C++ API)  0.12.0
GeneralIndexer.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 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 
31 #include "open3d/core/Tensor.h"
32 #include "open3d/utility/Console.h"
33 #include "open3d/utility/Helper.h"
34 
35 namespace open3d {
36 namespace core {
37 namespace kernel {
38 
41 public:
44  TransformIndexer(const Tensor& intrinsic,
45  const Tensor& extrinsic = Tensor::Eye(
46  4, core::Dtype::Float32, core::Device("CPU:0")),
47  float scale = 1.0f) {
48  intrinsic.AssertShape({3, 3});
49  extrinsic.AssertShape({4, 4});
51  extrinsic.AssertDtype(core::Dtype::Float32);
52 
53  for (int i = 0; i < 3; ++i) {
54  for (int j = 0; j < 4; ++j) {
55  extrinsic_[i][j] = extrinsic[i][j].Item<float>();
56  }
57  }
58 
59  fx_ = intrinsic[0][0].Item<float>();
60  fy_ = intrinsic[1][1].Item<float>();
61  cx_ = intrinsic[0][2].Item<float>();
62  cy_ = intrinsic[1][2].Item<float>();
63 
64  scale_ = scale;
65  }
66 
69  float y_in,
70  float z_in,
71  float* x_out,
72  float* y_out,
73  float* z_out) const {
74  x_in *= scale_;
75  y_in *= scale_;
76  z_in *= scale_;
77 
78  *x_out = x_in * extrinsic_[0][0] + y_in * extrinsic_[0][1] +
79  z_in * extrinsic_[0][2] + extrinsic_[0][3];
80  *y_out = x_in * extrinsic_[1][0] + y_in * extrinsic_[1][1] +
81  z_in * extrinsic_[1][2] + extrinsic_[1][3];
82  *z_out = x_in * extrinsic_[2][0] + y_in * extrinsic_[2][1] +
83  z_in * extrinsic_[2][2] + extrinsic_[2][3];
84  }
85 
87  OPEN3D_HOST_DEVICE void Project(float x_in,
88  float y_in,
89  float z_in,
90  float* u_out,
91  float* v_out) const {
92  float inv_z = 1.0f / z_in;
93  *u_out = fx_ * x_in * inv_z + cx_;
94  *v_out = fy_ * y_in * inv_z + cy_;
95  }
96 
98  OPEN3D_HOST_DEVICE void Unproject(float u_in,
99  float v_in,
100  float d_in,
101  float* x_out,
102  float* y_out,
103  float* z_out) const {
104  *x_out = (u_in - cx_) * d_in / fx_;
105  *y_out = (v_in - cy_) * d_in / fy_;
106  *z_out = d_in;
107  }
108 
109 private:
110  float extrinsic_[3][4];
111 
112  float fx_;
113  float fy_;
114  float cx_;
115  float cy_;
116 
117  float scale_;
118 };
119 
132 const int64_t MAX_RESOLUTION_DIMS = 4;
134 public:
135  NDArrayIndexer() : ptr_(nullptr), element_byte_size_(0), active_dims_(0) {
136  for (int i = 0; i < MAX_RESOLUTION_DIMS; ++i) {
137  shape_[i] = 0;
138  }
139  }
140 
141  NDArrayIndexer(const Tensor& ndarray, int64_t active_dims) {
142  if (!ndarray.IsContiguous()) {
144  "[NDArrayIndexer] Only support contiguous tensors for "
145  "general operations.");
146  }
147 
148  SizeVector shape = ndarray.GetShape();
149  int64_t n = ndarray.NumDims();
150  if (active_dims > MAX_RESOLUTION_DIMS || active_dims > n) {
152  "[NDArrayIndexer] Tensor shape too large, only <= {} is "
153  "supported, but received {}.",
154  MAX_RESOLUTION_DIMS, active_dims);
155  }
156 
157  // Leading dimensions are coordinates
158  active_dims_ = active_dims;
159  for (int64_t i = 0; i < active_dims_; ++i) {
160  shape_[i] = shape[i];
161  }
162  // Trailing dimensions are channels
163  element_byte_size_ = ndarray.GetDtype().ByteSize();
164  for (int64_t i = active_dims_; i < n; ++i) {
165  element_byte_size_ *= shape[i];
166  }
167  ptr_ = const_cast<void*>(ndarray.GetDataPtr());
168  }
169 
171  NDArrayIndexer(const SizeVector& shape) {
172  int64_t n = static_cast<int64_t>(shape.size());
173  if (n > MAX_RESOLUTION_DIMS) {
175  "[NDArrayIndexer] SizeVector too large, only <= {} is "
176  "supported, but received {}.",
177  MAX_RESOLUTION_DIMS, n);
178  }
179  active_dims_ = n;
180  for (int64_t i = 0; i < active_dims_; ++i) {
181  shape_[i] = shape[i];
182  }
183 
184  // Reserved
185  element_byte_size_ = 0;
186  ptr_ = nullptr;
187  }
188 
189  OPEN3D_HOST_DEVICE int64_t ElementByteSize() { return element_byte_size_; }
190 
192  int64_t num_elems = 1;
193  for (int64_t i = 0; i < active_dims_; ++i) {
194  num_elems *= shape_[i];
195  }
196  return num_elems;
197  }
198 
200  inline OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in,
201  int64_t y_in,
202  int64_t* workload) const {
203  *workload = y_in * shape_[1] + x_in;
204  }
205 
207  inline OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in,
208  int64_t y_in,
209  int64_t z_in,
210  int64_t* workload) const {
211  *workload = (z_in * shape_[1] + y_in) * shape_[2] + x_in;
212  }
213 
215  inline OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in,
216  int64_t y_in,
217  int64_t z_in,
218  int64_t t_in,
219  int64_t* workload) const {
220  *workload = ((t_in * shape_[1] + z_in) * shape_[2] + y_in) * shape_[3] +
221  x_in;
222  }
223 
225  inline OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload,
226  int64_t* x_out,
227  int64_t* y_out) const {
228  *x_out = workload % shape_[1];
229  *y_out = workload / shape_[1];
230  }
231 
233  inline OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload,
234  int64_t* x_out,
235  int64_t* y_out,
236  int64_t* z_out) const {
237  *x_out = workload % shape_[2];
238  workload = (workload - *x_out) / shape_[2];
239  *y_out = workload % shape_[1];
240  *z_out = workload / shape_[1];
241  }
242 
244  inline OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload,
245  int64_t* x_out,
246  int64_t* y_out,
247  int64_t* z_out,
248  int64_t* t_out) const {
249  *x_out = workload % shape_[3];
250  workload = (workload - *x_out) / shape_[3];
251  *y_out = workload % shape_[2];
252  workload = (workload - *y_out) / shape_[2];
253  *z_out = workload % shape_[1];
254  *t_out = workload / shape_[1];
255  }
256 
257  inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const {
258  return y >= 0 && x >= 0 && y <= shape_[0] - 1.0f &&
259  x <= shape_[1] - 1.0f;
260  }
261  inline OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const {
262  return z >= 0 && y >= 0 && x >= 0 && z <= shape_[0] - 1.0f &&
263  y <= shape_[1] - 1.0f && x <= shape_[2] - 1.0f;
264  }
265  inline OPEN3D_HOST_DEVICE bool InBoundary(float x,
266  float y,
267  float z,
268  float t) const {
269  return t >= 0 && z >= 0 && y >= 0 && x >= 0 && t <= shape_[0] - 1.0f &&
270  z <= shape_[1] - 1.0f && y <= shape_[2] - 1.0f &&
271  x <= shape_[3] - 1.0f;
272  }
273 
274  inline OPEN3D_HOST_DEVICE int64_t GetShape(int i) const {
275  return shape_[i];
276  }
277 
278  inline OPEN3D_HOST_DEVICE void* GetDataPtrFromCoord(int64_t x) const {
279  return static_cast<void*>(static_cast<uint8_t*>(ptr_) +
280  x * element_byte_size_);
281  }
282 
283  inline OPEN3D_HOST_DEVICE void* GetDataPtrFromCoord(int64_t x,
284  int64_t y) const {
285  int64_t workload;
286  CoordToWorkload(x, y, &workload);
287  return static_cast<void*>(static_cast<uint8_t*>(ptr_) +
288  workload * element_byte_size_);
289  }
290  inline OPEN3D_HOST_DEVICE void* GetDataPtrFromCoord(int64_t x,
291  int64_t y,
292  int64_t z) const {
293  int64_t workload;
294  CoordToWorkload(x, y, z, &workload);
295  return static_cast<void*>(static_cast<uint8_t*>(ptr_) +
296  workload * element_byte_size_);
297  }
298  inline OPEN3D_HOST_DEVICE void* GetDataPtrFromCoord(int64_t x,
299  int64_t y,
300  int64_t z,
301  int64_t t) const {
302  int64_t workload;
303  CoordToWorkload(x, y, z, t, &workload);
304  return static_cast<void*>(static_cast<uint8_t*>(ptr_) +
305  workload * element_byte_size_);
306  }
307 
308 private:
309  void* ptr_;
310  int64_t element_byte_size_;
311  int64_t active_dims_;
312 
313  int64_t shape_[MAX_RESOLUTION_DIMS];
314 };
315 
316 } // namespace kernel
317 } // namespace core
318 } // namespace open3d
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z) const
Definition: GeneralIndexer.h:261
OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in, int64_t y_in, int64_t *workload) const
2D coordinate => workload
Definition: GeneralIndexer.h:200
NDArrayIndexer()
Definition: GeneralIndexer.h:135
int64_t NumDims() const
Definition: Tensor.h:973
void AssertDtype(const Dtype &expected_dtype) const
Assert that the Tensor has the specified dtype.
Definition: Tensor.cpp:1268
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: GeneralIndexer.h:87
OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload, int64_t *x_out, int64_t *y_out, int64_t *z_out, int64_t *t_out) const
Workload => 4D coordinate.
Definition: GeneralIndexer.h:244
void * GetDataPtr()
Definition: Tensor.h:961
void LogError(const char *format, const Args &... args)
Definition: Console.h:176
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: GeneralIndexer.h:68
NDArrayIndexer(const SizeVector &shape)
Only used for simple shapes.
Definition: GeneralIndexer.h:171
TransformIndexer(const Tensor &intrinsic, const Tensor &extrinsic=Tensor::Eye(4, core::Dtype::Float32, core::Device("CPU:0")), float scale=1.0f)
Definition: GeneralIndexer.h:44
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y, float z, float t) const
Definition: GeneralIndexer.h:265
Definition: GeneralIndexer.h:133
Definition: SizeVector.h:102
OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload, int64_t *x_out, int64_t *y_out) const
Workload => 2D coordinate.
Definition: GeneralIndexer.h:225
Dtype GetDtype() const
Definition: Tensor.h:965
OPEN3D_HOST_DEVICE void * GetDataPtrFromCoord(int64_t x) const
Definition: GeneralIndexer.h:278
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:54
OPEN3D_HOST_DEVICE void * GetDataPtrFromCoord(int64_t x, int64_t y, int64_t z) const
Definition: GeneralIndexer.h:290
Definition: Device.h:39
bool IsContiguous() const
Definition: Tensor.h:912
OPEN3D_HOST_DEVICE int64_t ElementByteSize()
Definition: GeneralIndexer.h:189
OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in, int64_t y_in, int64_t z_in, int64_t *workload) const
3D coordinate => workload
Definition: GeneralIndexer.h:207
static const Dtype Float32
Definition: Dtype.h:42
OPEN3D_HOST_DEVICE int64_t GetShape(int i) const
Definition: GeneralIndexer.h:274
OPEN3D_HOST_DEVICE void * GetDataPtrFromCoord(int64_t x, int64_t y) const
Definition: GeneralIndexer.h:283
NDArrayIndexer(const Tensor &ndarray, int64_t active_dims)
Definition: GeneralIndexer.h:141
SizeVector GetShape() const
Definition: Tensor.h:945
OPEN3D_HOST_DEVICE int64_t NumElements()
Definition: GeneralIndexer.h:191
Definition: PinholeCameraIntrinsic.cpp:35
Definition: Tensor.h:48
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition: GeneralIndexer.h:40
OPEN3D_HOST_DEVICE void CoordToWorkload(int64_t x_in, int64_t y_in, int64_t z_in, int64_t t_in, int64_t *workload) const
4D coordinate => workload
Definition: GeneralIndexer.h:215
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const
Definition: GeneralIndexer.h:257
int64_t ByteSize() const
Definition: Dtype.h:71
OPEN3D_HOST_DEVICE void WorkloadToCoord(int64_t workload, int64_t *x_out, int64_t *y_out, int64_t *z_out) const
Workload => 3D coordinate.
Definition: GeneralIndexer.h:233
const int64_t MAX_RESOLUTION_DIMS
Definition: GeneralIndexer.h:132
static Tensor Eye(int64_t n, Dtype dtype, const Device &device)
Create a identity matrix of size n x n.
Definition: Tensor.cpp:194
void AssertShape(const SizeVector &expected_shape) const
Assert that the Tensor has the specified shape.
Definition: Tensor.cpp:1249
T Item() const
Definition: Tensor.h:446
OPEN3D_HOST_DEVICE void * GetDataPtrFromCoord(int64_t x, int64_t y, int64_t z, int64_t t) const
Definition: GeneralIndexer.h:298
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: GeneralIndexer.h:98