Debian Codegraph @

libs/tfest/src/se2_l2.SSE2.cpp

raw
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