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