Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpMbtTukeyEstimator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Tukey M-estimator.
33  *
34 *****************************************************************************/
35 
36 #ifndef _vpMbtTukeyEstimator_h_
37 #define _vpMbtTukeyEstimator_h_
38 
39 #include <vector>
40 #include <visp3/core/vpColVector.h>
41 
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 
44 template <typename T> class vpMbtTukeyEstimator
45 {
46 public:
47  void MEstimator(const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
48  void MEstimator(const vpColVector &residues, vpColVector &weights, double NoiseThreshold);
49 
50 private:
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);
56 
57  std::vector<T> m_normres;
58  std::vector<T> m_residues;
59 };
60 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
61 
62 /*
63  * The code bellow previously in vpMbtTuckeyEstimator.cpp produced
64  * a link issue with MinGW-W64 x86_64-8.1.0-posix-seh-rt_v6-rev0 (g++ 8.1.0)
65  * libvisp_mbt.so.3.1.0: undefined reference to
66  * `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
67  * std::allocator<double> > const&, std::vector<double, std::allocator<double>
68  * >&, double)'
69  * Note that with the previous MinGW-W64 version x86_64-7.3.0-posix-seh-rt_v6-rev0 (g++ 7.3.0)
70  * the build succeed.
71  *
72  * To remove this link issue the solution was to move the content of vpMbtTuckeyEstimator.cpp
73  * before remove.
74  */
75 #include <algorithm>
76 #include <cmath>
77 #include <iostream>
78 
79 #include <visp3/core/vpCPUFeatures.h>
80 
81 #define USE_TRANSFORM 1
82 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) && USE_TRANSFORM
83 #define HAVE_TRANSFORM 1
84 #include <functional>
85 #endif
86 
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
90 
91 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
92 #include <pmmintrin.h>
93 #define VISP_HAVE_SSE3 1
94 #endif
95 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
96 #include <tmmintrin.h>
97 #define VISP_HAVE_SSSE3 1
98 #endif
99 #endif
100 
101 #if defined _WIN32 && defined(_M_ARM64)
102 # define _ARM64_DISTINCT_NEON_TYPES
103 # include <Intrin.h>
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
109 #endif
110 
111 #ifndef DOXYGEN_SHOULD_SKIP_THIS
112 
113 #if HAVE_TRANSFORM
114 namespace
115 {
116 // Check if std:c++14 or higher
117 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
118 auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
119 #else
120 template <typename T> struct AbsDiff : public std::binary_function<T, T, T>
121 {
122  T operator()(const T a, const T b) const { return std::fabs(a - b); }
123 };
124 #endif
125 } // namespace
126 #endif
127 
128 template class vpMbtTukeyEstimator<float>;
129 template class vpMbtTukeyEstimator<double>;
130 
131 #if VISP_HAVE_SSSE3
132 namespace
133 {
134 inline __m128 abs_ps(__m128 x)
135 {
136  static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
137  return _mm_andnot_ps(sign_mask, x);
138 }
139 } // namespace
140 #endif
141 
142 template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
143 {
144  // Not the exact median when even number of elements
145  int index = (int)(ceil(vec.size() / 2.0)) - 1;
146  std::nth_element(vec.begin(), vec.begin() + index, vec.end());
147  return vec[index];
148 }
149 
150 // Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
151 // Ubuntu-12.04-Linux-i386-g++4.6-Dyn-RelWithDebInfo-dc1394-v4l2-X11-OpenCV2.3.1-lapack-gsl-Coin-jpeg-png-xml-pthread-OpenMP-dmtx-zbar-Wov-Weq-Moment:
152 // libvisp_mbt.so.3.1.0: undefined reference to
153 // `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
154 // std::allocator<double> > const&, std::vector<double, std::allocator<double>
155 // >&, double)'
156 template <typename T>
157 void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
158  const T NoiseThreshold)
159 {
160  if (residues.empty()) {
161  return;
162  }
163 
164  m_residues = residues;
165 
166  T med = getMedian(m_residues);
167  m_normres.resize(residues.size());
168 
169 #if HAVE_TRANSFORM
170 // Check if std:c++14 or higher
171 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
172  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
173 #else
174  std::transform(residues.begin(), residues.end(), m_normres.begin(),
175  std::bind(AbsDiff<T>(), std::placeholders::_1, med));
176 #endif
177 #else
178  for (size_t i = 0; i < m_residues.size(); i++) {
179  m_normres[i] = (std::fabs(residues[i] - med));
180  }
181 #endif
182 
183  m_residues = m_normres;
184  T normmedian = getMedian(m_residues);
185 
186  // 1.48 keeps scale estimate consistent for a normal probability dist.
187  T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
188 
189  // Set a minimum threshold for sigma
190  // (when sigma reaches the level of noise in the image)
191  if (sigma < NoiseThreshold) {
192  sigma = NoiseThreshold;
193  }
194 
195  psiTukey(sigma, m_normres, weights);
196 }
197 
198 template <>
199 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_simd(const std::vector<float> &residues,
200  std::vector<float> &weights,
201  float NoiseThreshold)
202 {
203 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
204  if (residues.empty()) {
205  return;
206  }
207 
208  m_residues = residues;
209 
210  float med = getMedian(m_residues);
211  m_normres.resize(residues.size());
212 
213  size_t i = 0;
214 #if VISP_HAVE_SSSE3
215  __m128 med_128 = _mm_set_ps1(med);
216 #else
217  float32x4_t med_128 = vdupq_n_f32(med);
218 #endif
219 
220  if (m_residues.size() >= 4) {
221  for (i = 0; i <= m_residues.size() - 4; i += 4) {
222 #if VISP_HAVE_SSSE3
223  __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
224  _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
225 #else
226  float32x4_t residues_128 = vld1q_f32(residues.data() + i);
227  vst1q_f32(m_normres.data() + i, vabsq_f32(vsubq_f32(residues_128, med_128)));
228 #endif
229  }
230  }
231 
232  for (; i < m_residues.size(); i++) {
233  m_normres[i] = (std::fabs(residues[i] - med));
234  }
235 
236  m_residues = m_normres;
237  float normmedian = getMedian(m_residues);
238 
239  // 1.48 keeps scale estimate consistent for a normal probability dist.
240  float sigma = 1.4826f * normmedian; // median Absolute Deviation
241 
242  // Set a minimum threshold for sigma
243  // (when sigma reaches the level of noise in the image)
244  if (sigma < NoiseThreshold) {
245  sigma = NoiseThreshold;
246  }
247 
248  psiTukey(sigma, m_normres, weights);
249 #else
250  (void)residues;
251  (void)weights;
252  (void)NoiseThreshold;
253 #endif
254 }
255 
259 template <>
260 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_simd(const std::vector<double> &residues,
261  std::vector<double> &weights,
262  double NoiseThreshold)
263 {
264 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
265  if (residues.empty()) {
266  return;
267  }
268 
269  m_residues = residues;
270 
271  double med = getMedian(m_residues);
272  m_normres.resize(residues.size());
273 
274 #if HAVE_TRANSFORM
275 // Check if std:c++14 or higher
276 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
277  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
278 #else
279  std::transform(residues.begin(), residues.end(), m_normres.begin(),
280  std::bind(AbsDiff<double>(), std::placeholders::_1, med));
281 #endif
282 #else
283  for (size_t i = 0; i < m_residues.size(); i++) {
284  m_normres[i] = (std::fabs(residues[i] - med));
285  }
286 #endif
287 
288  m_residues = m_normres;
289  double normmedian = getMedian(m_residues);
290 
291  // 1.48 keeps scale estimate consistent for a normal probability dist.
292  double sigma = 1.4826 * normmedian; // median Absolute Deviation
293 
294  // Set a minimum threshold for sigma
295  // (when sigma reaches the level of noise in the image)
296  if (sigma < NoiseThreshold) {
297  sigma = NoiseThreshold;
298  }
299 
300  psiTukey(sigma, m_normres, weights);
301 #else
302  (void)residues;
303  (void)weights;
304  (void)NoiseThreshold;
305 #endif
306 }
307 
311 template <>
312 inline void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
313  float NoiseThreshold)
314 {
315 #if defined(VISP_HAVE_SIMDLIB)
316  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
317 #else
318  bool checkSimd = vpCPUFeatures::checkSSSE3();
319 #endif
320 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
321  checkSimd = false;
322 #endif
323 
324  if (checkSimd)
325  MEstimator_impl_simd(residues, weights, NoiseThreshold);
326  else
327  MEstimator_impl(residues, weights, NoiseThreshold);
328 }
329 
333 template <>
334 inline void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
335  double NoiseThreshold)
336 {
337 #if defined(VISP_HAVE_SIMDLIB)
338  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
339 #else
340  bool checkSimd = vpCPUFeatures::checkSSSE3();
341 #endif
342 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
343  checkSimd = false;
344 #endif
345 
346  if (checkSimd)
347  MEstimator_impl_simd(residues, weights, NoiseThreshold);
348  else
349  MEstimator_impl(residues, weights, NoiseThreshold);
350 }
351 
355 template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
356 {
357  double C = sig * 4.6851;
358 
359  // Here we consider that sig cannot be equal to 0
360  for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
361  double xi = x[i] / C;
362  xi *= xi;
363 
364  if (xi > 1.) {
365  weights[i] = 0;
366  }
367  else {
368  xi = 1 - xi;
369  xi *= xi;
370  weights[i] = xi;
371  }
372  }
373 }
374 
378 template <>
379 inline void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
380  double NoiseThreshold)
381 {
382  if (residues.size() == 0) {
383  return;
384  }
385 
386  m_residues.resize(0);
387  m_residues.reserve(residues.size());
388  m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
389 
390  double med = getMedian(m_residues);
391 
392  m_normres.resize(residues.size());
393  for (size_t i = 0; i < m_residues.size(); i++) {
394  m_normres[i] = std::fabs(residues[(unsigned int)i] - med);
395  }
396 
397  m_residues = m_normres;
398  double normmedian = getMedian(m_residues);
399 
400  // 1.48 keeps scale estimate consistent for a normal probability dist.
401  double sigma = 1.4826 * normmedian; // median Absolute Deviation
402 
403  // Set a minimum threshold for sigma
404  // (when sigma reaches the level of noise in the image)
405  if (sigma < NoiseThreshold) {
406  sigma = NoiseThreshold;
407  }
408 
409  psiTukey(sigma, m_normres, weights);
410 }
411 
415 template <>
416 inline void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
417  double NoiseThreshold)
418 {
419  if (residues.size() == 0) {
420  return;
421  }
422 
423  m_residues.resize(0);
424  m_residues.reserve(residues.size());
425  for (unsigned int i = 0; i < residues.size(); i++) {
426  m_residues.push_back((float)residues[i]);
427  }
428 
429  float med = getMedian(m_residues);
430 
431  m_normres.resize(residues.size());
432  for (size_t i = 0; i < m_residues.size(); i++) {
433  m_normres[i] = (float)std::fabs(residues[(unsigned int)i] - med);
434  }
435 
436  m_residues = m_normres;
437  float normmedian = getMedian(m_residues);
438 
439  // 1.48 keeps scale estimate consistent for a normal probability dist.
440  float sigma = 1.4826f * normmedian; // median Absolute Deviation
441 
442  // Set a minimum threshold for sigma
443  // (when sigma reaches the level of noise in the image)
444  if (sigma < NoiseThreshold) {
445  sigma = (float)NoiseThreshold;
446  }
447 
448  psiTukey(sigma, m_normres, weights);
449 }
450 
454 template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
455 {
456  T C = static_cast<T>(4.6851) * sig;
457  weights.resize(x.size());
458 
459  // Here we consider that sig cannot be equal to 0
460  for (size_t i = 0; i < x.size(); i++) {
461  T xi = x[i] / C;
462  xi *= xi;
463 
464  if (xi > 1.) {
465  weights[i] = 0;
466  }
467  else {
468  xi = 1 - xi;
469  xi *= xi;
470  weights[i] = xi;
471  }
472  }
473 }
474 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
475 
476 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()