Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, 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/utils/SSE_types.h>
18 #endif
19
20 using namespace mrpt;
21 using namespace mrpt::tfest;
22 using namespace mrpt::utils;
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 = 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,
47  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 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(); corrIt!=in_correspondences.end(); corrIt++)
88  {
89  // Get the pair of points in the correspondence:
90  // a_xyyx = [ xa ay | xa ya ]
91  // b_xyyx = [ xb yb | yb xb ]
92  // (product)
93  // [ xa*xb ya*yb xa*yb xb*ya
94  // LO0 LO1 HI2 HI3
95  // Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0)
98
99  const __m128 a_xyxy = _mm_shuffle_ps(a_xyz,a_xyz, _MM_SHUFFLE(1,0,1,0) );
100  const __m128 b_xyyx = _mm_shuffle_ps(b_xyz,b_xyz, _MM_SHUFFLE(0,1,1,0) );
101
102  // Compute the terms:
105
106  // [ f0 f1 f2 f3 ]
107  // xa*xb ya*yb xa*yb xb*ya
109
110  }
111
112  MRPT_ALIGN16 float sums_a[4], sums_b[4];
113  _mm_store_ps(sums_a,sum_a_xyz);
114  _mm_store_ps(sums_b,sum_b_xyz);
115
116  const float &SumXa=sums_a[0];
117  const float &SumYa=sums_a[1];
118  const float &SumXb=sums_b[0];
119  const float &SumYb=sums_b[1];
120
121  // Compute all four means:
122  const __m128 Ninv_4val = _mm_set1_ps(N_inv); // load 4 copies of the same value
123  sum_a_xyz = _mm_mul_ps(sum_a_xyz,Ninv_4val);
124  sum_b_xyz = _mm_mul_ps(sum_b_xyz,Ninv_4val);
125
126
127  // means_a[0]: mean_x_a
128  // means_a[1]: mean_y_a
129  // means_b[0]: mean_x_b
130  // means_b[1]: mean_y_b
131  MRPT_ALIGN16 float means_a[4], means_b[4];
132  _mm_store_ps(means_a,sum_a_xyz);
133  _mm_store_ps(means_b,sum_b_xyz);
134
135  const float &mean_x_a = means_a[0];
136  const float &mean_y_a = means_a[1];
137  const float &mean_x_b = means_b[0];
138  const float &mean_y_b = means_b[1];
139
140  // Sxx Syy Sxy Syx
141  // xa*xb ya*yb xa*yb xb*ya
142  MRPT_ALIGN16 float cross_sums[4];
143  _mm_store_ps(cross_sums,sum_ab_xyz);
144
145  const float &Sxx = cross_sums[0];
146  const float &Syy = cross_sums[1];
147  const float &Sxy = cross_sums[2];
148  const float &Syx = cross_sums[3];
149
150  // Auxiliary variables Ax,Ay:
151  const float Ax = N*(Sxx + Syy) - SumXa*SumXb - SumYa*SumYb;
152  const float Ay = SumXa * SumYb + N*(Syx-Sxy)- SumXb * SumYa;
153
154 #else
155  // Non vectorized version:
156  float SumXa=0, SumXb=0, SumYa=0, SumYb=0;
157  float Sxx=0, Sxy=0, Syx=0, Syy=0;
158
159  for (TMatchingPairList::const_iterator corrIt=in_correspondences.begin(); corrIt!=in_correspondences.end(); corrIt++)
160  {
161  // Get the pair of points in the correspondence:
162  const float xa = corrIt->this_x;
163  const float ya = corrIt->this_y;
164  const float xb = corrIt->other_x;
165  const float yb = corrIt->other_y;
166
167  // Compute the terms:
168  SumXa+=xa;
169  SumYa+=ya;
170
171  SumXb += xb;
172  SumYb += yb;
173
174  Sxx += xa * xb;
175  Sxy += xa * yb;
176  Syx += ya * xb;
177  Syy += ya * yb;
178  } // End of "for all correspondences"...
179
180  const float mean_x_a = SumXa * N_inv;
181  const float mean_y_a = SumYa * N_inv;
182  const float mean_x_b = SumXb * N_inv;
183  const float mean_y_b = SumYb * N_inv;
184
185  // Auxiliary variables Ax,Ay:
186  const float Ax = N*(Sxx + Syy) - SumXa*SumXb - SumYa*SumYb;
187  const float Ay = SumXa * SumYb + N*(Syx-Sxy)- SumXb * SumYa;
188
189 #endif
190
191
192  out_transformation.phi = (Ax!=0 || Ay!=0) ? atan2( static_cast<double>(Ay), static_cast<double>(Ax)) : 0.0;
193
194  const double ccos = cos( out_transformation.phi );
195  const double csin = sin( out_transformation.phi );
196
197  out_transformation.x = mean_x_a - mean_x_b * ccos + mean_y_b * csin;
198  out_transformation.y = mean_y_a - mean_x_b * csin - mean_y_b * ccos;
199
200  if ( out_estimateCovariance )
201  {
202  CMatrixDouble33 *C = out_estimateCovariance; // less typing!
203
204  // Compute the normalized covariance matrix:
205  // -------------------------------------------
206  double var_x_a = 0,var_y_a = 0,var_x_b = 0,var_y_b = 0;
207  const double N_1_inv = 1.0/(N-1);
208
209  // 0) Precompute the unbiased variances estimations:
210  // ----------------------------------------------------
211  for (TMatchingPairList::const_iterator corrIt=in_correspondences.begin(); corrIt!=in_correspondences.end(); corrIt++)
212  {
213  var_x_a += square( corrIt->this_x - mean_x_a );
214  var_y_a += square( corrIt->this_y - mean_y_a );
215  var_x_b += square( corrIt->other_x - mean_x_b );
216  var_y_b += square( corrIt->other_y - mean_y_b );
217  }
218  var_x_a *= N_1_inv; // /= (N-1)
219  var_y_a *= N_1_inv;
220  var_x_b *= N_1_inv;
221  var_y_b *= N_1_inv;
222
223  // 1) Compute BETA = s_Delta^2 / s_p^2
224  // --------------------------------
225  const double BETA = (var_x_a+var_y_a+var_x_b+var_y_b)*pow(static_cast<double>(N),2.0)*static_cast<double>(N-1);
226
227  // 2) And the final covariance matrix:
228  // (remember: this matrix has yet to be
229  // multiplied by var_p to be the actual covariance!)
230  // -------------------------------------------------------
231  const double D = square(Ax)+square(Ay);
232
233  C->get_unsafe(0,0) = 2.0*N_inv + BETA * square((mean_x_b*Ay+mean_y_b*Ax)/D);
234  C->get_unsafe(1,1) = 2.0*N_inv + BETA * square((mean_x_b*Ax-mean_y_b*Ay)/D);
235  C->get_unsafe(2,2) = BETA / D;
236
237  C->get_unsafe(0,1) =
238  C->get_unsafe(1,0) = -BETA*(mean_x_b*Ay+mean_y_b*Ax)*(mean_x_b*Ax-mean_y_b*Ay)/square(D);
239
240  C->get_unsafe(0,2) =
241  C->get_unsafe(2,0) = BETA*(mean_x_b*Ay+mean_y_b*Ax)/pow(D,1.5);
242
243  C->get_unsafe(1,2) =
244  C->get_unsafe(2,1) = BETA*(mean_y_b*Ay-mean_x_b*Ax)/pow(D,1.5);
245  }
246
247  return true;
248
249  MRPT_END
250 }
251
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
A list of TMatchingPair.
Definition: TMatchingPair.h:78
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double y
X,Y coordinates.
#define MRPT_START
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:36
Lightweight 2D pose.
bool TFEST_IMPEXP se2_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=NULL)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:45
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:5587
double phi