/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "tfest-precomp.h" // Precompiled headers // #include #include "se2_l2_internal.h" #if MRPT_ARCH_INTEL_COMPATIBLE #include #include using namespace mrpt::tfest; internal::se2_l2_impl_return_t internal::se2_l2_impl_SSE2( const TMatchingPairList& in_correspondences) { // SSE vectorized version: const size_t N = in_correspondences.size(); ASSERT_(N >= 2); const float N_inv = 1.0f / N; // For efficiency, keep this value. // Ensure correct types: static_assert(sizeof(TMatchingPair::global.x) == sizeof(float)); static_assert(sizeof(TMatchingPair::local.x) == sizeof(float)); internal::se2_l2_impl_return_t ret; __m128 sum_a_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) __m128 sum_b_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) // [ f0 f1 f2 f3 ] // xa*xb ya*yb xa*yb xb*ya __m128 sum_ab_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) for (const auto& in_correspondence : in_correspondences) { // Get the pair of points in the correspondence: // a_xyyx = [ xa ay | xa ya ] // b_xyyx = [ xb yb | yb xb ] // (product) // [ xa*xb ya*yb xa*yb xb*ya // LO0 LO1 HI2 HI3 // Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0) const __m128 a_xyz = _mm_loadu_ps(&in_correspondence.global.x); // *Unaligned* load const __m128 b_xyz = _mm_loadu_ps(&in_correspondence.local.x); // *Unaligned* load const auto a_xyxy = _mm_shuffle_ps(a_xyz, a_xyz, _MM_SHUFFLE(1, 0, 1, 0)); const auto b_xyyx = _mm_shuffle_ps(b_xyz, b_xyz, _MM_SHUFFLE(0, 1, 1, 0)); // Compute the terms: sum_a_xyz = _mm_add_ps(sum_a_xyz, a_xyz); sum_b_xyz = _mm_add_ps(sum_b_xyz, b_xyz); // [ f0 f1 f2 f3 ] // xa*xb ya*yb xa*yb xb*ya sum_ab_xyz = _mm_add_ps(sum_ab_xyz, _mm_mul_ps(a_xyxy, b_xyyx)); } alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float sums_a[4], sums_b[4]; _mm_store_ps(sums_a, sum_a_xyz); _mm_store_ps(sums_b, sum_b_xyz); float SumXa = sums_a[0]; float SumYa = sums_a[1]; float SumXb = sums_b[0]; float SumYb = sums_b[1]; // Compute all four means: const __m128 Ninv_4val = _mm_set1_ps(N_inv); // load 4 copies of the same value sum_a_xyz = _mm_mul_ps(sum_a_xyz, Ninv_4val); sum_b_xyz = _mm_mul_ps(sum_b_xyz, Ninv_4val); // means_a[0]: mean_x_a // means_a[1]: mean_y_a // means_b[0]: mean_x_b // means_b[1]: mean_y_b alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float means_a[4], means_b[4]; _mm_store_ps(means_a, sum_a_xyz); _mm_store_ps(means_b, sum_b_xyz); ret.mean_x_a = means_a[0]; ret.mean_y_a = means_a[1]; ret.mean_x_b = means_b[0]; ret.mean_y_b = means_b[1]; // Sxx Syy Sxy Syx // xa*xb ya*yb xa*yb xb*ya alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float cross_sums[4]; _mm_store_ps(cross_sums, sum_ab_xyz); float Sxx = cross_sums[0]; float Syy = cross_sums[1]; float Sxy = cross_sums[2]; float Syx = cross_sums[3]; // Auxiliary variables Ax,Ay: ret.Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb; ret.Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa; return ret; } #endif // MRPT_ARCH_INTEL_COMPATIBLE