Open3D (C++ API)  0.17.0
Loading...
Searching...
No Matches
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
13#include "open3d/core/Tensor.h"
15
16namespace open3d {
17namespace t {
18namespace pipelines {
19namespace kernel {
20
21void ComputePosePointToPlaneCPU(const core::Tensor &source_points,
22 const core::Tensor &target_points,
23 const core::Tensor &target_normals,
24 const core::Tensor &correspondence_indices,
25 core::Tensor &pose,
26 float &residual,
27 int &inlier_count,
28 const core::Dtype &dtype,
29 const core::Device &device,
30 const registration::RobustKernel &kernel);
31
32void ComputePoseColoredICPCPU(const core::Tensor &source_points,
33 const core::Tensor &source_colors,
34 const core::Tensor &target_points,
35 const core::Tensor &target_normals,
36 const core::Tensor &target_colors,
37 const core::Tensor &target_color_gradients,
38 const core::Tensor &correspondence_indices,
39 core::Tensor &pose,
40 float &residual,
41 int &inlier_count,
42 const core::Dtype &dtype,
43 const core::Device &device,
44 const registration::RobustKernel &kernel,
45 const double &lambda_geometric);
46
47#ifdef BUILD_CUDA_MODULE
48void ComputePosePointToPlaneCUDA(const core::Tensor &source_points,
49 const core::Tensor &target_points,
50 const core::Tensor &target_normals,
51 const core::Tensor &correspondence_indices,
52 core::Tensor &pose,
53 float &residual,
54 int &inlier_count,
55 const core::Dtype &dtype,
56 const core::Device &device,
57 const registration::RobustKernel &kernel);
58
59void ComputePoseColoredICPCUDA(const core::Tensor &source_points,
60 const core::Tensor &source_colors,
61 const core::Tensor &target_points,
62 const core::Tensor &target_normals,
63 const core::Tensor &target_colors,
64 const core::Tensor &target_color_gradients,
65 const core::Tensor &correspondence_indices,
66 core::Tensor &pose,
67 float &residual,
68 int &inlier_count,
69 const core::Dtype &dtype,
70 const core::Device &device,
71 const registration::RobustKernel &kernel,
72 const double &lambda_geometric);
73#endif
74
75void ComputeRtPointToPointCPU(const core::Tensor &source_points,
76 const core::Tensor &target_points,
77 const core::Tensor &correspondence_indices,
78 core::Tensor &R,
79 core::Tensor &t,
80 int &inlier_count,
81 const core::Dtype &dtype,
82 const core::Device &device);
83
84void ComputeInformationMatrixCPU(const core::Tensor &target_points,
85 const core::Tensor &correspondence_indices,
86 core::Tensor &information_matrix,
87 const core::Dtype &dtype,
88 const core::Device &device);
89
90#ifdef BUILD_CUDA_MODULE
91void ComputeInformationMatrixCUDA(const core::Tensor &target_points,
92 const core::Tensor &correspondence_indices,
93 core::Tensor &information_matrix,
94 const core::Dtype &dtype,
95 const core::Device &device);
96#endif
97
98template <typename scalar_t>
100 int64_t workload_idx,
101 const scalar_t *source_points_ptr,
102 const scalar_t *target_points_ptr,
103 const scalar_t *target_normals_ptr,
104 const int64_t *correspondence_indices,
105 scalar_t *J_ij,
106 scalar_t &r) {
107 if (correspondence_indices[workload_idx] == -1) {
108 return false;
109 }
110
111 const int64_t target_idx = 3 * correspondence_indices[workload_idx];
112 const int64_t source_idx = 3 * workload_idx;
113
114 const scalar_t &sx = source_points_ptr[source_idx + 0];
115 const scalar_t &sy = source_points_ptr[source_idx + 1];
116 const scalar_t &sz = source_points_ptr[source_idx + 2];
117 const scalar_t &tx = target_points_ptr[target_idx + 0];
118 const scalar_t &ty = target_points_ptr[target_idx + 1];
119 const scalar_t &tz = target_points_ptr[target_idx + 2];
120 const scalar_t &nx = target_normals_ptr[target_idx + 0];
121 const scalar_t &ny = target_normals_ptr[target_idx + 1];
122 const scalar_t &nz = target_normals_ptr[target_idx + 2];
123
124 r = (sx - tx) * nx + (sy - ty) * ny + (sz - tz) * nz;
125
126 J_ij[0] = nz * sy - ny * sz;
127 J_ij[1] = nx * sz - nz * sx;
128 J_ij[2] = ny * sx - nx * sy;
129 J_ij[3] = nx;
130 J_ij[4] = ny;
131 J_ij[5] = nz;
132
133 return true;
134}
135
136template bool GetJacobianPointToPlane(int64_t workload_idx,
137 const float *source_points_ptr,
138 const float *target_points_ptr,
139 const float *target_normals_ptr,
140 const int64_t *correspondence_indices,
141 float *J_ij,
142 float &r);
143
144template bool GetJacobianPointToPlane(int64_t workload_idx,
145 const double *source_points_ptr,
146 const double *target_points_ptr,
147 const double *target_normals_ptr,
148 const int64_t *correspondence_indices,
149 double *J_ij,
150 double &r);
151
152template <typename scalar_t>
154 const int64_t workload_idx,
155 const scalar_t *source_points_ptr,
156 const scalar_t *source_colors_ptr,
157 const scalar_t *target_points_ptr,
158 const scalar_t *target_normals_ptr,
159 const scalar_t *target_colors_ptr,
160 const scalar_t *target_color_gradients_ptr,
161 const int64_t *correspondence_indices,
162 const scalar_t &sqrt_lambda_geometric,
163 const scalar_t &sqrt_lambda_photometric,
164 scalar_t *J_G,
165 scalar_t *J_I,
166 scalar_t &r_G,
167 scalar_t &r_I) {
168 if (correspondence_indices[workload_idx] == -1) {
169 return false;
170 }
171
172 const int64_t target_idx = 3 * correspondence_indices[workload_idx];
173 const int64_t source_idx = 3 * workload_idx;
174
175 const scalar_t vs[3] = {source_points_ptr[source_idx],
176 source_points_ptr[source_idx + 1],
177 source_points_ptr[source_idx + 2]};
178
179 const scalar_t vt[3] = {target_points_ptr[target_idx],
180 target_points_ptr[target_idx + 1],
181 target_points_ptr[target_idx + 2]};
182
183 const scalar_t nt[3] = {target_normals_ptr[target_idx],
184 target_normals_ptr[target_idx + 1],
185 target_normals_ptr[target_idx + 2]};
186
187 const scalar_t d = (vs[0] - vt[0]) * nt[0] + (vs[1] - vt[1]) * nt[1] +
188 (vs[2] - vt[2]) * nt[2];
189
190 J_G[0] = sqrt_lambda_geometric * (-vs[2] * nt[1] + vs[1] * nt[2]);
191 J_G[1] = sqrt_lambda_geometric * (vs[2] * nt[0] - vs[0] * nt[2]);
192 J_G[2] = sqrt_lambda_geometric * (-vs[1] * nt[0] + vs[0] * nt[1]);
193 J_G[3] = sqrt_lambda_geometric * nt[0];
194 J_G[4] = sqrt_lambda_geometric * nt[1];
195 J_G[5] = sqrt_lambda_geometric * nt[2];
196 r_G = sqrt_lambda_geometric * d;
197
198 const scalar_t vs_proj[3] = {vs[0] - d * nt[0], vs[1] - d * nt[1],
199 vs[2] - d * nt[2]};
200
201 const scalar_t intensity_source =
202 (source_colors_ptr[source_idx] + source_colors_ptr[source_idx + 1] +
203 source_colors_ptr[source_idx + 2]) /
204 3.0;
205
206 const scalar_t intensity_target =
207 (target_colors_ptr[target_idx] + target_colors_ptr[target_idx + 1] +
208 target_colors_ptr[target_idx + 2]) /
209 3.0;
210
211 const scalar_t dit[3] = {target_color_gradients_ptr[target_idx],
212 target_color_gradients_ptr[target_idx + 1],
213 target_color_gradients_ptr[target_idx + 2]};
214
215 const scalar_t is_proj = dit[0] * (vs_proj[0] - vt[0]) +
216 dit[1] * (vs_proj[1] - vt[1]) +
217 dit[2] * (vs_proj[2] - vt[2]) + intensity_target;
218
219 const scalar_t s = dit[0] * nt[0] + dit[1] * nt[1] + dit[2] * nt[2];
220 const scalar_t ditM[3] = {s * nt[0] - dit[0], s * nt[1] - dit[1],
221 s * nt[2] - dit[2]};
222
223 J_I[0] = sqrt_lambda_photometric * (-vs[2] * ditM[1] + vs[1] * ditM[2]);
224 J_I[1] = sqrt_lambda_photometric * (vs[2] * ditM[0] - vs[0] * ditM[2]);
225 J_I[2] = sqrt_lambda_photometric * (-vs[1] * ditM[0] + vs[0] * ditM[1]);
226 J_I[3] = sqrt_lambda_photometric * ditM[0];
227 J_I[4] = sqrt_lambda_photometric * ditM[1];
228 J_I[5] = sqrt_lambda_photometric * ditM[2];
229 r_I = sqrt_lambda_photometric * (intensity_source - is_proj);
230
231 return true;
232}
233
234template bool GetJacobianColoredICP(const int64_t workload_idx,
235 const float *source_points_ptr,
236 const float *source_colors_ptr,
237 const float *target_points_ptr,
238 const float *target_normals_ptr,
239 const float *target_colors_ptr,
240 const float *target_color_gradients_ptr,
241 const int64_t *correspondence_indices,
242 const float &sqrt_lambda_geometric,
243 const float &sqrt_lambda_photometric,
244 float *J_G,
245 float *J_I,
246 float &r_G,
247 float &r_I);
248
249template bool GetJacobianColoredICP(const int64_t workload_idx,
250 const double *source_points_ptr,
251 const double *source_colors_ptr,
252 const double *target_points_ptr,
253 const double *target_normals_ptr,
254 const double *target_colors_ptr,
255 const double *target_color_gradients_ptr,
256 const int64_t *correspondence_indices,
257 const double &sqrt_lambda_geometric,
258 const double &sqrt_lambda_photometric,
259 double *J_G,
260 double *J_I,
261 double &r_G,
262 double &r_I);
263
264template <typename scalar_t>
266 int64_t workload_idx,
267 const scalar_t *target_points_ptr,
268 const int64_t *correspondence_indices,
269 scalar_t *jacobian_x,
270 scalar_t *jacobian_y,
271 scalar_t *jacobian_z) {
272 if (correspondence_indices[workload_idx] == -1) {
273 return false;
274 }
275
276 const int64_t target_idx = 3 * correspondence_indices[workload_idx];
277
278 jacobian_x[0] = jacobian_x[4] = jacobian_x[5] = 0.0;
279 jacobian_x[1] = target_points_ptr[target_idx + 2];
280 jacobian_x[2] = -target_points_ptr[target_idx + 1];
281 jacobian_x[3] = 1.0;
282
283 jacobian_y[1] = jacobian_y[3] = jacobian_y[5] = 0.0;
284 jacobian_y[0] = -target_points_ptr[target_idx + 2];
285 jacobian_y[2] = target_points_ptr[target_idx];
286 jacobian_y[4] = 1.0;
287
288 jacobian_z[2] = jacobian_z[3] = jacobian_z[4] = 0.0;
289 jacobian_z[0] = target_points_ptr[target_idx + 1];
290 jacobian_z[1] = -target_points_ptr[target_idx];
291 jacobian_z[5] = 1.0;
292
293 return true;
294}
295
296template bool GetInformationJacobians(int64_t workload_idx,
297 const float *target_points_ptr,
298 const int64_t *correspondence_indices,
299 float *jacobian_x,
300 float *jacobian_y,
301 float *jacobian_z);
302
303template bool GetInformationJacobians(int64_t workload_idx,
304 const double *target_points_ptr,
305 const int64_t *correspondence_indices,
306 double *jacobian_x,
307 double *jacobian_y,
308 double *jacobian_z);
309
310} // namespace kernel
311} // namespace pipelines
312} // namespace t
313} // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition CUDAUtils.h:44
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:99
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:468
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:376
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:153
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:265
Definition PinholeCameraIntrinsic.cpp:16