MRPT  1.9.9
se2_l2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 #include "tfest-precomp.h" // Precompiled headers
11
12 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/random.h>
15 #include <mrpt/tfest/se2.h>
16
17 #if MRPT_HAS_SSE2
18 #include <mrpt/core/SSE_types.h>
19 #endif
20
21 using namespace mrpt;
22 using namespace mrpt::tfest;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace std;
26
27 // Wrapper:
29  const TMatchingPairList& in_correspondences,
30  CPosePDFGaussian& out_transformation)
31 {
33  const bool ret =
34  tfest::se2_l2(in_correspondences, p, &out_transformation.cov);
35  out_transformation.mean = CPose2D(p);
36  return ret;
37 }
38
39 /*---------------------------------------------------------------
40  leastSquareErrorRigidTransformation
41
42  Compute the best transformation (x,y,phi) given a set of
43  correspondences between 2D points in two different maps.
44  This method is intensively used within ICP.
45  ---------------------------------------------------------------*/
47  const TMatchingPairList& in_correspondences, TPose2D& out_transformation,
48  CMatrixDouble33* out_estimateCovariance)
49 {
51
52  const size_t N = in_correspondences.size();
53
54  if (N < 2) return false;
55
56  const float N_inv = 1.0f / N; // For efficiency, keep this value.
57
58 // ----------------------------------------------------------------------
59 // Compute the estimated pose. Notation from the paper:
60 // "Mobile robot motion estimation by 2d scan matching with genetic and
61 // iterative
62 // closest point algorithms", J.L. Martinez Rodriguez, A.J. Gonzalez, J. Morales
63 // Rodriguez, A. Mandow Andaluz, A. J. Garcia Cerezo,
64 // Journal of Field Robotics, 2006.
65 // ----------------------------------------------------------------------
66
67 // ----------------------------------------------------------------------
68 // For the formulas of the covariance, see:
69 // https://www.mrpt.org/Paper:Occupancy_Grid_Matching
70 // and Jose Luis Blanco's PhD thesis.
71 // ----------------------------------------------------------------------
72 #if MRPT_HAS_SSE2
73  // SSE vectorized version:
74
75  //{
76  // TMatchingPair dumm;
77  // static_assert(sizeof(dumm.this_x)==sizeof(float));
78  // static_assert(sizeof(dumm.other_x)==sizeof(float));
79  //}
80
81  __m128 sum_a_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
82  __m128 sum_b_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
83
84  // [ f0 f1 f2 f3 ]
85  // xa*xb ya*yb xa*yb xb*ya
86  __m128 sum_ab_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
87
88  for (const auto& in_correspondence : in_correspondences)
89  {
90  // Get the pair of points in the correspondence:
91  // a_xyyx = [ xa ay | xa ya ]
92  // b_xyyx = [ xb yb | yb xb ]
93  // (product)
94  // [ xa*xb ya*yb xa*yb xb*ya
95  // LO0 LO1 HI2 HI3
96  // Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0)
97  const __m128 a_xyz =
99  const __m128 b_xyz =
101
102  const auto a_xyxy =
103  _mm_shuffle_ps(a_xyz, a_xyz, _MM_SHUFFLE(1, 0, 1, 0));
104  const auto b_xyyx =
105  _mm_shuffle_ps(b_xyz, b_xyz, _MM_SHUFFLE(0, 1, 1, 0));
106
107  // Compute the terms:
110
111  // [ f0 f1 f2 f3 ]
112  // xa*xb ya*yb xa*yb xb*ya
113  sum_ab_xyz = _mm_add_ps(sum_ab_xyz, _mm_mul_ps(a_xyxy, b_xyyx));
114  }
115
116  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float sums_a[4], sums_b[4];
117  _mm_store_ps(sums_a, sum_a_xyz);
118  _mm_store_ps(sums_b, sum_b_xyz);
119
120  const float& SumXa = sums_a[0];
121  const float& SumYa = sums_a[1];
122  const float& SumXb = sums_b[0];
123  const float& SumYb = sums_b[1];
124
125  // Compute all four means:
126  const __m128 Ninv_4val =
127  _mm_set1_ps(N_inv); // load 4 copies of the same value
128  sum_a_xyz = _mm_mul_ps(sum_a_xyz, Ninv_4val);
129  sum_b_xyz = _mm_mul_ps(sum_b_xyz, Ninv_4val);
130
131  // means_a[0]: mean_x_a
132  // means_a[1]: mean_y_a
133  // means_b[0]: mean_x_b
134  // means_b[1]: mean_y_b
135  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float means_a[4], means_b[4];
136  _mm_store_ps(means_a, sum_a_xyz);
137  _mm_store_ps(means_b, sum_b_xyz);
138
139  const float& mean_x_a = means_a[0];
140  const float& mean_y_a = means_a[1];
141  const float& mean_x_b = means_b[0];
142  const float& mean_y_b = means_b[1];
143
144  // Sxx Syy Sxy Syx
145  // xa*xb ya*yb xa*yb xb*ya
146  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float cross_sums[4];
147  _mm_store_ps(cross_sums, sum_ab_xyz);
148
149  const float& Sxx = cross_sums[0];
150  const float& Syy = cross_sums[1];
151  const float& Sxy = cross_sums[2];
152  const float& Syx = cross_sums[3];
153
154  // Auxiliary variables Ax,Ay:
155  const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb;
156  const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa;
157
158 #else
159  // Non vectorized version:
160  float SumXa = 0, SumXb = 0, SumYa = 0, SumYb = 0;
161  float Sxx = 0, Sxy = 0, Syx = 0, Syy = 0;
162
163  for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin();
164  corrIt != in_correspondences.end(); corrIt++)
165  {
166  // Get the pair of points in the correspondence:
167  const float xa = corrIt->this_x;
168  const float ya = corrIt->this_y;
169  const float xb = corrIt->other_x;
170  const float yb = corrIt->other_y;
171
172  // Compute the terms:
173  SumXa += xa;
174  SumYa += ya;
175
176  SumXb += xb;
177  SumYb += yb;
178
179  Sxx += xa * xb;
180  Sxy += xa * yb;
181  Syx += ya * xb;
182  Syy += ya * yb;
183  } // End of "for all correspondences"...
184
185  const float mean_x_a = SumXa * N_inv;
186  const float mean_y_a = SumYa * N_inv;
187  const float mean_x_b = SumXb * N_inv;
188  const float mean_y_b = SumYb * N_inv;
189
190  // Auxiliary variables Ax,Ay:
191  const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb;
192  const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa;
193
194 #endif
195
196  out_transformation.phi =
197  (Ax != 0 || Ay != 0)
198  ? atan2(static_cast<double>(Ay), static_cast<double>(Ax))
199  : 0.0;
200
201  const double ccos = cos(out_transformation.phi);
202  const double csin = sin(out_transformation.phi);
203
204  out_transformation.x = mean_x_a - mean_x_b * ccos + mean_y_b * csin;
205  out_transformation.y = mean_y_a - mean_x_b * csin - mean_y_b * ccos;
206
207  if (out_estimateCovariance)
208  {
209  CMatrixDouble33* C = out_estimateCovariance; // less typing!
210
211  // Compute the normalized covariance matrix:
212  // -------------------------------------------
213  double var_x_a = 0, var_y_a = 0, var_x_b = 0, var_y_b = 0;
214  const double N_1_inv = 1.0 / (N - 1);
215
216  // 0) Precompute the unbiased variances estimations:
217  // ----------------------------------------------------
218  for (const auto& in_correspondence : in_correspondences)
219  {
220  var_x_a += square(in_correspondence.this_x - mean_x_a);
221  var_y_a += square(in_correspondence.this_y - mean_y_a);
222  var_x_b += square(in_correspondence.other_x - mean_x_b);
223  var_y_b += square(in_correspondence.other_y - mean_y_b);
224  }
225  var_x_a *= N_1_inv; // /= (N-1)
226  var_y_a *= N_1_inv;
227  var_x_b *= N_1_inv;
228  var_y_b *= N_1_inv;
229
230  // 1) Compute BETA = s_Delta^2 / s_p^2
231  // --------------------------------
232  const double BETA = (var_x_a + var_y_a + var_x_b + var_y_b) *
233  pow(static_cast<double>(N), 2.0) *
234  static_cast<double>(N - 1);
235
236  // 2) And the final covariance matrix:
237  // (remember: this matrix has yet to be
238  // multiplied by var_p to be the actual covariance!)
239  // -------------------------------------------------------
240  const double D = square(Ax) + square(Ay);
241
242  (*C)(0, 0) =
243  2.0 * N_inv + BETA * square((mean_x_b * Ay + mean_y_b * Ax) / D);
244  (*C)(1, 1) =
245  2.0 * N_inv + BETA * square((mean_x_b * Ax - mean_y_b * Ay) / D);
246  (*C)(2, 2) = BETA / D;
247
248  (*C)(0, 1) = (*C)(1, 0) = -BETA * (mean_x_b * Ay + mean_y_b * Ax) *
249  (mean_x_b * Ax - mean_y_b * Ay) / square(D);
250
251  (*C)(0, 2) = (*C)(2, 0) =
252  BETA * (mean_x_b * Ay + mean_y_b * Ax) / pow(D, 1.5);
253
254  (*C)(1, 2) = (*C)(2, 1) =
255  BETA * (mean_y_b * Ay - mean_x_b * Ax) / pow(D, 1.5);
256  }
257
258  return true;
259
260  MRPT_END
261 }
#define MRPT_START
Definition: exceptions.h:241
CPose2D mean
The mean value.
double x
X,Y coordinates.
Definition: TPose2D.h:30
bool se2_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:46
STL namespace.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:70
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
#define MRPT_END
Definition: exceptions.h:245
Lightweight 2D pose.
Definition: TPose2D.h:22
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6398
double phi