Visual Servoing Platform  version 3.5.1 under development (2023-01-31)
vpMbtTukeyEstimator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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 (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && 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 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
117 auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
118 #else
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); }
121 };
122 #endif
123 } // namespace
124 #endif
125 
126 template class vpMbtTukeyEstimator<float>;
127 template class vpMbtTukeyEstimator<double>;
128 
129 #if VISP_HAVE_SSSE3
130 namespace
131 {
132 inline __m128 abs_ps(__m128 x)
133 {
134  static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
135  return _mm_andnot_ps(sign_mask, x);
136 }
137 } // namespace
138 #endif
139 
140 template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
141 {
142  // Not the exact median when even number of elements
143  int index = (int)(ceil(vec.size() / 2.0)) - 1;
144  std::nth_element(vec.begin(), vec.begin() + index, vec.end());
145  return vec[index];
146 }
147 
148 // Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
149 // 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:
150 // libvisp_mbt.so.3.1.0: undefined reference to
151 // `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
152 // std::allocator<double> > const&, std::vector<double, std::allocator<double>
153 // >&, double)'
154 template <typename T>
155 void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
156  const T NoiseThreshold)
157 {
158  if (residues.empty()) {
159  return;
160  }
161 
162  m_residues = residues;
163 
164  T med = getMedian(m_residues);
165  m_normres.resize(residues.size());
166 
167 #if HAVE_TRANSFORM
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));
170 #else
171  std::transform(residues.begin(), residues.end(), m_normres.begin(),
172  std::bind(AbsDiff<T>(), std::placeholders::_1, med));
173 #endif
174 #else
175  for (size_t i = 0; i < m_residues.size(); i++) {
176  m_normres[i] = (std::fabs(residues[i] - med));
177  }
178 #endif
179 
180  m_residues = m_normres;
181  T normmedian = getMedian(m_residues);
182 
183  // 1.48 keeps scale estimate consistent for a normal probability dist.
184  T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
185 
186  // Set a minimum threshold for sigma
187  // (when sigma reaches the level of noise in the image)
188  if (sigma < NoiseThreshold) {
189  sigma = NoiseThreshold;
190  }
191 
192  psiTukey(sigma, m_normres, weights);
193 }
194 
195 template <>
196 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_simd(const std::vector<float> &residues,
197  std::vector<float> &weights,
198  float NoiseThreshold)
199 {
200 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
201  if (residues.empty()) {
202  return;
203  }
204 
205  m_residues = residues;
206 
207  float med = getMedian(m_residues);
208  m_normres.resize(residues.size());
209 
210  size_t i = 0;
211 #if VISP_HAVE_SSSE3
212  __m128 med_128 = _mm_set_ps1(med);
213 #else
214  float32x4_t med_128 = vdupq_n_f32(med);
215 #endif
216 
217  if (m_residues.size() >= 4) {
218  for (i = 0; i <= m_residues.size() - 4; i += 4) {
219 #if VISP_HAVE_SSSE3
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)));
222 #else
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)));
225 #endif
226  }
227  }
228 
229  for (; i < m_residues.size(); i++) {
230  m_normres[i] = (std::fabs(residues[i] - med));
231  }
232 
233  m_residues = m_normres;
234  float normmedian = getMedian(m_residues);
235 
236  // 1.48 keeps scale estimate consistent for a normal probability dist.
237  float sigma = 1.4826f * normmedian; // median Absolute Deviation
238 
239  // Set a minimum threshold for sigma
240  // (when sigma reaches the level of noise in the image)
241  if (sigma < NoiseThreshold) {
242  sigma = NoiseThreshold;
243  }
244 
245  psiTukey(sigma, m_normres, weights);
246 #else
247  (void)residues;
248  (void)weights;
249  (void)NoiseThreshold;
250 #endif
251 }
252 
256 template <>
257 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_simd(const std::vector<double> &residues,
258  std::vector<double> &weights,
259  double NoiseThreshold)
260 {
261 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
262  if (residues.empty()) {
263  return;
264  }
265 
266  m_residues = residues;
267 
268  double med = getMedian(m_residues);
269  m_normres.resize(residues.size());
270 
271 #if HAVE_TRANSFORM
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));
274 #else
275  std::transform(residues.begin(), residues.end(), m_normres.begin(),
276  std::bind(AbsDiff<double>(), std::placeholders::_1, med));
277 #endif
278 #else
279  for (size_t i = 0; i < m_residues.size(); i++) {
280  m_normres[i] = (std::fabs(residues[i] - med));
281  }
282 #endif
283 
284  m_residues = m_normres;
285  double normmedian = getMedian(m_residues);
286 
287  // 1.48 keeps scale estimate consistent for a normal probability dist.
288  double sigma = 1.4826 * normmedian; // median Absolute Deviation
289 
290  // Set a minimum threshold for sigma
291  // (when sigma reaches the level of noise in the image)
292  if (sigma < NoiseThreshold) {
293  sigma = NoiseThreshold;
294  }
295 
296  psiTukey(sigma, m_normres, weights);
297 #else
298  (void)residues;
299  (void)weights;
300  (void)NoiseThreshold;
301 #endif
302 }
303 
307 template <>
308 inline void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
309  float NoiseThreshold)
310 {
311  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
312 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
313  checkSimd = false;
314 #endif
315 
316  if (checkSimd)
317  MEstimator_impl_simd(residues, weights, NoiseThreshold);
318  else
319  MEstimator_impl(residues, weights, NoiseThreshold);
320 }
321 
325 template <>
326 inline void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
327  double NoiseThreshold)
328 {
329  bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon();
330 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
331  checkSimd = false;
332 #endif
333 
334  if (checkSimd)
335  MEstimator_impl_simd(residues, weights, NoiseThreshold);
336  else
337  MEstimator_impl(residues, weights, NoiseThreshold);
338 }
339 
343 template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
344 {
345  double C = sig * 4.6851;
346 
347  // Here we consider that sig cannot be equal to 0
348  for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
349  double xi = x[i] / C;
350  xi *= xi;
351 
352  if (xi > 1.) {
353  weights[i] = 0;
354  } else {
355  xi = 1 - xi;
356  xi *= xi;
357  weights[i] = xi;
358  }
359  }
360 }
361 
365 template <>
366 inline void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
367  double NoiseThreshold)
368 {
369  if (residues.size() == 0) {
370  return;
371  }
372 
373  m_residues.resize(0);
374  m_residues.reserve(residues.size());
375  m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
376 
377  double med = getMedian(m_residues);
378 
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);
382  }
383 
384  m_residues = m_normres;
385  double normmedian = getMedian(m_residues);
386 
387  // 1.48 keeps scale estimate consistent for a normal probability dist.
388  double sigma = 1.4826 * normmedian; // median Absolute Deviation
389 
390  // Set a minimum threshold for sigma
391  // (when sigma reaches the level of noise in the image)
392  if (sigma < NoiseThreshold) {
393  sigma = NoiseThreshold;
394  }
395 
396  psiTukey(sigma, m_normres, weights);
397 }
398 
402 template <>
403 inline void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
404  double NoiseThreshold)
405 {
406  if (residues.size() == 0) {
407  return;
408  }
409 
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]);
414  }
415 
416  float med = getMedian(m_residues);
417 
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);
421  }
422 
423  m_residues = m_normres;
424  float normmedian = getMedian(m_residues);
425 
426  // 1.48 keeps scale estimate consistent for a normal probability dist.
427  float sigma = 1.4826f * normmedian; // median Absolute Deviation
428 
429  // Set a minimum threshold for sigma
430  // (when sigma reaches the level of noise in the image)
431  if (sigma < NoiseThreshold) {
432  sigma = (float)NoiseThreshold;
433  }
434 
435  psiTukey(sigma, m_normres, weights);
436 }
437 
441 template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
442 {
443  T C = static_cast<T>(4.6851) * sig;
444  weights.resize(x.size());
445 
446  // Here we consider that sig cannot be equal to 0
447  for (size_t i = 0; i < x.size(); i++) {
448  T xi = x[i] / C;
449  xi *= xi;
450 
451  if (xi > 1.) {
452  weights[i] = 0;
453  } else {
454  xi = 1 - xi;
455  xi *= xi;
456  weights[i] = xi;
457  }
458  }
459 }
460 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
461 
462 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()