MRPT  2.0.5
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-2020, 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>
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:
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 // Non-vectorized version
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.this_x;
55  const float ya = p.this_y;
56  const float xb = p.other_x;
57  const float yb = p.other_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 
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 
85 /*---------------------------------------------------------------
86  leastSquareErrorRigidTransformation
87 
88  Compute the best transformation (x,y,phi) given a set of
89  correspondences between 2D points in two different maps.
90  This method is intensively used within ICP.
91  ---------------------------------------------------------------*/
93  const TMatchingPairList& in_correspondences, TPose2D& out_transformation,
94  CMatrixDouble33* out_estimateCovariance)
95 {
97 
98  const size_t N = in_correspondences.size();
99 
100  if (N < 2) return false;
101 
102  const float N_inv = 1.0f / N; // For efficiency, keep this value.
103 
104  // ----------------------------------------------------------------------
105  // Compute the estimated pose. Notation from the paper:
106  // "Mobile robot motion estimation by 2d scan matching with genetic and
107  // iterative
108  // closest point algorithms", J.L. Martinez Rodriguez, A.J. Gonzalez, J.
109  // Morales Rodriguez, A. Mandow Andaluz, A. J. Garcia Cerezo, Journal of
110  // Field Robotics, 2006.
111  // ----------------------------------------------------------------------
112 
113  // ----------------------------------------------------------------------
114  // For the formulas of the covariance, see:
115  // https://www.mrpt.org/Paper:Occupancy_Grid_Matching
116  // and Jose Luis Blanco's PhD thesis.
117  // ----------------------------------------------------------------------
119 #if MRPT_ARCH_INTEL_COMPATIBLE
121  {
122  implRet = mrpt::tfest::internal::se2_l2_impl_SSE2(in_correspondences);
123  }
124  else
125 #endif
126  {
127  implRet = se2_l2_impl(in_correspondences);
128  }
129 
130  out_transformation.phi = (implRet.Ax != 0 || implRet.Ay != 0)
131  ? atan2(
132  static_cast<double>(implRet.Ay),
133  static_cast<double>(implRet.Ax))
134  : 0.0;
135 
136  const double ccos = cos(out_transformation.phi);
137  const double csin = sin(out_transformation.phi);
138 
139  out_transformation.x =
140  implRet.mean_x_a - implRet.mean_x_b * ccos + implRet.mean_y_b * csin;
141  out_transformation.y =
142  implRet.mean_y_a - implRet.mean_x_b * csin - implRet.mean_y_b * ccos;
143 
144  if (out_estimateCovariance)
145  {
146  CMatrixDouble33* C = out_estimateCovariance; // less typing!
147 
148  // Compute the normalized covariance matrix:
149  // -------------------------------------------
150  double var_x_a = 0, var_y_a = 0, var_x_b = 0, var_y_b = 0;
151  const double N_1_inv = 1.0 / (N - 1);
152 
153  // 0) Precompute the unbiased variances estimations:
154  // ----------------------------------------------------
155  for (const auto& in_correspondence : in_correspondences)
156  {
157  var_x_a += square(in_correspondence.this_x - implRet.mean_x_a);
158  var_y_a += square(in_correspondence.this_y - implRet.mean_y_a);
159  var_x_b += square(in_correspondence.other_x - implRet.mean_x_b);
160  var_y_b += square(in_correspondence.other_y - implRet.mean_y_b);
161  }
162  var_x_a *= N_1_inv; // /= (N-1)
163  var_y_a *= N_1_inv;
164  var_x_b *= N_1_inv;
165  var_y_b *= N_1_inv;
166 
167  // 1) Compute BETA = s_Delta^2 / s_p^2
168  // --------------------------------
169  const double BETA = (var_x_a + var_y_a + var_x_b + var_y_b) *
170  pow(static_cast<double>(N), 2.0) *
171  static_cast<double>(N - 1);
172 
173  // 2) And the final covariance matrix:
174  // (remember: this matrix has yet to be
175  // multiplied by var_p to be the actual covariance!)
176  // -------------------------------------------------------
177  const double D = square(implRet.Ax) + square(implRet.Ay);
178 
179  (*C)(0, 0) = 2.0 * N_inv + BETA * square(
180  (implRet.mean_x_b * implRet.Ay +
181  implRet.mean_y_b * implRet.Ax) /
182  D);
183  (*C)(1, 1) = 2.0 * N_inv + BETA * square(
184  (implRet.mean_x_b * implRet.Ax -
185  implRet.mean_y_b * implRet.Ay) /
186  D);
187  (*C)(2, 2) = BETA / D;
188 
189  (*C)(0, 1) = (*C)(1, 0) =
190  -BETA *
191  (implRet.mean_x_b * implRet.Ay + implRet.mean_y_b * implRet.Ax) *
192  (implRet.mean_x_b * implRet.Ax - implRet.mean_y_b * implRet.Ay) /
193  square(D);
194 
195  (*C)(0, 2) = (*C)(2, 0) =
196  BETA *
197  (implRet.mean_x_b * implRet.Ay + implRet.mean_y_b * implRet.Ax) /
198  pow(D, 1.5);
199 
200  (*C)(1, 2) = (*C)(2, 1) =
201  BETA *
202  (implRet.mean_y_b * implRet.Ay - implRet.mean_x_b * implRet.Ax) /
203  pow(D, 1.5);
204  }
205 
206  return true;
207 
208  MRPT_END
209 }
#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:92
static mrpt::tfest::internal::se2_l2_impl_return_t< float > se2_l2_impl(const TMatchingPairList &in_correspondences)
Definition: se2_l2.cpp:39
STL namespace.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
bool supports(feature f) noexcept
Returns true if the current CPU (and OS) supports the given CPU feature.
Definition: cpu.h:75
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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...
return_t square(const num_t x)
Inline function for the square of a number.
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...
double phi
Orientation (rads)
Definition: TPose2D.h:32



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: e84511500 Thu Jul 2 23:53:34 2020 +0200 at vie jul 3 00:00:10 CEST 2020