errorTerms.cpp
Go to the documentation of this file.
1 /* -------------------------------------------------------------------------
2  * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
3  * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
4  * See LICENSE for license information.
5  * ------------------------------------------------------------------------- */
13 #include <mp2p_icp/errorTerms.h>
14 #include <mp2p_icp/optimal_tf_gauss_newton.h>
15 #include <mrpt/math/CVectorFixed.h>
16 #include <mrpt/math/TPoint3D.h>
17 #include <mrpt/math/geometry.h>
18 #include <mrpt/math/ops_containers.h> // dotProduct()
19 #include <mrpt/poses/CPose3D.h>
20 #include <mrpt/poses/Lie/SE.h>
21 
22 #include <Eigen/Dense>
23 #include <iostream>
24 
25 using namespace mp2p_icp;
26 using namespace mrpt::math;
27 
28 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2point(
29  const mrpt::tfest::TMatchingPair& pairing, const mrpt::poses::CPose3D& relativePose,
30  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
31 {
32  MRPT_START
33  mrpt::math::CVectorFixedDouble<3> error;
34  const mrpt::math::TPoint3D& l = pairing.local;
35 
36  const mrpt::math::TPoint3D g = relativePose.composePoint(l);
37 
38  error[0] = g.x - pairing.global.x;
39  error[1] = g.y - pairing.global.y;
40  error[2] = g.z - pairing.global.z;
41 
42  // It's possible change the error to scalar with the function
43  // g.DistanceTo(l) Eval Jacobian:
44  if (jacobian)
45  {
46  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
47  // clang-format off
48  J_aux = (Eigen::Matrix<double, 3, 12>() <<
49  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
50  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
51  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
52  ).finished();
53  // clang-format on
54  }
55 
56  return error;
57  MRPT_END
58 }
59 
60 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2line(
61  const mp2p_icp::point_line_pair_t& pairing, const mrpt::poses::CPose3D& relativePose,
62  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
63 {
64  MRPT_START
65  mrpt::math::CVectorFixedDouble<3> error;
66  const auto& lnGlob = pairing.ln_global;
67 
68  // local and global point:
69  const mrpt::math::TPoint3D l = pairing.pt_local;
70  const mrpt::math::TPoint3D g = relativePose.composePoint(l);
71 
72  // Module of vector director of line
73  const auto& u = lnGlob.director;
74  const auto q = (g - lnGlob.pBase);
75 
76  const auto uq = mrpt::math::dotProduct<3, double>(u, q);
77 
78  error[0] = q[0] - u.x * uq;
79  error[1] = q[1] - u.y * uq;
80  error[2] = q[2] - u.z * uq;
81 
82  if (jacobian)
83  {
84  // J1
85  const Eigen::Matrix<double, 3, 3> J1 =
86  (Eigen::Matrix<double, 3, 3>() << 1 - mrpt::square(u.x), -u.x * u.y, -u.x * u.z,
87  -u.y * u.x, 1 - mrpt::square(u.y), -u.y * u.z, -u.z * u.x, -u.z * u.y,
88  1 - mrpt::square(u.z))
89  .finished();
90  // J2
91  // clang-format off
92  Eigen::Matrix<double, 3, 12> J2 =
93  (Eigen::Matrix<double, 3, 12>() <<
94  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
95  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
96  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
97  ).finished();
98  // clang-format on
99  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
100 
101  J_aux = J1 * J2;
102  }
103  return error;
104  MRPT_END
105 }
106 
107 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2plane(
108  const mp2p_icp::point_plane_pair_t& pairing, const mrpt::poses::CPose3D& relativePose,
109  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
110 {
111  MRPT_START
112  mrpt::math::CVectorFixedDouble<3> error;
113  const auto& p = pairing.pt_local;
114  const auto& pl_aux = pairing.pl_global.plane;
115  const mrpt::math::TPoint3D l = TPoint3D(p.x, p.y, p.z);
116  mrpt::math::TPoint3D g;
117  relativePose.composePoint(l, g);
118  double mod_n = mrpt::square(pl_aux.coefs[0]) + mrpt::square(pl_aux.coefs[1]) +
119  mrpt::square(pl_aux.coefs[2]);
120 
121  error[0] = -(pl_aux.coefs[0] / mod_n) * (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
122  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
123  error[1] = -(pl_aux.coefs[1] / mod_n) * (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
124  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
125  error[2] = -(pl_aux.coefs[2] / mod_n) * (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
126  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
127  if (jacobian)
128  {
129  // Eval Jacobian:
130  // J1
131  const Eigen::Matrix<double, 3, 3> J1 =
132  (Eigen::Matrix<double, 3, 3>() << -mrpt::square(pl_aux.coefs[0]) / mod_n,
133  -pl_aux.coefs[0] * pl_aux.coefs[1] / mod_n, -pl_aux.coefs[0] * pl_aux.coefs[2] / mod_n,
134  -pl_aux.coefs[1] * pl_aux.coefs[0] / mod_n, -mrpt::square(pl_aux.coefs[1]) / mod_n,
135  -pl_aux.coefs[1] * pl_aux.coefs[2] / mod_n, -pl_aux.coefs[2] * pl_aux.coefs[0] / mod_n,
136  -pl_aux.coefs[2] * pl_aux.coefs[1] / mod_n, -mrpt::square(pl_aux.coefs[2]) / mod_n)
137  .finished();
138  // J2
139  // clang-format off
140  const Eigen::Matrix<double, 3, 12> J2 =
141  (Eigen::Matrix<double, 3, 12>() <<
142  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
143  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
144  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
145  ).finished();
146  // clang-format on
147 
148  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
149  J_aux = J1 * J2;
150  }
151  return error;
152  MRPT_END
153 }
154 
155 mrpt::math::CVectorFixedDouble<4> mp2p_icp::error_line2line(
156  const mp2p_icp::matched_line_t& pairing, const mrpt::poses::CPose3D& relativePose,
157  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 12>> jacobian)
158 {
159  MRPT_START
160  mrpt::math::CVectorFixedDouble<4> error;
161  mrpt::math::TLine3D ln_aux;
162  mrpt::math::TPoint3D g;
163 
164  const auto& p0 = pairing.ln_local.pBase;
165  const auto& u0 = pairing.ln_local.director;
166  const auto& p1 = pairing.ln_global.pBase;
167  const auto& u1 = pairing.ln_global.director;
168 
169  relativePose.composePoint(p0, g);
170  ln_aux.pBase = mrpt::math::TPoint3D(g);
171 
172  // Homogeneous matrix calculation
173  mrpt::math::CMatrixDouble44 aux;
174  relativePose.getHomogeneousMatrix(aux);
175  const Eigen::Matrix<double, 4, 4> T = aux.asEigen();
176 
177  // Projection of the director vector for the new pose
178  const Eigen::Matrix<double, 1, 4> U =
179  (Eigen::Matrix<double, 1, 4>() << u0[0], u0[1], u0[2], 1).finished();
180  const Eigen::Matrix<double, 1, 4> U_T = U * T;
181  ln_aux.director = {U_T[0], U_T[1], U_T[2]};
182 
183  // Angle formed between the lines
184  double alfa = getAngle(pairing.ln_global, ln_aux) * 180 / (2 * 3.14159265);
185  /*
186  std::cout << "\nLine 1:\n"
187  << pairing.ln_this << "\nLine 2:\n"
188  << pairing.ln_other << "\nLine 2':\n"
189  << ln_aux << "\nAngle:\n"
190  << alfa << "\nT:\n"
191  << T << "\n";
192  */
193 
194  // p_r0 = (p-r_{0,r}). Ec.20
195  const Eigen::Matrix<double, 1, 3> p_r2 =
196  (Eigen::Matrix<double, 1, 3>() << ln_aux.pBase.x - p1.x, ln_aux.pBase.y - p1.y,
197  ln_aux.pBase.z - p1.z)
198  .finished();
199 
200  const Eigen::Matrix<double, 1, 3> rv =
201  (Eigen::Matrix<double, 1, 3>() << u1[0], u1[1], u1[2]).finished();
202 
203  // Relationship between lines
204  const double tolerance = 0.01;
205  if (abs(alfa) < tolerance)
206  { // Parallel
207  // Error: Ec.20
208  error[0] = mrpt::square(pairing.ln_global.distance(ln_aux.pBase));
209  if (jacobian)
210  {
211  // Module of vector director of line
212  double mod_rv = rv * rv.transpose();
213 
214  // J1: Ec.22
215  Eigen::Matrix<double, 1, 3> J1 = 2 * p_r2 - (2 / mod_rv) * (p_r2 * rv.transpose()) * rv;
216  // J2: Ec.23
217  // clang-format off
218  const Eigen::Matrix<double, 3, 12> J2 =
219  (Eigen::Matrix<double, 3, 12>() <<
220  p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0, 0,
221  0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0,
222  0, 0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1
223  ).finished();
224  // clang-format on
225  // Build Jacobian
226  mrpt::math::CMatrixFixed<double, 4, 12>& J_auxp = jacobian.value().get();
227  J_auxp.block<1, 12>(0, 0) = J1 * J2;
228  }
229  }
230  else
231  { // Rest
232  // Error:
233  // Cross product (r_u x r_2,v)
234  const double rw_x = U_T[1] * u1[2] - U_T[2] * u1[1];
235  const double rw_y = -(U_T[0] * u1[2] - U_T[2] * u1[0]);
236  const double rw_z = U_T[0] * u1[1] - U_T[1] * u1[0];
237 
238  const Eigen::Matrix<double, 1, 3> r_w =
239  (Eigen::Matrix<double, 1, 3>() << rw_x, rw_y, rw_z).finished();
240  double aux_rw = r_w * r_w.transpose();
241  // Error 1. Ec.26
242  error[0] = p_r2.dot(r_w) / sqrt(aux_rw);
243  // Error 2. Ec.27
244  error[1] = U_T[0] - u1[0];
245  error[2] = U_T[1] - u1[1];
246  error[3] = U_T[2] - u1[2];
247  if (jacobian)
248  {
249  // J1.1: Ec.32
250  Eigen::Matrix<double, 1, 3> J1_1 = r_w / sqrt(aux_rw);
251 
252  // J1.2:
253  // A
254  const double A = p_r2[0] * r_w[0] + p_r2[1] * r_w[1] + p_r2[2] * r_w[2];
255  const double Ax = -u1[2] * p_r2[1] + u1[1] * p_r2[2];
256  const double Ay = u1[2] * p_r2[0] - u1[0] * p_r2[2];
257  const double Az = -u1[1] * p_r2[0] + u1[0] * p_r2[1];
258  // B
259  const double B = sqrt(aux_rw);
260  const double Bx = (-u1[2] * r_w[1] + u1[1] * r_w[2]) / B;
261  const double By = (u1[2] * r_w[0] + u1[0] * r_w[2]) / B;
262  const double Bz = (-u1[1] * r_w[0] + u1[0] * r_w[1]) / B;
263 
264 #if 0
265  std::cout << "\nA: " << A << "\nAx: " << Ax << "\nAy: " << Ay
266  << "\nAz: " << Az << "\nB: " << B << "\nBx: " << Bx
267  << "\nBy: " << By << "\nBz: " << Bz << "\n";
268 #endif
269 
270  // Ec.36
271  // clang-format off
272  Eigen::Matrix<double, 1, 3> J1_2 =
273  (Eigen::Matrix<double, 1, 3>() <<
274  (Ax * B - A * Bx) / mrpt::square(B),
275  (Ay * B - A * By) / mrpt::square(B),
276  (Az * B - A * Bz) / mrpt::square(B)
277  ).finished();
278  // clang-format on
279 
280  // J1.3: Ec.37-38
281  // clang-format off
282  Eigen::Matrix<double, 3, 6> J1_3 =
283  (Eigen::Matrix<double, 3, 6>() <<
284  0, 0, 0, 1, 0, 0,
285  0, 0, 0, 0, 1, 0,
286  0, 0, 0, 0, 0, 1
287  ).finished();
288  // clang-format on
289 
290  // J1: Ec.29
291  Eigen::Matrix<double, 4, 6> J1;
292  J1.block<1, 3>(0, 0) = J1_1;
293  J1.block<1, 3>(0, 3) = J1_2;
294  J1.block<3, 6>(1, 0) = J1_3;
295 
296  // J2: Ec.39-41
297  // clang-format off
298  const Eigen::Matrix<double, 6, 12> J2 =
299  (Eigen::Matrix<double, 6, 12>() <<
300  p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0, 0,
301  0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0,
302  0, 0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1,
303  -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0, 0, 0,
304  0, -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0, 0,
305  0, 0, -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0
306  ).finished();
307  // clang-format on
308  // Build Jacobian
309  mrpt::math::CMatrixFixed<double, 4, 12>& J_aux = jacobian.value().get();
310  J_aux.block<4, 12>(0, 0) = J1 * J2;
311 
312  std::cout << "\nJ1:\n" << J1 << "\nJ2:\n" << J2 << "\n";
313  }
314  }
315  // std::cout<<"\nError:\n"<<error;
316  return error;
317  MRPT_END
318 }
319 
320 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_plane2plane(
321  const mp2p_icp::matched_plane_t& pairing, const mrpt::poses::CPose3D& relativePose,
322  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
323 {
324  MRPT_START
325  mrpt::math::CVectorFixedDouble<3> error;
326 
327  const auto nl = pairing.p_local.plane.getNormalVector();
328  const auto ng = pairing.p_global.plane.getNormalVector();
329 
330  const auto p_oplus_nl = relativePose.rotateVector(nl);
331 
332  for (int i = 0; i < 3; i++) error[i] = p_oplus_nl[i] - ng[i];
333 
334  if (jacobian)
335  {
336  // Eval Jacobian:
337 
338  // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
339  // "A tutorial on SE(3) transformation parameterizations and
340  // on-manifold optimization"
341  // Modified, to discard the last I_3 block, since this particular
342  // cost function is insensible to translations.
343 
344  // clang-format off
345  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
346  J_aux = (Eigen::Matrix<double, 3, 12>() <<
347  nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0, 0, 0,
348  0, nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0, 0,
349  0, 0, nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0
350  ).finished();
351  // clang-format on
352  }
353  return error;
354  MRPT_END
355 }
mp2p_icp
Definition: covariance.h:17
mp2p_icp::matched_plane_t::p_global
plane_patch_t p_global
Definition: Pairings.h:31
mp2p_icp::point_plane_pair_t
Definition: point_plane_pair_t.h:26
mp2p_icp::point_plane_pair_t::pl_global
plane_patch_t pl_global
Definition: point_plane_pair_t.h:28
mp2p_icp::matched_plane_t::p_local
plane_patch_t p_local
Definition: Pairings.h:31
mp2p_icp::point_line_pair_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:55
optimal_tf_gauss_newton.h
Simple non-linear optimizer to find the SE(3) optimal transformation.
errorTerms.h
mp2p_icp::error_plane2plane
mrpt::math::CVectorFixedDouble< 3 > error_plane2plane(const mp2p_icp::matched_plane_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:320
mp2p_icp::matched_line_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:46
mp2p_icp::matched_line_t
Definition: Pairings.h:44
mp2p_icp::error_point2plane
mrpt::math::CVectorFixedDouble< 3 > error_point2plane(const mp2p_icp::point_plane_pair_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:107
mp2p_icp::error_point2point
mrpt::math::CVectorFixedDouble< 3 > error_point2point(const mrpt::tfest::TMatchingPair &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:28
mp2p_icp::point_line_pair_t
Definition: Pairings.h:53
mp2p_icp::point_line_pair_t::pt_local
mrpt::math::TPoint3D pt_local
Definition: Pairings.h:56
mp2p_icp::point_plane_pair_t::pt_local
mrpt::math::TPoint3Df pt_local
Definition: point_plane_pair_t.h:29
mp2p_icp::matched_plane_t
Definition: Pairings.h:29
mp2p_icp::matched_line_t::ln_local
mrpt::math::TLine3D ln_local
Definition: Pairings.h:46
mp2p_icp::plane_patch_t::plane
mrpt::math::TPlane plane
Definition: plane_patch.h:24
test.q
q
Definition: test.py:8
mp2p_icp::error_point2line
mrpt::math::CVectorFixedDouble< 3 > error_point2line(const mp2p_icp::point_line_pair_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:60
mp2p_icp::error_line2line
mrpt::math::CVectorFixedDouble< 4 > error_line2line(const mp2p_icp::matched_line_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:155

mp2p_icp
Author(s):
autogenerated on Mon May 26 2025 02:45:48