Open3D (C++ API)  0.18.0+252c867
RegistrationImpl.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 // Private header. Do not include in Open3d.h.
9 
10 #pragma once
11 
12 #include <cmath>
13 
14 #include "open3d/core/CUDAUtils.h"
15 #include "open3d/core/Tensor.h"
19 
20 #ifndef __CUDACC__
21 using std::abs;
22 #endif
23 
24 namespace open3d {
25 namespace t {
26 namespace pipelines {
27 namespace kernel {
28 
29 void ComputePosePointToPlaneCPU(const core::Tensor &source_points,
30  const core::Tensor &target_points,
31  const core::Tensor &target_normals,
32  const core::Tensor &correspondence_indices,
33  core::Tensor &pose,
34  float &residual,
35  int &inlier_count,
36  const core::Dtype &dtype,
37  const core::Device &device,
38  const registration::RobustKernel &kernel);
39 
40 void ComputePoseColoredICPCPU(const core::Tensor &source_points,
41  const core::Tensor &source_colors,
42  const core::Tensor &target_points,
43  const core::Tensor &target_normals,
44  const core::Tensor &target_colors,
45  const core::Tensor &target_color_gradients,
46  const core::Tensor &correspondence_indices,
47  core::Tensor &pose,
48  float &residual,
49  int &inlier_count,
50  const core::Dtype &dtype,
51  const core::Device &device,
52  const registration::RobustKernel &kernel,
53  const double &lambda_geometric);
54 
56  const core::Tensor &source_points,
57  const core::Tensor &source_dopplers,
58  const core::Tensor &source_directions,
59  const core::Tensor &target_points,
60  const core::Tensor &target_normals,
61  const core::Tensor &correspondence_indices,
62  core::Tensor &output_pose,
63  float &residual,
64  int &inlier_count,
65  const core::Dtype &dtype,
66  const core::Device &device,
67  const core::Tensor &R_S_to_V,
68  const core::Tensor &r_v_to_s_in_V,
69  const core::Tensor &w_v_in_V,
70  const core::Tensor &v_v_in_V,
71  const double period,
72  const bool reject_dynamic_outliers,
73  const double doppler_outlier_threshold,
74  const registration::RobustKernel &kernel_geometric,
75  const registration::RobustKernel &kernel_doppler,
76  const double lambda_doppler);
77 
78 #ifdef BUILD_CUDA_MODULE
79 void ComputePosePointToPlaneCUDA(const core::Tensor &source_points,
80  const core::Tensor &target_points,
81  const core::Tensor &target_normals,
82  const core::Tensor &correspondence_indices,
83  core::Tensor &pose,
84  float &residual,
85  int &inlier_count,
86  const core::Dtype &dtype,
87  const core::Device &device,
88  const registration::RobustKernel &kernel);
89 
90 void ComputePoseColoredICPCUDA(const core::Tensor &source_points,
91  const core::Tensor &source_colors,
92  const core::Tensor &target_points,
93  const core::Tensor &target_normals,
94  const core::Tensor &target_colors,
95  const core::Tensor &target_color_gradients,
96  const core::Tensor &correspondence_indices,
97  core::Tensor &pose,
98  float &residual,
99  int &inlier_count,
100  const core::Dtype &dtype,
101  const core::Device &device,
102  const registration::RobustKernel &kernel,
103  const double &lambda_geometric);
104 
105 void ComputePoseDopplerICPCUDA(
106  const core::Tensor &source_points,
107  const core::Tensor &source_dopplers,
108  const core::Tensor &source_directions,
109  const core::Tensor &target_points,
110  const core::Tensor &target_normals,
111  const core::Tensor &correspondence_indices,
112  core::Tensor &output_pose,
113  float &residual,
114  int &inlier_count,
115  const core::Dtype &dtype,
116  const core::Device &device,
117  const core::Tensor &R_S_to_V,
118  const core::Tensor &r_v_to_s_in_V,
119  const core::Tensor &w_v_in_V,
120  const core::Tensor &v_v_in_V,
121  const double period,
122  const bool reject_dynamic_outliers,
123  const double doppler_outlier_threshold,
124  const registration::RobustKernel &kernel_geometric,
125  const registration::RobustKernel &kernel_doppler,
126  const double lambda_doppler);
127 #endif
128 
129 void ComputeRtPointToPointCPU(const core::Tensor &source_points,
130  const core::Tensor &target_points,
131  const core::Tensor &correspondence_indices,
132  core::Tensor &R,
133  core::Tensor &t,
134  int &inlier_count,
135  const core::Dtype &dtype,
136  const core::Device &device);
137 
138 void ComputeInformationMatrixCPU(const core::Tensor &target_points,
139  const core::Tensor &correspondence_indices,
140  core::Tensor &information_matrix,
141  const core::Dtype &dtype,
142  const core::Device &device);
143 
144 #ifdef BUILD_CUDA_MODULE
145 void ComputeInformationMatrixCUDA(const core::Tensor &target_points,
146  const core::Tensor &correspondence_indices,
147  core::Tensor &information_matrix,
148  const core::Dtype &dtype,
149  const core::Device &device);
150 #endif
151 
152 template <typename scalar_t>
154  int64_t workload_idx,
155  const scalar_t *source_points_ptr,
156  const scalar_t *target_points_ptr,
157  const scalar_t *target_normals_ptr,
158  const int64_t *correspondence_indices,
159  scalar_t *J_ij,
160  scalar_t &r) {
161  if (correspondence_indices[workload_idx] == -1) {
162  return false;
163  }
164 
165  const int64_t target_idx = 3 * correspondence_indices[workload_idx];
166  const int64_t source_idx = 3 * workload_idx;
167 
168  const scalar_t &sx = source_points_ptr[source_idx + 0];
169  const scalar_t &sy = source_points_ptr[source_idx + 1];
170  const scalar_t &sz = source_points_ptr[source_idx + 2];
171  const scalar_t &tx = target_points_ptr[target_idx + 0];
172  const scalar_t &ty = target_points_ptr[target_idx + 1];
173  const scalar_t &tz = target_points_ptr[target_idx + 2];
174  const scalar_t &nx = target_normals_ptr[target_idx + 0];
175  const scalar_t &ny = target_normals_ptr[target_idx + 1];
176  const scalar_t &nz = target_normals_ptr[target_idx + 2];
177 
178  r = (sx - tx) * nx + (sy - ty) * ny + (sz - tz) * nz;
179 
180  J_ij[0] = nz * sy - ny * sz;
181  J_ij[1] = nx * sz - nz * sx;
182  J_ij[2] = ny * sx - nx * sy;
183  J_ij[3] = nx;
184  J_ij[4] = ny;
185  J_ij[5] = nz;
186 
187  return true;
188 }
189 
190 template bool GetJacobianPointToPlane(int64_t workload_idx,
191  const float *source_points_ptr,
192  const float *target_points_ptr,
193  const float *target_normals_ptr,
194  const int64_t *correspondence_indices,
195  float *J_ij,
196  float &r);
197 
198 template bool GetJacobianPointToPlane(int64_t workload_idx,
199  const double *source_points_ptr,
200  const double *target_points_ptr,
201  const double *target_normals_ptr,
202  const int64_t *correspondence_indices,
203  double *J_ij,
204  double &r);
205 
206 template <typename scalar_t>
208  const int64_t workload_idx,
209  const scalar_t *source_points_ptr,
210  const scalar_t *source_colors_ptr,
211  const scalar_t *target_points_ptr,
212  const scalar_t *target_normals_ptr,
213  const scalar_t *target_colors_ptr,
214  const scalar_t *target_color_gradients_ptr,
215  const int64_t *correspondence_indices,
216  const scalar_t &sqrt_lambda_geometric,
217  const scalar_t &sqrt_lambda_photometric,
218  scalar_t *J_G,
219  scalar_t *J_I,
220  scalar_t &r_G,
221  scalar_t &r_I) {
222  if (correspondence_indices[workload_idx] == -1) {
223  return false;
224  }
225 
226  const int64_t target_idx = 3 * correspondence_indices[workload_idx];
227  const int64_t source_idx = 3 * workload_idx;
228 
229  const scalar_t vs[3] = {source_points_ptr[source_idx],
230  source_points_ptr[source_idx + 1],
231  source_points_ptr[source_idx + 2]};
232 
233  const scalar_t vt[3] = {target_points_ptr[target_idx],
234  target_points_ptr[target_idx + 1],
235  target_points_ptr[target_idx + 2]};
236 
237  const scalar_t nt[3] = {target_normals_ptr[target_idx],
238  target_normals_ptr[target_idx + 1],
239  target_normals_ptr[target_idx + 2]};
240 
241  const scalar_t d = (vs[0] - vt[0]) * nt[0] + (vs[1] - vt[1]) * nt[1] +
242  (vs[2] - vt[2]) * nt[2];
243 
244  J_G[0] = sqrt_lambda_geometric * (-vs[2] * nt[1] + vs[1] * nt[2]);
245  J_G[1] = sqrt_lambda_geometric * (vs[2] * nt[0] - vs[0] * nt[2]);
246  J_G[2] = sqrt_lambda_geometric * (-vs[1] * nt[0] + vs[0] * nt[1]);
247  J_G[3] = sqrt_lambda_geometric * nt[0];
248  J_G[4] = sqrt_lambda_geometric * nt[1];
249  J_G[5] = sqrt_lambda_geometric * nt[2];
250  r_G = sqrt_lambda_geometric * d;
251 
252  const scalar_t vs_proj[3] = {vs[0] - d * nt[0], vs[1] - d * nt[1],
253  vs[2] - d * nt[2]};
254 
255  const scalar_t intensity_source =
256  (source_colors_ptr[source_idx] + source_colors_ptr[source_idx + 1] +
257  source_colors_ptr[source_idx + 2]) /
258  3.0;
259 
260  const scalar_t intensity_target =
261  (target_colors_ptr[target_idx] + target_colors_ptr[target_idx + 1] +
262  target_colors_ptr[target_idx + 2]) /
263  3.0;
264 
265  const scalar_t dit[3] = {target_color_gradients_ptr[target_idx],
266  target_color_gradients_ptr[target_idx + 1],
267  target_color_gradients_ptr[target_idx + 2]};
268 
269  const scalar_t is_proj = dit[0] * (vs_proj[0] - vt[0]) +
270  dit[1] * (vs_proj[1] - vt[1]) +
271  dit[2] * (vs_proj[2] - vt[2]) + intensity_target;
272 
273  const scalar_t s = dit[0] * nt[0] + dit[1] * nt[1] + dit[2] * nt[2];
274  const scalar_t ditM[3] = {s * nt[0] - dit[0], s * nt[1] - dit[1],
275  s * nt[2] - dit[2]};
276 
277  J_I[0] = sqrt_lambda_photometric * (-vs[2] * ditM[1] + vs[1] * ditM[2]);
278  J_I[1] = sqrt_lambda_photometric * (vs[2] * ditM[0] - vs[0] * ditM[2]);
279  J_I[2] = sqrt_lambda_photometric * (-vs[1] * ditM[0] + vs[0] * ditM[1]);
280  J_I[3] = sqrt_lambda_photometric * ditM[0];
281  J_I[4] = sqrt_lambda_photometric * ditM[1];
282  J_I[5] = sqrt_lambda_photometric * ditM[2];
283  r_I = sqrt_lambda_photometric * (intensity_source - is_proj);
284 
285  return true;
286 }
287 
288 template bool GetJacobianColoredICP(const int64_t workload_idx,
289  const float *source_points_ptr,
290  const float *source_colors_ptr,
291  const float *target_points_ptr,
292  const float *target_normals_ptr,
293  const float *target_colors_ptr,
294  const float *target_color_gradients_ptr,
295  const int64_t *correspondence_indices,
296  const float &sqrt_lambda_geometric,
297  const float &sqrt_lambda_photometric,
298  float *J_G,
299  float *J_I,
300  float &r_G,
301  float &r_I);
302 
303 template bool GetJacobianColoredICP(const int64_t workload_idx,
304  const double *source_points_ptr,
305  const double *source_colors_ptr,
306  const double *target_points_ptr,
307  const double *target_normals_ptr,
308  const double *target_colors_ptr,
309  const double *target_color_gradients_ptr,
310  const int64_t *correspondence_indices,
311  const double &sqrt_lambda_geometric,
312  const double &sqrt_lambda_photometric,
313  double *J_G,
314  double *J_I,
315  double &r_G,
316  double &r_I);
317 
318 template <typename scalar_t>
320  const scalar_t *R_S_to_V,
321  const scalar_t *r_v_to_s_in_V,
322  const scalar_t *w_v_in_V,
323  const scalar_t *v_v_in_V,
324  scalar_t *v_s_in_S) {
325  // Compute v_s_in_V = v_v_in_V + w_v_in_V.cross(r_v_to_s_in_V).
326  scalar_t v_s_in_V[3] = {0};
327  core::linalg::kernel::cross_3x1(w_v_in_V, r_v_to_s_in_V, v_s_in_V);
328  v_s_in_V[0] += v_v_in_V[0];
329  v_s_in_V[1] += v_v_in_V[1];
330  v_s_in_V[2] += v_v_in_V[2];
331 
332  // Compute v_s_in_S = R_S_to_V * v_s_in_V.
333  core::linalg::kernel::matmul3x3_3x1(R_S_to_V, v_s_in_V, v_s_in_S);
334 }
335 
336 template void PreComputeForDopplerICP(const float *R_S_to_V,
337  const float *r_v_to_s_in_V,
338  const float *w_v_in_V,
339  const float *v_v_in_V,
340  float *v_s_in_S);
341 
342 template void PreComputeForDopplerICP(const double *R_S_to_V,
343  const double *r_v_to_s_in_V,
344  const double *w_v_in_V,
345  const double *v_v_in_V,
346  double *v_s_in_S);
347 
348 template <typename scalar_t>
350  const int64_t workload_idx,
351  const scalar_t *source_points_ptr,
352  const scalar_t *source_dopplers_ptr,
353  const scalar_t *source_directions_ptr,
354  const scalar_t *target_points_ptr,
355  const scalar_t *target_normals_ptr,
356  const int64_t *correspondence_indices,
357  const scalar_t *R_S_to_V,
358  const scalar_t *r_v_to_s_in_V,
359  const scalar_t *v_s_in_S,
360  const bool reject_dynamic_outliers,
361  const scalar_t doppler_outlier_threshold,
362  const scalar_t &sqrt_lambda_geometric,
363  const scalar_t &sqrt_lambda_doppler,
364  const scalar_t &sqrt_lambda_doppler_by_dt,
365  scalar_t *J_G,
366  scalar_t *J_D,
367  scalar_t &r_G,
368  scalar_t &r_D) {
369  if (correspondence_indices[workload_idx] == -1) {
370  return false;
371  }
372 
373  const int64_t target_idx = 3 * correspondence_indices[workload_idx];
374  const int64_t source_idx = 3 * workload_idx;
375 
376  const scalar_t &doppler_in_S = source_dopplers_ptr[workload_idx];
377 
378  const scalar_t ds_in_V[3] = {source_directions_ptr[source_idx],
379  source_directions_ptr[source_idx + 1],
380  source_directions_ptr[source_idx + 2]};
381 
382  // Compute predicted Doppler velocity (in sensor frame).
383  scalar_t ds_in_S[3] = {0};
384  core::linalg::kernel::matmul3x3_3x1(R_S_to_V, ds_in_V, ds_in_S);
385  const scalar_t doppler_pred_in_S =
386  -core::linalg::kernel::dot_3x1(ds_in_S, v_s_in_S);
387 
388  // Compute Doppler error.
389  const double doppler_error = doppler_in_S - doppler_pred_in_S;
390 
391  // Dynamic point outlier rejection.
392  if (reject_dynamic_outliers &&
393  abs(doppler_error) > doppler_outlier_threshold) {
394  // Jacobian and residual are set to 0 by default.
395  return true;
396  }
397 
398  // Compute Doppler residual and Jacobian.
399  scalar_t J_D_w[3] = {0};
400  core::linalg::kernel::cross_3x1(ds_in_V, r_v_to_s_in_V, J_D_w);
401  J_D[0] = sqrt_lambda_doppler_by_dt * J_D_w[0];
402  J_D[1] = sqrt_lambda_doppler_by_dt * J_D_w[1];
403  J_D[2] = sqrt_lambda_doppler_by_dt * J_D_w[2];
404  J_D[3] = sqrt_lambda_doppler_by_dt * -ds_in_V[0];
405  J_D[4] = sqrt_lambda_doppler_by_dt * -ds_in_V[1];
406  J_D[5] = sqrt_lambda_doppler_by_dt * -ds_in_V[2];
407  r_D = sqrt_lambda_doppler * doppler_error;
408 
409  const scalar_t ps[3] = {source_points_ptr[source_idx],
410  source_points_ptr[source_idx + 1],
411  source_points_ptr[source_idx + 2]};
412 
413  const scalar_t pt[3] = {target_points_ptr[target_idx],
414  target_points_ptr[target_idx + 1],
415  target_points_ptr[target_idx + 2]};
416 
417  const scalar_t nt[3] = {target_normals_ptr[target_idx],
418  target_normals_ptr[target_idx + 1],
419  target_normals_ptr[target_idx + 2]};
420 
421  // Compute geometric point-to-plane error.
422  const scalar_t p2p_error = (ps[0] - pt[0]) * nt[0] +
423  (ps[1] - pt[1]) * nt[1] +
424  (ps[2] - pt[2]) * nt[2];
425 
426  // Compute geometric point-to-plane residual and Jacobian.
427  J_G[0] = sqrt_lambda_geometric * (-ps[2] * nt[1] + ps[1] * nt[2]);
428  J_G[1] = sqrt_lambda_geometric * (ps[2] * nt[0] - ps[0] * nt[2]);
429  J_G[2] = sqrt_lambda_geometric * (-ps[1] * nt[0] + ps[0] * nt[1]);
430  J_G[3] = sqrt_lambda_geometric * nt[0];
431  J_G[4] = sqrt_lambda_geometric * nt[1];
432  J_G[5] = sqrt_lambda_geometric * nt[2];
433  r_G = sqrt_lambda_geometric * p2p_error;
434 
435  return true;
436 }
437 
438 template bool GetJacobianDopplerICP(const int64_t workload_idx,
439  const float *source_points_ptr,
440  const float *source_dopplers_ptr,
441  const float *source_directions_ptr,
442  const float *target_points_ptr,
443  const float *target_normals_ptr,
444  const int64_t *correspondence_indices,
445  const float *R_S_to_V,
446  const float *r_v_to_s_in_V,
447  const float *v_s_in_S,
448  const bool reject_dynamic_outliers,
449  const float doppler_outlier_threshold,
450  const float &sqrt_lambda_geometric,
451  const float &sqrt_lambda_doppler,
452  const float &sqrt_lambda_doppler_by_dt,
453  float *J_G,
454  float *J_D,
455  float &r_G,
456  float &r_D);
457 
458 template bool GetJacobianDopplerICP(const int64_t workload_idx,
459  const double *source_points_ptr,
460  const double *source_dopplers_ptr,
461  const double *source_directions_ptr,
462  const double *target_points_ptr,
463  const double *target_normals_ptr,
464  const int64_t *correspondence_indices,
465  const double *R_S_to_V,
466  const double *r_v_to_s_in_V,
467  const double *v_s_in_S,
468  const bool reject_dynamic_outliers,
469  const double doppler_outlier_threshold,
470  const double &sqrt_lambda_geometric,
471  const double &sqrt_lambda_doppler,
472  const double &sqrt_lambda_doppler_by_dt,
473  double *J_G,
474  double *J_D,
475  double &r_G,
476  double &r_D);
477 
478 template <typename scalar_t>
480  int64_t workload_idx,
481  const scalar_t *target_points_ptr,
482  const int64_t *correspondence_indices,
483  scalar_t *jacobian_x,
484  scalar_t *jacobian_y,
485  scalar_t *jacobian_z) {
486  if (correspondence_indices[workload_idx] == -1) {
487  return false;
488  }
489 
490  const int64_t target_idx = 3 * correspondence_indices[workload_idx];
491 
492  jacobian_x[0] = jacobian_x[4] = jacobian_x[5] = 0.0;
493  jacobian_x[1] = target_points_ptr[target_idx + 2];
494  jacobian_x[2] = -target_points_ptr[target_idx + 1];
495  jacobian_x[3] = 1.0;
496 
497  jacobian_y[1] = jacobian_y[3] = jacobian_y[5] = 0.0;
498  jacobian_y[0] = -target_points_ptr[target_idx + 2];
499  jacobian_y[2] = target_points_ptr[target_idx];
500  jacobian_y[4] = 1.0;
501 
502  jacobian_z[2] = jacobian_z[3] = jacobian_z[4] = 0.0;
503  jacobian_z[0] = target_points_ptr[target_idx + 1];
504  jacobian_z[1] = -target_points_ptr[target_idx];
505  jacobian_z[5] = 1.0;
506 
507  return true;
508 }
509 
510 template bool GetInformationJacobians(int64_t workload_idx,
511  const float *target_points_ptr,
512  const int64_t *correspondence_indices,
513  float *jacobian_x,
514  float *jacobian_y,
515  float *jacobian_z);
516 
517 template bool GetInformationJacobians(int64_t workload_idx,
518  const double *target_points_ptr,
519  const int64_t *correspondence_indices,
520  double *jacobian_x,
521  double *jacobian_y,
522  double *jacobian_z);
523 
524 } // namespace kernel
525 } // namespace pipelines
526 } // namespace t
527 } // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:44
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void cross_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input, scalar_t *C_3x1_output)
Definition: Matrix.h:63
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE scalar_t dot_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input)
Definition: Matrix.h:89
OPEN3D_HOST_DEVICE bool GetJacobianPointToPlane(int64_t workload_idx, const scalar_t *source_points_ptr, const scalar_t *target_points_ptr, const scalar_t *target_normals_ptr, const int64_t *correspondence_indices, scalar_t *J_ij, scalar_t &r)
Definition: RegistrationImpl.h:153
void ComputeInformationMatrixCPU(const core::Tensor &target_points, const core::Tensor &correspondence_indices, core::Tensor &information_matrix, const core::Dtype &dtype, const core::Device &device)
Definition: RegistrationCPU.cpp:636
OPEN3D_HOST_DEVICE bool GetJacobianDopplerICP(const int64_t workload_idx, const scalar_t *source_points_ptr, const scalar_t *source_dopplers_ptr, const scalar_t *source_directions_ptr, const scalar_t *target_points_ptr, const scalar_t *target_normals_ptr, const int64_t *correspondence_indices, const scalar_t *R_S_to_V, const scalar_t *r_v_to_s_in_V, const scalar_t *v_s_in_S, const bool reject_dynamic_outliers, const scalar_t doppler_outlier_threshold, const scalar_t &sqrt_lambda_geometric, const scalar_t &sqrt_lambda_doppler, const scalar_t &sqrt_lambda_doppler_by_dt, scalar_t *J_G, scalar_t *J_D, scalar_t &r_G, scalar_t &r_D)
Definition: RegistrationImpl.h:349
void ComputeRtPointToPointCPU(const core::Tensor &source_points, const core::Tensor &target_points, const core::Tensor &corres, core::Tensor &R, core::Tensor &t, int &inlier_count, const core::Dtype &dtype, const core::Device &device)
Definition: RegistrationCPU.cpp:544
void ComputePoseDopplerICPCPU(const core::Tensor &source_points, const core::Tensor &source_dopplers, const core::Tensor &source_directions, const core::Tensor &target_points, const core::Tensor &target_normals, const core::Tensor &correspondence_indices, core::Tensor &output_pose, float &residual, int &inlier_count, const core::Dtype &dtype, const core::Device &device, const core::Tensor &R_S_to_V, const core::Tensor &r_v_to_s_in_V, const core::Tensor &w_v_in_V, const core::Tensor &v_v_in_V, const double period, const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, const double lambda_doppler)
Definition: RegistrationCPU.cpp:351
void ComputePosePointToPlaneCPU(const core::Tensor &source_points, const core::Tensor &target_points, const core::Tensor &target_normals, const core::Tensor &correspondence_indices, core::Tensor &pose, float &residual, int &inlier_count, const core::Dtype &dtype, const core::Device &device, const registration::RobustKernel &kernel)
Definition: RegistrationCPU.cpp:98
void ComputePoseColoredICPCPU(const core::Tensor &source_points, const core::Tensor &source_colors, const core::Tensor &target_points, const core::Tensor &target_normals, const core::Tensor &target_colors, const core::Tensor &target_color_gradients, const core::Tensor &correspondence_indices, core::Tensor &pose, float &residual, int &inlier_count, const core::Dtype &dtype, const core::Device &device, const registration::RobustKernel &kernel, const double &lambda_geometric)
Definition: RegistrationCPU.cpp:209
OPEN3D_HOST_DEVICE bool GetJacobianColoredICP(const int64_t workload_idx, const scalar_t *source_points_ptr, const scalar_t *source_colors_ptr, const scalar_t *target_points_ptr, const scalar_t *target_normals_ptr, const scalar_t *target_colors_ptr, const scalar_t *target_color_gradients_ptr, const int64_t *correspondence_indices, const scalar_t &sqrt_lambda_geometric, const scalar_t &sqrt_lambda_photometric, scalar_t *J_G, scalar_t *J_I, scalar_t &r_G, scalar_t &r_I)
Definition: RegistrationImpl.h:207
OPEN3D_HOST_DEVICE void PreComputeForDopplerICP(const scalar_t *R_S_to_V, const scalar_t *r_v_to_s_in_V, const scalar_t *w_v_in_V, const scalar_t *v_v_in_V, scalar_t *v_s_in_S)
Definition: RegistrationImpl.h:319
OPEN3D_HOST_DEVICE bool GetInformationJacobians(int64_t workload_idx, const scalar_t *target_points_ptr, const int64_t *correspondence_indices, scalar_t *jacobian_x, scalar_t *jacobian_y, scalar_t *jacobian_z)
Definition: RegistrationImpl.h:479
Definition: PinholeCameraIntrinsic.cpp:16