Debian Codegraph @

libs/tfest/src/se2_l2.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "tfest-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/core/cpu.h>
13 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/poses/CPosePDFGaussian.h>
15 #include <mrpt/random.h>
16 #include <mrpt/tfest/se2.h>
17
18 #include "se2_l2_internal.h"
19
20 using namespace mrpt;
21 using namespace mrpt::tfest;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 using namespace std;
25
26 // Wrapper:
27 bool tfest::se2_l2(
28 const TMatchingPairList& in_correspondences, CPosePDFGaussian& out_transformation)
29 {
30 mrpt::math::TPose2D p;
31 const bool ret = tfest::se2_l2(in_correspondences, p, &out_transformation.cov);
32 out_transformation.mean = CPose2D(p);
33 return ret;
34 }
35
36 // Non-vectorized version
37 namespace
38 {
39 mrpt::tfest::internal::se2_l2_impl_return_t<float> se2_l2_impl(
40 const TMatchingPairList& in_correspondences)
41 {
42 // SSE vectorized version:
43 const size_t N = in_correspondences.size();
44 ASSERT_(N >= 2);
45 const float N_inv = 1.0f / N; // For efficiency, keep this value.
46
47 // Non vectorized version:
48 float SumXa = 0, SumXb = 0, SumYa = 0, SumYb = 0;
49 float Sxx = 0, Sxy = 0, Syx = 0, Syy = 0;
50
51 for (const auto& p : in_correspondences)
52 {
53 // Get the pair of points in the correspondence:
54 const float xa = p.global.x;
55 const float ya = p.global.y;
56 const float xb = p.local.x;
57 const float yb = p.local.y;
58
59 // Compute the terms:
60 SumXa += xa;
61 SumYa += ya;
62
63 SumXb += xb;
64 SumYb += yb;
65
66 Sxx += xa * xb;
67 Sxy += xa * yb;
68 Syx += ya * xb;
69 Syy += ya * yb;
70 } // End of "for all correspondences"...
71
72 mrpt::tfest::internal::se2_l2_impl_return_t<float> ret;
73 ret.mean_x_a = SumXa * N_inv;
74 ret.mean_y_a = SumYa * N_inv;
75 ret.mean_x_b = SumXb * N_inv;
76 ret.mean_y_b = SumYb * N_inv;
77
78 // Auxiliary variables Ax,Ay:
79 ret.Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb;
80 ret.Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa;
81
82 return ret;
83 }
84 } // namespace
85
86 /*---------------------------------------------------------------
87 leastSquareErrorRigidTransformation
88
89 Compute the best transformation (x,y,phi) given a set of
90 correspondences between 2D points in two different maps.
91 This method is intensively used within ICP.
92 ---------------------------------------------------------------*/
93 bool tfest::se2_l2(
94 const TMatchingPairList& in_correspondences,
95 TPose2D& out_transformation,
96 CMatrixDouble33* out_estimateCovariance)
97 {
98 MRPT_START
99
100 const size_t N = in_correspondences.size();
101
102 if (N < 2) return false;
103
104 const float N_inv = 1.0f / N; // For efficiency, keep this value.
105
106 // ----------------------------------------------------------------------
107 // Compute the estimated pose. Notation from the paper:
108 // "Mobile robot motion estimation by 2d scan matching with genetic and
109 // iterative
110 // closest point algorithms", J.L. Martinez Rodriguez, A.J. Gonzalez, J.
111 // Morales Rodriguez, A. Mandow Andaluz, A. J. Garcia Cerezo, Journal of
112 // Field Robotics, 2006.
113 // ----------------------------------------------------------------------
114
115 // ----------------------------------------------------------------------
116 // For the formulas of the covariance, see:
117 // https://www.mrpt.org/Paper:Occupancy_Grid_Matching
118 // and Jose Luis Blanco's PhD thesis.
119 // ----------------------------------------------------------------------
120 internal::se2_l2_impl_return_t<float> implRet;
121 #if MRPT_ARCH_INTEL_COMPATIBLE
122 if (mrpt::cpu::supports(mrpt::cpu::feature::SSE2))
123 {
124 implRet = mrpt::tfest::internal::se2_l2_impl_SSE2(in_correspondences);
125 }
126 else
127 #endif
128 {
129 implRet = se2_l2_impl(in_correspondences);
130 }
131
132 out_transformation.phi =
133 (implRet.Ax != 0 || implRet.Ay != 0)
134 ? atan2(static_cast<double>(implRet.Ay), static_cast<double>(implRet.Ax))
135 : 0.0;
136
137 const double ccos = cos(out_transformation.phi);
138 const double csin = sin(out_transformation.phi);
139
140 out_transformation.x = implRet.mean_x_a - implRet.mean_x_b * ccos + implRet.mean_y_b * csin;
141 out_transformation.y = implRet.mean_y_a - implRet.mean_x_b * csin - implRet.mean_y_b * ccos;
142
143 if (out_estimateCovariance)
144 {
145 CMatrixDouble33* C = out_estimateCovariance; // less typing!
146
147 // Compute the normalized covariance matrix:
148 // -------------------------------------------
149 double var_x_a = 0, var_y_a = 0, var_x_b = 0, var_y_b = 0;
150 const double N_1_inv = 1.0 / (N - 1);
151
152 // 0) Precompute the unbiased variances estimations:
153 // ----------------------------------------------------
154 for (const auto& in_correspondence : in_correspondences)
155 {
156 var_x_a += square(in_correspondence.global.x - implRet.mean_x_a);
157 var_y_a += square(in_correspondence.global.y - implRet.mean_y_a);
158 var_x_b += square(in_correspondence.local.x - implRet.mean_x_b);
159 var_y_b += square(in_correspondence.local.y - implRet.mean_y_b);
160 }
161 var_x_a *= N_1_inv; // /= (N-1)
162 var_y_a *= N_1_inv;
163 var_x_b *= N_1_inv;
164 var_y_b *= N_1_inv;
165
166 // 1) Compute BETA = s_Delta^2 / s_p^2
167 // --------------------------------
168 const double BETA = (var_x_a + var_y_a + var_x_b + var_y_b) * pow(static_cast<double>(N), 2.0) *
169 static_cast<double>(N - 1);
170
171 // 2) And the final covariance matrix:
172 // (remember: this matrix has yet to be
173 // multiplied by var_p to be the actual covariance!)
174 // -------------------------------------------------------
175 const double D = square(implRet.Ax) + square(implRet.Ay);
176
177 (*C)(0, 0) = 2.0 * N_inv +
178 BETA * square((implRet.mean_x_b * implRet.Ay + implRet.mean_y_b * implRet.Ax) / D);
179 (*C)(1, 1) = 2.0 * N_inv +
180 BETA * square((implRet.mean_x_b * implRet.Ax - implRet.mean_y_b * implRet.Ay) / D);
181 (*C)(2, 2) = BETA / D;
182
183 (*C)(0, 1) = (*C)(1, 0) =
184 -BETA * (implRet.mean_x_b * implRet.Ay + implRet.mean_y_b * implRet.Ax) *
185 (implRet.mean_x_b * implRet.Ax - implRet.mean_y_b * implRet.Ay) / square(D);
186
187 (*C)(0, 2) = (*C)(2, 0) =
188 BETA * (implRet.mean_x_b * implRet.Ay + implRet.mean_y_b * implRet.Ax) / pow(D, 1.5);
189
190 (*C)(1, 2) = (*C)(2, 1) =
191 BETA * (implRet.mean_y_b * implRet.Ay - implRet.mean_x_b * implRet.Ax) / pow(D, 1.5);
192 }
193
194 return true;
195
196 MRPT_END
197 }