| 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/config.h> |
| 13 |
|
| 14 |
#include "se2_l2_internal.h" |
| 15 |
|
| 16 |
#if MRPT_ARCH_INTEL_COMPATIBLE |
| 17 |
|
| 18 |
#include <mrpt/core/SSE_types.h> |
| 19 |
#include <mrpt/core/exceptions.h> |
| 20 |
|
| 21 |
using namespace mrpt::tfest; |
| 22 |
|
| 23 |
internal::se2_l2_impl_return_t<float> internal::se2_l2_impl_SSE2( |
| 24 |
const TMatchingPairList& in_correspondences) |
| 25 |
{ |
| 26 |
// SSE vectorized version: |
| 27 |
const size_t N = in_correspondences.size(); |
| 28 |
ASSERT_(N >= 2); |
| 29 |
const float N_inv = 1.0f / N; // For efficiency, keep this value. |
| 30 |
|
| 31 |
// Ensure correct types: |
| 32 |
static_assert(sizeof(TMatchingPair::global.x) == sizeof(float)); |
| 33 |
static_assert(sizeof(TMatchingPair::local.x) == sizeof(float)); |
| 34 |
|
| 35 |
internal::se2_l2_impl_return_t<float> ret; |
| 36 |
|
| 37 |
__m128 sum_a_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) |
| 38 |
__m128 sum_b_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) |
| 39 |
|
| 40 |
// [ f0 f1 f2 f3 ] |
| 41 |
// xa*xb ya*yb xa*yb xb*ya |
| 42 |
__m128 sum_ab_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) |
| 43 |
|
| 44 |
for (const auto& in_correspondence : in_correspondences) |
| 45 |
{ |
| 46 |
// Get the pair of points in the correspondence: |
| 47 |
// a_xyyx = [ xa ay | xa ya ] |
| 48 |
// b_xyyx = [ xb yb | yb xb ] |
| 49 |
// (product) |
| 50 |
// [ xa*xb ya*yb xa*yb xb*ya |
| 51 |
// LO0 LO1 HI2 HI3 |
| 52 |
// Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0) |
| 53 |
const __m128 a_xyz = _mm_loadu_ps(&in_correspondence.global.x); // *Unaligned* load |
| 54 |
const __m128 b_xyz = _mm_loadu_ps(&in_correspondence.local.x); // *Unaligned* load |
| 55 |
|
| 56 |
const auto a_xyxy = _mm_shuffle_ps(a_xyz, a_xyz, _MM_SHUFFLE(1, 0, 1, 0)); |
| 57 |
const auto b_xyyx = _mm_shuffle_ps(b_xyz, b_xyz, _MM_SHUFFLE(0, 1, 1, 0)); |
| 58 |
|
| 59 |
// Compute the terms: |
| 60 |
sum_a_xyz = _mm_add_ps(sum_a_xyz, a_xyz); |
| 61 |
sum_b_xyz = _mm_add_ps(sum_b_xyz, b_xyz); |
| 62 |
|
| 63 |
// [ f0 f1 f2 f3 ] |
| 64 |
// xa*xb ya*yb xa*yb xb*ya |
| 65 |
sum_ab_xyz = _mm_add_ps(sum_ab_xyz, _mm_mul_ps(a_xyxy, b_xyyx)); |
| 66 |
} |
| 67 |
|
| 68 |
alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float sums_a[4], sums_b[4]; |
| 69 |
_mm_store_ps(sums_a, sum_a_xyz); |
| 70 |
_mm_store_ps(sums_b, sum_b_xyz); |
| 71 |
|
| 72 |
float SumXa = sums_a[0]; |
| 73 |
float SumYa = sums_a[1]; |
| 74 |
float SumXb = sums_b[0]; |
| 75 |
float SumYb = sums_b[1]; |
| 76 |
|
| 77 |
// Compute all four means: |
| 78 |
const __m128 Ninv_4val = _mm_set1_ps(N_inv); // load 4 copies of the same value |
| 79 |
sum_a_xyz = _mm_mul_ps(sum_a_xyz, Ninv_4val); |
| 80 |
sum_b_xyz = _mm_mul_ps(sum_b_xyz, Ninv_4val); |
| 81 |
|
| 82 |
// means_a[0]: mean_x_a |
| 83 |
// means_a[1]: mean_y_a |
| 84 |
// means_b[0]: mean_x_b |
| 85 |
// means_b[1]: mean_y_b |
| 86 |
alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float means_a[4], means_b[4]; |
| 87 |
_mm_store_ps(means_a, sum_a_xyz); |
| 88 |
_mm_store_ps(means_b, sum_b_xyz); |
| 89 |
|
| 90 |
ret.mean_x_a = means_a[0]; |
| 91 |
ret.mean_y_a = means_a[1]; |
| 92 |
ret.mean_x_b = means_b[0]; |
| 93 |
ret.mean_y_b = means_b[1]; |
| 94 |
|
| 95 |
// Sxx Syy Sxy Syx |
| 96 |
// xa*xb ya*yb xa*yb xb*ya |
| 97 |
alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float cross_sums[4]; |
| 98 |
_mm_store_ps(cross_sums, sum_ab_xyz); |
| 99 |
|
| 100 |
float Sxx = cross_sums[0]; |
| 101 |
float Syy = cross_sums[1]; |
| 102 |
float Sxy = cross_sums[2]; |
| 103 |
float Syx = cross_sums[3]; |
| 104 |
|
| 105 |
// Auxiliary variables Ax,Ay: |
| 106 |
ret.Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb; |
| 107 |
ret.Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa; |
| 108 |
|
| 109 |
return ret; |
| 110 |
} |
| 111 |
|
| 112 |
#endif // MRPT_ARCH_INTEL_COMPATIBLE |