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