36 #ifndef _vpMbtTukeyEstimator_h_
37 #define _vpMbtTukeyEstimator_h_
40 #include <visp3/core/vpColVector.h>
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
44 template <
typename T>
class vpMbtTukeyEstimator
47 void MEstimator(
const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
51 T getMedian(std::vector<T> &vec);
52 void MEstimator_impl(
const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
53 void MEstimator_impl_simd(
const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
54 void psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights);
55 void psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights);
57 std::vector<T> m_normres;
58 std::vector<T> m_residues;
79 #include <visp3/core/vpCPUFeatures.h>
81 #define USE_TRANSFORM 1
82 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && USE_TRANSFORM
83 #define HAVE_TRANSFORM 1
87 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
88 #include <emmintrin.h>
89 #define VISP_HAVE_SSE2 1
91 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
92 #include <pmmintrin.h>
93 #define VISP_HAVE_SSE3 1
95 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
96 #include <tmmintrin.h>
97 #define VISP_HAVE_SSSE3 1
101 #if defined _WIN32 && defined(_M_ARM64)
102 # define _ARM64_DISTINCT_NEON_TYPES
104 # include <arm_neon.h>
105 # define VISP_HAVE_NEON 1
106 #elif (defined(__ARM_NEON__) || defined (__ARM_NEON)) && defined(__aarch64__)
107 # include <arm_neon.h>
108 # define VISP_HAVE_NEON 1
111 #ifndef DOXYGEN_SHOULD_SKIP_THIS
116 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
117 auto AbsDiff = [](
const auto &a,
const auto &b) {
return std::fabs(a - b); };
119 template <
typename T>
struct AbsDiff :
public std::binary_function<T, T, T> {
120 T operator()(
const T a,
const T b)
const {
return std::fabs(a - b); }
126 template class vpMbtTukeyEstimator<float>;
127 template class vpMbtTukeyEstimator<double>;
132 inline __m128 abs_ps(__m128 x)
134 static const __m128 sign_mask = _mm_set1_ps(-0.f);
135 return _mm_andnot_ps(sign_mask, x);
140 template <
typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
143 int index = (int)(ceil(vec.size() / 2.0)) - 1;
144 std::nth_element(vec.begin(), vec.begin() + index, vec.end());
154 template <
typename T>
155 void vpMbtTukeyEstimator<T>::MEstimator_impl(
const std::vector<T> &residues, std::vector<T> &weights,
156 const T NoiseThreshold)
158 if (residues.empty()) {
162 m_residues = residues;
164 T med = getMedian(m_residues);
165 m_normres.resize(residues.size());
168 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
169 std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
171 std::transform(residues.begin(), residues.end(), m_normres.begin(),
172 std::bind(AbsDiff<T>(), std::placeholders::_1, med));
175 for (
size_t i = 0; i < m_residues.size(); i++) {
176 m_normres[i] = (std::fabs(residues[i] - med));
180 m_residues = m_normres;
181 T normmedian = getMedian(m_residues);
184 T sigma =
static_cast<T
>(1.4826 * normmedian);
188 if (sigma < NoiseThreshold) {
189 sigma = NoiseThreshold;
192 psiTukey(sigma, m_normres, weights);
196 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_simd(
const std::vector<float> &residues,
197 std::vector<float> &weights,
198 float NoiseThreshold)
200 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
201 if (residues.empty()) {
205 m_residues = residues;
207 float med = getMedian(m_residues);
208 m_normres.resize(residues.size());
212 __m128 med_128 = _mm_set_ps1(med);
214 float32x4_t med_128 = vdupq_n_f32(med);
217 if (m_residues.size() >= 4) {
218 for (i = 0; i <= m_residues.size() - 4; i += 4) {
220 __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
221 _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
223 float32x4_t residues_128 = vld1q_f32(residues.data() + i);
224 vst1q_f32(m_normres.data() + i, vabsq_f32(vsubq_f32(residues_128, med_128)));
229 for (; i < m_residues.size(); i++) {
230 m_normres[i] = (std::fabs(residues[i] - med));
233 m_residues = m_normres;
234 float normmedian = getMedian(m_residues);
237 float sigma = 1.4826f * normmedian;
241 if (sigma < NoiseThreshold) {
242 sigma = NoiseThreshold;
245 psiTukey(sigma, m_normres, weights);
249 (void)NoiseThreshold;
257 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_simd(
const std::vector<double> &residues,
258 std::vector<double> &weights,
259 double NoiseThreshold)
261 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
262 if (residues.empty()) {
266 m_residues = residues;
268 double med = getMedian(m_residues);
269 m_normres.resize(residues.size());
272 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
273 std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
275 std::transform(residues.begin(), residues.end(), m_normres.begin(),
276 std::bind(AbsDiff<double>(), std::placeholders::_1, med));
279 for (
size_t i = 0; i < m_residues.size(); i++) {
280 m_normres[i] = (std::fabs(residues[i] - med));
284 m_residues = m_normres;
285 double normmedian = getMedian(m_residues);
288 double sigma = 1.4826 * normmedian;
292 if (sigma < NoiseThreshold) {
293 sigma = NoiseThreshold;
296 psiTukey(sigma, m_normres, weights);
300 (void)NoiseThreshold;
308 inline void vpMbtTukeyEstimator<float>::MEstimator(
const std::vector<float> &residues, std::vector<float> &weights,
309 float NoiseThreshold)
312 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
317 MEstimator_impl_simd(residues, weights, NoiseThreshold);
319 MEstimator_impl(residues, weights, NoiseThreshold);
326 inline void vpMbtTukeyEstimator<double>::MEstimator(
const std::vector<double> &residues, std::vector<double> &weights,
327 double NoiseThreshold)
330 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
335 MEstimator_impl_simd(residues, weights, NoiseThreshold);
337 MEstimator_impl(residues, weights, NoiseThreshold);
343 template <
typename T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights)
345 double C = sig * 4.6851;
348 for (
unsigned int i = 0; i < (
unsigned int)x.size(); i++) {
349 double xi = x[i] / C;
367 double NoiseThreshold)
369 if (residues.
size() == 0) {
374 m_residues.reserve(residues.
size());
375 m_residues.insert(m_residues.end(), &residues.
data[0], &residues.
data[residues.
size()]);
377 double med = getMedian(m_residues);
379 m_normres.resize(residues.
size());
380 for (
size_t i = 0; i < m_residues.size(); i++) {
381 m_normres[i] = std::fabs(residues[(
unsigned int)i] - med);
384 m_residues = m_normres;
385 double normmedian = getMedian(m_residues);
388 double sigma = 1.4826 * normmedian;
392 if (sigma < NoiseThreshold) {
393 sigma = NoiseThreshold;
396 psiTukey(sigma, m_normres, weights);
404 double NoiseThreshold)
406 if (residues.
size() == 0) {
410 m_residues.resize(0);
411 m_residues.reserve(residues.
size());
412 for (
unsigned int i = 0; i < residues.
size(); i++) {
413 m_residues.push_back((
float)residues[i]);
416 float med = getMedian(m_residues);
418 m_normres.resize(residues.
size());
419 for (
size_t i = 0; i < m_residues.size(); i++) {
420 m_normres[i] = (float)std::fabs(residues[(
unsigned int)i] - med);
423 m_residues = m_normres;
424 float normmedian = getMedian(m_residues);
427 float sigma = 1.4826f * normmedian;
431 if (sigma < NoiseThreshold) {
432 sigma = (float)NoiseThreshold;
435 psiTukey(sigma, m_normres, weights);
441 template <
class T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights)
443 T C =
static_cast<T
>(4.6851) * sig;
444 weights.resize(x.size());
447 for (
size_t i = 0; i < x.size(); i++) {
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()