Open3D (C++ API)  0.16.1
FillInLinearSystemImpl.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
30
31namespace open3d {
32namespace t {
33namespace pipelines {
34namespace kernel {
35#if defined(__CUDACC__)
36void FillInRigidAlignmentTermCUDA
37#else
39#endif
40 (core::Tensor &AtA,
41 core::Tensor &Atb,
42 core::Tensor &residual,
43 const core::Tensor &Ti_ps,
44 const core::Tensor &Tj_qs,
45 const core::Tensor &Ri_normal_ps,
46 int i,
47 int j,
48 float threshold) {
49
50 core::Device device = AtA.GetDevice();
51 int64_t n = Ti_ps.GetLength();
52 if (Tj_qs.GetLength() != n || Ri_normal_ps.GetLength() != n) {
54 "Unable to setup linear system: input length mismatch.");
55 }
56
57 // First fill in a small 12 x 12 linear system
58 core::Tensor AtA_local =
59 core::Tensor::Zeros({12, 12}, core::Float32, device);
60 core::Tensor Atb_local = core::Tensor::Zeros({12}, core::Float32, device);
61
62 float *AtA_local_ptr = static_cast<float *>(AtA_local.GetDataPtr());
63 float *Atb_local_ptr = static_cast<float *>(Atb_local.GetDataPtr());
64 float *residual_ptr = static_cast<float *>(residual.GetDataPtr());
65
66 const float *Ti_ps_ptr = static_cast<const float *>(Ti_ps.GetDataPtr());
67 const float *Tj_qs_ptr = static_cast<const float *>(Tj_qs.GetDataPtr());
68 const float *Ri_normal_ps_ptr =
69 static_cast<const float *>(Ri_normal_ps.GetDataPtr());
70
72 AtA.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
73 const float *p_prime = Ti_ps_ptr + 3 * workload_idx;
74 const float *q_prime = Tj_qs_ptr + 3 * workload_idx;
75 const float *normal_p_prime =
76 Ri_normal_ps_ptr + 3 * workload_idx;
77
78 float r = (p_prime[0] - q_prime[0]) * normal_p_prime[0] +
79 (p_prime[1] - q_prime[1]) * normal_p_prime[1] +
80 (p_prime[2] - q_prime[2]) * normal_p_prime[2];
81 if (abs(r) > threshold) return;
82
83 float J_ij[12];
84 J_ij[0] = -q_prime[2] * normal_p_prime[1] +
85 q_prime[1] * normal_p_prime[2];
86 J_ij[1] = q_prime[2] * normal_p_prime[0] -
87 q_prime[0] * normal_p_prime[2];
88 J_ij[2] = -q_prime[1] * normal_p_prime[0] +
89 q_prime[0] * normal_p_prime[1];
90 J_ij[3] = normal_p_prime[0];
91 J_ij[4] = normal_p_prime[1];
92 J_ij[5] = normal_p_prime[2];
93 for (int k = 0; k < 6; ++k) {
94 J_ij[k + 6] = -J_ij[k];
95 }
96
97 // Not optimized; Switch to reduction if necessary.
98#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__)
99 for (int i_local = 0; i_local < 12; ++i_local) {
100 for (int j_local = 0; j_local < 12; ++j_local) {
101 atomicAdd(&AtA_local_ptr[i_local * 12 + j_local],
102 J_ij[i_local] * J_ij[j_local]);
103 }
104 atomicAdd(&Atb_local_ptr[i_local], J_ij[i_local] * r);
105 }
106 atomicAdd(residual_ptr, r * r);
107#else
108#pragma omp critical(FillInRigidAlignmentTermCPU)
109 {
110 for (int i_local = 0; i_local < 12; ++i_local) {
111 for (int j_local = 0; j_local < 12; ++j_local) {
112 AtA_local_ptr[i_local * 12 + j_local]
113 += J_ij[i_local] * J_ij[j_local];
114 }
115 Atb_local_ptr[i_local] += J_ij[i_local] * r;
116 }
117 *residual_ptr += r * r;
118 }
119#endif
120 });
121
122 // Then fill-in the large linear system
123 std::vector<int64_t> indices_vec(12);
124 for (int k = 0; k < 6; ++k) {
125 indices_vec[k] = i * 6 + k;
126 indices_vec[k + 6] = j * 6 + k;
127 }
128
129 std::vector<int64_t> indices_i_vec;
130 std::vector<int64_t> indices_j_vec;
131 for (int local_i = 0; local_i < 12; ++local_i) {
132 for (int local_j = 0; local_j < 12; ++local_j) {
133 indices_i_vec.push_back(indices_vec[local_i]);
134 indices_j_vec.push_back(indices_vec[local_j]);
135 }
136 }
137
138 core::Tensor indices(indices_vec, {12}, core::Int64, device);
139 core::Tensor indices_i(indices_i_vec, {12 * 12}, core::Int64, device);
140 core::Tensor indices_j(indices_j_vec, {12 * 12}, core::Int64, device);
141
142 core::Tensor AtA_sub = AtA.IndexGet({indices_i, indices_j});
143 AtA.IndexSet({indices_i, indices_j}, AtA_sub + AtA_local.View({12 * 12}));
144
145 core::Tensor Atb_sub = Atb.IndexGet({indices});
146 Atb.IndexSet({indices}, Atb_sub + Atb_local.View({12, 1}));
147}
148
149#if defined(__CUDACC__)
150void FillInSLACAlignmentTermCUDA
151#else
153#endif
154 (core::Tensor &AtA,
155 core::Tensor &Atb,
156 core::Tensor &residual,
157 const core::Tensor &Ti_Cps,
158 const core::Tensor &Tj_Cqs,
159 const core::Tensor &Cnormal_ps,
160 const core::Tensor &Ri_Cnormal_ps,
161 const core::Tensor &RjT_Ri_Cnormal_ps,
162 const core::Tensor &cgrid_idx_ps,
163 const core::Tensor &cgrid_idx_qs,
164 const core::Tensor &cgrid_ratio_qs,
165 const core::Tensor &cgrid_ratio_ps,
166 int i,
167 int j,
168 int n_frags,
169 float threshold) {
170 int64_t n = Ti_Cps.GetLength();
171 if (Tj_Cqs.GetLength() != n || Cnormal_ps.GetLength() != n ||
172 Ri_Cnormal_ps.GetLength() != n || RjT_Ri_Cnormal_ps.GetLength() != n ||
173 cgrid_idx_ps.GetLength() != n || cgrid_ratio_ps.GetLength() != n ||
174 cgrid_idx_qs.GetLength() != n || cgrid_ratio_qs.GetLength() != n) {
176 "Unable to setup linear system: input length mismatch.");
177 }
178
179 int n_vars = Atb.GetLength();
180 float *AtA_ptr = static_cast<float *>(AtA.GetDataPtr());
181 float *Atb_ptr = static_cast<float *>(Atb.GetDataPtr());
182 float *residual_ptr = static_cast<float *>(residual.GetDataPtr());
183
184 // Geometric properties
185 const float *Ti_Cps_ptr = static_cast<const float *>(Ti_Cps.GetDataPtr());
186 const float *Tj_Cqs_ptr = static_cast<const float *>(Tj_Cqs.GetDataPtr());
187 const float *Cnormal_ps_ptr =
188 static_cast<const float *>(Cnormal_ps.GetDataPtr());
189 const float *Ri_Cnormal_ps_ptr =
190 static_cast<const float *>(Ri_Cnormal_ps.GetDataPtr());
191 const float *RjT_Ri_Cnormal_ps_ptr =
192 static_cast<const float *>(RjT_Ri_Cnormal_ps.GetDataPtr());
193
194 // Association properties
195 const int *cgrid_idx_ps_ptr =
196 static_cast<const int *>(cgrid_idx_ps.GetDataPtr());
197 const int *cgrid_idx_qs_ptr =
198 static_cast<const int *>(cgrid_idx_qs.GetDataPtr());
199 const float *cgrid_ratio_ps_ptr =
200 static_cast<const float *>(cgrid_ratio_ps.GetDataPtr());
201 const float *cgrid_ratio_qs_ptr =
202 static_cast<const float *>(cgrid_ratio_qs.GetDataPtr());
203
205 AtA.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
206 const float *Ti_Cp = Ti_Cps_ptr + 3 * workload_idx;
207 const float *Tj_Cq = Tj_Cqs_ptr + 3 * workload_idx;
208 const float *Cnormal_p = Cnormal_ps_ptr + 3 * workload_idx;
209 const float *Ri_Cnormal_p =
210 Ri_Cnormal_ps_ptr + 3 * workload_idx;
211 const float *RjTRi_Cnormal_p =
212 RjT_Ri_Cnormal_ps_ptr + 3 * workload_idx;
213
214 const int *cgrid_idx_p = cgrid_idx_ps_ptr + 8 * workload_idx;
215 const int *cgrid_idx_q = cgrid_idx_qs_ptr + 8 * workload_idx;
216 const float *cgrid_ratio_p =
217 cgrid_ratio_ps_ptr + 8 * workload_idx;
218 const float *cgrid_ratio_q =
219 cgrid_ratio_qs_ptr + 8 * workload_idx;
220
221 float r = (Ti_Cp[0] - Tj_Cq[0]) * Ri_Cnormal_p[0] +
222 (Ti_Cp[1] - Tj_Cq[1]) * Ri_Cnormal_p[1] +
223 (Ti_Cp[2] - Tj_Cq[2]) * Ri_Cnormal_p[2];
224 if (abs(r) > threshold) return;
225
226 // Now we fill in a 60 x 60 sub-matrix: 2 x (6 + 8 x 3)
227 float J[60];
228 int idx[60];
229
230 // Jacobian w.r.t. Ti: 0-6
231 J[0] = -Tj_Cq[2] * Ri_Cnormal_p[1] + Tj_Cq[1] * Ri_Cnormal_p[2];
232 J[1] = Tj_Cq[2] * Ri_Cnormal_p[0] - Tj_Cq[0] * Ri_Cnormal_p[2];
233 J[2] = -Tj_Cq[1] * Ri_Cnormal_p[0] + Tj_Cq[0] * Ri_Cnormal_p[1];
234 J[3] = Ri_Cnormal_p[0];
235 J[4] = Ri_Cnormal_p[1];
236 J[5] = Ri_Cnormal_p[2];
237
238 // Jacobian w.r.t. Tj: 6-12
239 for (int k = 0; k < 6; ++k) {
240 J[k + 6] = -J[k];
241
242 idx[k + 0] = 6 * i + k;
243 idx[k + 6] = 6 * j + k;
244 }
245
246 // Jacobian w.r.t. C over p: 12-36
247 for (int k = 0; k < 8; ++k) {
248 J[12 + k * 3 + 0] = cgrid_ratio_p[k] * Cnormal_p[0];
249 J[12 + k * 3 + 1] = cgrid_ratio_p[k] * Cnormal_p[1];
250 J[12 + k * 3 + 2] = cgrid_ratio_p[k] * Cnormal_p[2];
251
252 idx[12 + k * 3 + 0] = 6 * n_frags + cgrid_idx_p[k] * 3 + 0;
253 idx[12 + k * 3 + 1] = 6 * n_frags + cgrid_idx_p[k] * 3 + 1;
254 idx[12 + k * 3 + 2] = 6 * n_frags + cgrid_idx_p[k] * 3 + 2;
255 }
256
257 // Jacobian w.r.t. C over q: 36-60
258 for (int k = 0; k < 8; ++k) {
259 J[36 + k * 3 + 0] = -cgrid_ratio_q[k] * RjTRi_Cnormal_p[0];
260 J[36 + k * 3 + 1] = -cgrid_ratio_q[k] * RjTRi_Cnormal_p[1];
261 J[36 + k * 3 + 2] = -cgrid_ratio_q[k] * RjTRi_Cnormal_p[2];
262
263 idx[36 + k * 3 + 0] = 6 * n_frags + cgrid_idx_q[k] * 3 + 0;
264 idx[36 + k * 3 + 1] = 6 * n_frags + cgrid_idx_q[k] * 3 + 1;
265 idx[36 + k * 3 + 2] = 6 * n_frags + cgrid_idx_q[k] * 3 + 2;
266 }
267
268 // Not optimized; Switch to reduction if necessary.
269#if defined(__CUDACC__)
270 for (int ki = 0; ki < 60; ++ki) {
271 for (int kj = 0; kj < 60; ++kj) {
272 float AtA_ij = J[ki] * J[kj];
273 int ij = idx[ki] * n_vars + idx[kj];
274 atomicAdd(AtA_ptr + ij, AtA_ij);
275 }
276 float Atb_i = J[ki] * r;
277 atomicAdd(Atb_ptr + idx[ki], Atb_i);
278 }
279 atomicAdd(residual_ptr, r * r);
280#else
281#pragma omp critical(FillInSLACAlignmentTermCPU)
282 {
283 for (int ki = 0; ki < 60; ++ki) {
284 for (int kj = 0; kj < 60; ++kj) {
285 AtA_ptr[idx[ki] * n_vars + idx[kj]]
286 += J[ki] * J[kj];
287 }
288 Atb_ptr[idx[ki]] += J[ki] * r;
289 }
290 *residual_ptr += r * r;
291 }
292#endif
293 });
294}
295
296#if defined(__CUDACC__)
297void FillInSLACRegularizerTermCUDA
298#else
300#endif
301 (core::Tensor &AtA,
302 core::Tensor &Atb,
303 core::Tensor &residual,
304 const core::Tensor &grid_idx,
305 const core::Tensor &grid_nbs_idx,
306 const core::Tensor &grid_nbs_mask,
307 const core::Tensor &positions_init,
308 const core::Tensor &positions_curr,
309 float weight,
310 int n_frags,
311 int anchor_idx) {
312
313 int64_t n = grid_idx.GetLength();
314 int64_t n_vars = Atb.GetLength();
315
316 float *AtA_ptr = static_cast<float *>(AtA.GetDataPtr());
317 float *Atb_ptr = static_cast<float *>(Atb.GetDataPtr());
318 float *residual_ptr = static_cast<float *>(residual.GetDataPtr());
319
320 const int *grid_idx_ptr = static_cast<const int *>(grid_idx.GetDataPtr());
321 const int *grid_nbs_idx_ptr =
322 static_cast<const int *>(grid_nbs_idx.GetDataPtr());
323 const bool *grid_nbs_mask_ptr =
324 static_cast<const bool *>(grid_nbs_mask.GetDataPtr());
325
326 const float *positions_init_ptr =
327 static_cast<const float *>(positions_init.GetDataPtr());
328 const float *positions_curr_ptr =
329 static_cast<const float *>(positions_curr.GetDataPtr());
330
332 AtA.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
333 // Enumerate 6 neighbors
334 int idx_i = grid_idx_ptr[workload_idx];
335
336 const int *idx_nbs = grid_nbs_idx_ptr + 6 * workload_idx;
337 const bool *mask_nbs = grid_nbs_mask_ptr + 6 * workload_idx;
338
339 // Build a 3x3 linear system to compute the local R
340 float cov[3][3] = {{0}};
341 float U[3][3], V[3][3], S[3];
342
343 int cnt = 0;
344 for (int k = 0; k < 6; ++k) {
345 bool mask_k = mask_nbs[k];
346 if (!mask_k) continue;
347
348 int idx_k = idx_nbs[k];
349
350 // Now build linear systems
351 float diff_ik_init[3] = {
352 positions_init_ptr[idx_i * 3 + 0] -
353 positions_init_ptr[idx_k * 3 + 0],
354 positions_init_ptr[idx_i * 3 + 1] -
355 positions_init_ptr[idx_k * 3 + 1],
356 positions_init_ptr[idx_i * 3 + 2] -
357 positions_init_ptr[idx_k * 3 + 2]};
358 float diff_ik_curr[3] = {
359 positions_curr_ptr[idx_i * 3 + 0] -
360 positions_curr_ptr[idx_k * 3 + 0],
361 positions_curr_ptr[idx_i * 3 + 1] -
362 positions_curr_ptr[idx_k * 3 + 1],
363 positions_curr_ptr[idx_i * 3 + 2] -
364 positions_curr_ptr[idx_k * 3 + 2]};
365
366 // Build linear system by computing XY^T when formulating Y
367 // = RX Y: curr X: init
368 for (int i = 0; i < 3; ++i) {
369 for (int j = 0; j < 3; ++j) {
370 cov[i][j] += diff_ik_init[i] * diff_ik_curr[j];
371 }
372 }
373 ++cnt;
374 }
375
376 if (cnt < 3) {
377 return;
378 }
379
380 core::linalg::kernel::svd3x3(*cov, *U, S, *V);
381
382 float R[3][3];
385
386 float d = core::linalg::kernel::det3x3(*R);
387
388 if (d < 0) {
389 U[2][0] = -U[2][0];
390 U[2][1] = -U[2][1];
391 U[2][2] = -U[2][2];
393 }
394
395 // Now we have R, we build Hessian and residuals
396 // But first, we need to anchor a point
397 if (idx_i == anchor_idx) {
398 R[0][0] = R[1][1] = R[2][2] = 1;
399 R[0][1] = R[0][2] = R[1][0] = R[1][2] = R[2][0] = R[2][1] =
400 0;
401 }
402 for (int k = 0; k < 6; ++k) {
403 bool mask_k = mask_nbs[k];
404
405 if (mask_k) {
406 int idx_k = idx_nbs[k];
407
408 float diff_ik_init[3] = {
409 positions_init_ptr[idx_i * 3 + 0] -
410 positions_init_ptr[idx_k * 3 + 0],
411 positions_init_ptr[idx_i * 3 + 1] -
412 positions_init_ptr[idx_k * 3 + 1],
413 positions_init_ptr[idx_i * 3 + 2] -
414 positions_init_ptr[idx_k * 3 + 2]};
415 float diff_ik_curr[3] = {
416 positions_curr_ptr[idx_i * 3 + 0] -
417 positions_curr_ptr[idx_k * 3 + 0],
418 positions_curr_ptr[idx_i * 3 + 1] -
419 positions_curr_ptr[idx_k * 3 + 1],
420 positions_curr_ptr[idx_i * 3 + 2] -
421 positions_curr_ptr[idx_k * 3 + 2]};
422 float R_diff_ik_curr[3];
423
424 core::linalg::kernel::matmul3x3_3x1(*R, diff_ik_init,
425 R_diff_ik_curr);
426
427 float local_r[3];
428 local_r[0] = diff_ik_curr[0] - R_diff_ik_curr[0];
429 local_r[1] = diff_ik_curr[1] - R_diff_ik_curr[1];
430 local_r[2] = diff_ik_curr[2] - R_diff_ik_curr[2];
431
432 int offset_idx_i = 3 * idx_i + 6 * n_frags;
433 int offset_idx_k = 3 * idx_k + 6 * n_frags;
434
435#if defined(__CUDACC__)
436 // Update residual
437 atomicAdd(residual_ptr,
438 weight * (local_r[0] * local_r[0] +
439 local_r[1] * local_r[1] +
440 local_r[2] * local_r[2]));
441
442 for (int axis = 0; axis < 3; ++axis) {
443 // Update AtA: 2x2
444 atomicAdd(&AtA_ptr[(offset_idx_i + axis) * n_vars +
445 offset_idx_i + axis],
446 weight);
447 atomicAdd(&AtA_ptr[(offset_idx_k + axis) * n_vars +
448 offset_idx_k + axis],
449 weight);
450 atomicAdd(&AtA_ptr[(offset_idx_i + axis) * n_vars +
451 offset_idx_k + axis],
452 -weight);
453 atomicAdd(&AtA_ptr[(offset_idx_k + axis) * n_vars +
454 offset_idx_i + axis],
455 -weight);
456
457 // Update Atb: 2x1
458 atomicAdd(&Atb_ptr[offset_idx_i + axis],
459 +weight * local_r[axis]);
460 atomicAdd(&Atb_ptr[offset_idx_k + axis],
461 -weight * local_r[axis]);
462 }
463#else
464#pragma omp critical(FillInSLACRegularizerTermCPU)
465 {
466 // Update residual
467 *residual_ptr += weight * (local_r[0] * local_r[0] +
468 local_r[1] * local_r[1] +
469 local_r[2] * local_r[2]);
470
471 for (int axis = 0; axis < 3; ++axis) {
472 // Update AtA: 2x2
473 AtA_ptr[(offset_idx_i + axis) * n_vars +
474 offset_idx_i + axis] += weight;
475 AtA_ptr[(offset_idx_k + axis) * n_vars +
476 offset_idx_k + axis] += weight;
477
478 AtA_ptr[(offset_idx_i + axis) * n_vars +
479 offset_idx_k + axis] -= weight;
480 AtA_ptr[(offset_idx_k + axis) * n_vars +
481 offset_idx_i + axis] -= weight;
482
483 // Update Atb: 2x1
484 Atb_ptr[offset_idx_i + axis] += weight * local_r[axis];
485 Atb_ptr[offset_idx_k + axis] -= weight * local_r[axis];
486 }
487 }
488#endif
489 }
490 }
491 });
492}
493} // namespace kernel
494} // namespace pipelines
495} // namespace t
496} // namespace open3d
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:64
#define LogError(...)
Definition: Logging.h:67
Definition: Device.h:37
Definition: Tensor.h:51
T * GetDataPtr()
Definition: Tensor.h:1149
Tensor View(const SizeVector &dst_shape) const
Definition: Tensor.cpp:707
static Tensor Zeros(const SizeVector &shape, Dtype dtype, const Device &device=Device("CPU:0"))
Create a tensor fill with zeros.
Definition: Tensor.cpp:392
void IndexSet(const std::vector< Tensor > &index_tensors, const Tensor &src_tensor)
Advanced indexing getter.
Definition: Tensor.cpp:922
Tensor IndexGet(const std::vector< Tensor > &index_tensors) const
Advanced indexing getter. This will always allocate a new Tensor.
Definition: Tensor.cpp:891
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void transpose3x3_(scalar_t *A_3x3)
Definition: Matrix.h:170
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x3(const scalar_t *A_3x3, const scalar_t *B_3x3, scalar_t *C_3x3)
Definition: Matrix.h:67
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void svd3x3(const scalar_t *A_3x3, scalar_t *U_3x3, scalar_t *S_3x1, scalar_t *V_3x3)
OPEN3D_DEVICE OPEN3D_FORCE_INLINE scalar_t det3x3(const scalar_t *A_3x3)
Definition: Matrix.h:108
const Dtype Int64
Definition: Dtype.cpp:66
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition: ParallelFor.h:122
const Dtype Float32
Definition: Dtype.cpp:61
void FillInSLACAlignmentTermCPU(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &Ti_qs, const core::Tensor &Tj_qs, const core::Tensor &normal_ps, const core::Tensor &Ri_normal_ps, const core::Tensor &RjT_Ri_normal_ps, const core::Tensor &cgrid_idx_ps, const core::Tensor &cgrid_idx_qs, const core::Tensor &cgrid_ratio_qs, const core::Tensor &cgrid_ratio_ps, int i, int j, int n, float threshold)
Definition: FillInLinearSystemImpl.h:154
void FillInRigidAlignmentTermCPU(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &Ti_qs, const core::Tensor &Tj_qs, const core::Tensor &Ri_normal_ps, int i, int j, float threshold)
Definition: FillInLinearSystemImpl.h:40
void FillInSLACRegularizerTermCPU(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &grid_idx, const core::Tensor &grid_nbs_idx, const core::Tensor &grid_nbs_mask, const core::Tensor &positions_init, const core::Tensor &positions_curr, float weight, int n, int anchor_idx)
Definition: FillInLinearSystemImpl.h:301
Definition: PinholeCameraIntrinsic.cpp:35