| 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 |
} |