Visual Servoing Platform  version 3.6.1 under development (2024-06-15)
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/vpConfig.h>
41 #include <visp3/core/vpColVector.h>
42 
43 #ifndef DOXYGEN_SHOULD_SKIP_THIS
44 
46 template <typename T> class vpMbtTukeyEstimator
47 {
48 public:
49  void MEstimator(const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
50  void MEstimator(const vpColVector &residues, vpColVector &weights, double NoiseThreshold);
51 
52 private:
53  T getMedian(std::vector<T> &vec);
54  void MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
55  void MEstimator_impl_simd(const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
56  void psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights);
57  void psiTukey(const T sig, std::vector<T> &x, vpColVector &weights);
58 
59  std::vector<T> m_normres;
60  std::vector<T> m_residues;
61 };
63 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
64 
65 /*
66  * The code bellow previously in vpMbtTuckeyEstimator.cpp produced
67  * a link issue with MinGW-W64 x86_64-8.1.0-posix-seh-rt_v6-rev0 (g++ 8.1.0)
68  * libvisp_mbt.so.3.1.0: undefined reference to
69  * `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
70  * std::allocator<double> > const&, std::vector<double, std::allocator<double>
71  * >&, double)'
72  * Note that with the previous MinGW-W64 version x86_64-7.3.0-posix-seh-rt_v6-rev0 (g++ 7.3.0)
73  * the build succeed.
74  *
75  * To remove this link issue the solution was to move the content of vpMbtTuckeyEstimator.cpp
76  * before remove.
77  */
78 #include <algorithm>
79 #include <cmath>
80 #include <iostream>
81 
82 #include <visp3/core/vpCPUFeatures.h>
83 
84 #define USE_TRANSFORM 1
85 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) && USE_TRANSFORM
86 #define HAVE_TRANSFORM 1
87 #include <functional>
88 #endif
89 
90 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
91 #include <emmintrin.h>
92 #define VISP_HAVE_SSE2 1
93 
94 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
95 #include <pmmintrin.h>
96 #define VISP_HAVE_SSE3 1
97 #endif
98 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
99 #include <tmmintrin.h>
100 #define VISP_HAVE_SSSE3 1
101 #endif
102 #endif
103 
104 #if defined _WIN32 && defined(_M_ARM64)
105 # define _ARM64_DISTINCT_NEON_TYPES
106 # include <Intrin.h>
107 # include <arm_neon.h>
108 # define VISP_HAVE_NEON 1
109 #elif (defined(__ARM_NEON__) || defined (__ARM_NEON)) && defined(__aarch64__)
110 # include <arm_neon.h>
111 # define VISP_HAVE_NEON 1
112 #endif
113 
114 #ifndef DOXYGEN_SHOULD_SKIP_THIS
115 
116 #if HAVE_TRANSFORM
117  namespace
118 {
119 // Check if std:c++14 or higher
120 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
121 auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
122 #else
123 template <typename T> struct AbsDiff : public std::binary_function<T, T, T>
124 {
125  T operator()(const T a, const T b) const { return std::fabs(a - b); }
126 };
127 #endif
128 } // namespace
129 #endif
130 
132 template class vpMbtTukeyEstimator<float>;
133 template class vpMbtTukeyEstimator<double>;
134 
135 #if VISP_HAVE_SSSE3
136 namespace
137 {
138 inline __m128 abs_ps(__m128 x)
139 {
140  static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
141  return _mm_andnot_ps(sign_mask, x);
142 }
143 } // namespace
144 #endif
145 
146 template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
147 {
148  // Not the exact median when even number of elements
149  int index = (int)(ceil(vec.size() / 2.0)) - 1;
150  std::nth_element(vec.begin(), vec.begin() + index, vec.end());
151  return vec[index];
152 }
153 
154 // Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
155 // 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:
156 // libvisp_mbt.so.3.1.0: undefined reference to
157 // `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
158 // std::allocator<double> > const&, std::vector<double, std::allocator<double>
159 // >&, double)'
160 template <typename T>
161 void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
162  const T NoiseThreshold)
163 {
164  if (residues.empty()) {
165  return;
166  }
167 
168  m_residues = residues;
169 
170  T med = getMedian(m_residues);
171  m_normres.resize(residues.size());
172 
173 #if HAVE_TRANSFORM
174 // Check if std:c++14 or higher
175 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
176  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
177 #else
178  std::transform(residues.begin(), residues.end(), m_normres.begin(),
179  std::bind(AbsDiff<T>(), std::placeholders::_1, med));
180 #endif
181 #else
182  for (size_t i = 0; i < m_residues.size(); i++) {
183  m_normres[i] = (std::fabs(residues[i] - med));
184  }
185 #endif
186 
187  m_residues = m_normres;
188  T normmedian = getMedian(m_residues);
189 
190  // 1.48 keeps scale estimate consistent for a normal probability dist.
191  T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
192 
193  // Set a minimum threshold for sigma
194  // (when sigma reaches the level of noise in the image)
195  if (sigma < NoiseThreshold) {
196  sigma = NoiseThreshold;
197  }
198 
199  psiTukey(sigma, m_normres, weights);
200 }
201 
202 template <>
203 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_simd(const std::vector<float> &residues,
204  std::vector<float> &weights,
205  float NoiseThreshold)
206 {
207 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
208  if (residues.empty()) {
209  return;
210  }
211 
212  m_residues = residues;
213 
214  float med = getMedian(m_residues);
215  m_normres.resize(residues.size());
216 
217  size_t i = 0;
218 #if VISP_HAVE_SSSE3
219  __m128 med_128 = _mm_set_ps1(med);
220 #else
221  float32x4_t med_128 = vdupq_n_f32(med);
222 #endif
223 
224  if (m_residues.size() >= 4) {
225  for (i = 0; i <= m_residues.size() - 4; i += 4) {
226 #if VISP_HAVE_SSSE3
227  __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
228  _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
229 #else
230  float32x4_t residues_128 = vld1q_f32(residues.data() + i);
231  vst1q_f32(m_normres.data() + i, vabsq_f32(vsubq_f32(residues_128, med_128)));
232 #endif
233  }
234  }
235 
236  for (; i < m_residues.size(); i++) {
237  m_normres[i] = (std::fabs(residues[i] - med));
238  }
239 
240  m_residues = m_normres;
241  float normmedian = getMedian(m_residues);
242 
243  // 1.48 keeps scale estimate consistent for a normal probability dist.
244  float sigma = 1.4826f * normmedian; // median Absolute Deviation
245 
246  // Set a minimum threshold for sigma
247  // (when sigma reaches the level of noise in the image)
248  if (sigma < NoiseThreshold) {
249  sigma = NoiseThreshold;
250  }
251 
252  psiTukey(sigma, m_normres, weights);
253 #else
254  (void)residues;
255  (void)weights;
256  (void)NoiseThreshold;
257 #endif
258 }
259 
263 template <>
264 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_simd(const std::vector<double> &residues,
265  std::vector<double> &weights,
266  double NoiseThreshold)
267 {
268 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
269  if (residues.empty()) {
270  return;
271  }
272 
273  m_residues = residues;
274 
275  double med = getMedian(m_residues);
276  m_normres.resize(residues.size());
277 
278 #if HAVE_TRANSFORM
279 // Check if std:c++14 or higher
280 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
281  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
282 #else
283  std::transform(residues.begin(), residues.end(), m_normres.begin(),
284  std::bind(AbsDiff<double>(), std::placeholders::_1, med));
285 #endif
286 #else
287  for (size_t i = 0; i < m_residues.size(); i++) {
288  m_normres[i] = (std::fabs(residues[i] - med));
289  }
290 #endif
291 
292  m_residues = m_normres;
293  double normmedian = getMedian(m_residues);
294 
295  // 1.48 keeps scale estimate consistent for a normal probability dist.
296  double sigma = 1.4826 * normmedian; // median Absolute Deviation
297 
298  // Set a minimum threshold for sigma
299  // (when sigma reaches the level of noise in the image)
300  if (sigma < NoiseThreshold) {
301  sigma = NoiseThreshold;
302  }
303 
304  psiTukey(sigma, m_normres, weights);
305 #else
306  (void)residues;
307  (void)weights;
308  (void)NoiseThreshold;
309 #endif
310 }
311 
315 template <>
316 inline void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
317  float NoiseThreshold)
318 {
319 #if defined(VISP_HAVE_SIMDLIB)
320  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
321 #else
322  bool checkSimd = vpCPUFeatures::checkSSSE3();
323 #endif
324 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
325  checkSimd = false;
326 #endif
327 
328  if (checkSimd)
329  MEstimator_impl_simd(residues, weights, NoiseThreshold);
330  else
331  MEstimator_impl(residues, weights, NoiseThreshold);
332 }
333 
337 template <>
338 inline void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
339  double NoiseThreshold)
340 {
341 #if defined(VISP_HAVE_SIMDLIB)
342  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
343 #else
344  bool checkSimd = vpCPUFeatures::checkSSSE3();
345 #endif
346 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
347  checkSimd = false;
348 #endif
349 
350  if (checkSimd)
351  MEstimator_impl_simd(residues, weights, NoiseThreshold);
352  else
353  MEstimator_impl(residues, weights, NoiseThreshold);
354 }
355 
359 template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
360 {
361  double C = sig * 4.6851;
362 
363  // Here we consider that sig cannot be equal to 0
364  for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
365  double xi = x[i] / C;
366  xi *= xi;
367 
368  if (xi > 1.) {
369  weights[i] = 0;
370  }
371  else {
372  xi = 1 - xi;
373  xi *= xi;
374  weights[i] = xi;
375  }
376  }
377 }
378 
382 template <>
383 inline void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
384  double NoiseThreshold)
385 {
386  if (residues.size() == 0) {
387  return;
388  }
389 
390  m_residues.resize(0);
391  m_residues.reserve(residues.size());
392  m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
393 
394  double med = getMedian(m_residues);
395 
396  m_normres.resize(residues.size());
397  for (size_t i = 0; i < m_residues.size(); i++) {
398  m_normres[i] = std::fabs(residues[(unsigned int)i] - med);
399  }
400 
401  m_residues = m_normres;
402  double normmedian = getMedian(m_residues);
403 
404  // 1.48 keeps scale estimate consistent for a normal probability dist.
405  double sigma = 1.4826 * normmedian; // median Absolute Deviation
406 
407  // Set a minimum threshold for sigma
408  // (when sigma reaches the level of noise in the image)
409  if (sigma < NoiseThreshold) {
410  sigma = NoiseThreshold;
411  }
412 
413  psiTukey(sigma, m_normres, weights);
414 }
415 
419 template <>
420 inline void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
421  double NoiseThreshold)
422 {
423  if (residues.size() == 0) {
424  return;
425  }
426 
427  m_residues.resize(0);
428  m_residues.reserve(residues.size());
429  for (unsigned int i = 0; i < residues.size(); i++) {
430  m_residues.push_back((float)residues[i]);
431  }
432 
433  float med = getMedian(m_residues);
434 
435  m_normres.resize(residues.size());
436  for (size_t i = 0; i < m_residues.size(); i++) {
437  m_normres[i] = (float)std::fabs(residues[(unsigned int)i] - med);
438  }
439 
440  m_residues = m_normres;
441  float normmedian = getMedian(m_residues);
442 
443  // 1.48 keeps scale estimate consistent for a normal probability dist.
444  float sigma = 1.4826f * normmedian; // median Absolute Deviation
445 
446  // Set a minimum threshold for sigma
447  // (when sigma reaches the level of noise in the image)
448  if (sigma < NoiseThreshold) {
449  sigma = (float)NoiseThreshold;
450  }
451 
452  psiTukey(sigma, m_normres, weights);
453 }
454 
458 template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
459 {
460  T C = static_cast<T>(4.6851) * sig;
461  weights.resize(x.size());
462 
463  // Here we consider that sig cannot be equal to 0
464  for (size_t i = 0; i < x.size(); i++) {
465  T xi = x[i] / C;
466  xi *= xi;
467 
468  if (xi > 1.) {
469  weights[i] = 0;
470  }
471  else {
472  xi = 1 - xi;
473  xi *= xi;
474  weights[i] = xi;
475  }
476  }
477 }
479 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
480 
481 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:342
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()