Visual Servoing Platform  version 3.5.1 under development (2022-10-02)
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, const T NoiseThreshold);
48  void MEstimator(const vpColVector &residues, vpColVector &weights, const 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, const T NoiseThreshold);
53  void MEstimator_impl_ssse3(const std::vector<T> &residues, std::vector<T> &weights, const 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 #ifndef DOXYGEN_SHOULD_SKIP_THIS
102 
103 #if HAVE_TRANSFORM
104 namespace
105 {
106 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
107 auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
108 #else
109 template <typename T> struct AbsDiff : public std::binary_function<T, T, T> {
110  T operator()(const T a, const T b) const { return std::fabs(a - b); }
111 };
112 #endif
113 } // namespace
114 #endif
115 
116 template class vpMbtTukeyEstimator<float>;
117 template class vpMbtTukeyEstimator<double>;
118 
119 #if VISP_HAVE_SSSE3
120 namespace
121 {
122 inline __m128 abs_ps(__m128 x)
123 {
124  static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
125  return _mm_andnot_ps(sign_mask, x);
126 }
127 } // namespace
128 #endif
129 
130 template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
131 {
132  // Not the exact median when even number of elements
133  int index = (int)(ceil(vec.size() / 2.0)) - 1;
134  std::nth_element(vec.begin(), vec.begin() + index, vec.end());
135  return vec[index];
136 }
137 
138 // Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
139 // 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:
140 // libvisp_mbt.so.3.1.0: undefined reference to
141 // `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
142 // std::allocator<double> > const&, std::vector<double, std::allocator<double>
143 // >&, double)'
144 template <typename T>
145 void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
146  const T NoiseThreshold)
147 {
148  if (residues.empty()) {
149  return;
150  }
151 
152  m_residues = residues;
153 
154  T med = getMedian(m_residues);
155  m_normres.resize(residues.size());
156 
157 #if HAVE_TRANSFORM
158 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
159  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
160 #else
161  std::transform(residues.begin(), residues.end(), m_normres.begin(),
162  std::bind(AbsDiff<T>(), std::placeholders::_1, med));
163 #endif
164 #else
165  for (size_t i = 0; i < m_residues.size(); i++) {
166  m_normres[i] = (std::fabs(residues[i] - med));
167  }
168 #endif
169 
170  m_residues = m_normres;
171  T normmedian = getMedian(m_residues);
172 
173  // 1.48 keeps scale estimate consistent for a normal probability dist.
174  T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
175 
176  // Set a minimum threshold for sigma
177  // (when sigma reaches the level of noise in the image)
178  if (sigma < NoiseThreshold) {
179  sigma = NoiseThreshold;
180  }
181 
182  psiTukey(sigma, m_normres, weights);
183 }
184 
185 template <>
186 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_ssse3(const std::vector<float> &residues,
187  std::vector<float> &weights, const float NoiseThreshold)
188 {
189 #if VISP_HAVE_SSSE3
190  if (residues.empty()) {
191  return;
192  }
193 
194  m_residues = residues;
195 
196  float med = getMedian(m_residues);
197  m_normres.resize(residues.size());
198 
199  size_t i = 0;
200  __m128 med_128 = _mm_set_ps1(med);
201 
202  if (m_residues.size() >= 4) {
203  for (i = 0; i <= m_residues.size() - 4; i += 4) {
204  __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
205  _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
206  }
207  }
208 
209  for (; i < m_residues.size(); i++) {
210  m_normres[i] = (std::fabs(residues[i] - med));
211  }
212 
213  m_residues = m_normres;
214  float normmedian = getMedian(m_residues);
215 
216  // 1.48 keeps scale estimate consistent for a normal probability dist.
217  float sigma = 1.4826f * normmedian; // median Absolute Deviation
218 
219  // Set a minimum threshold for sigma
220  // (when sigma reaches the level of noise in the image)
221  if (sigma < NoiseThreshold) {
222  sigma = NoiseThreshold;
223  }
224 
225  psiTukey(sigma, m_normres, weights);
226 #else
227  (void)residues;
228  (void)weights;
229  (void)NoiseThreshold;
230 #endif
231 }
232 
236 template <>
237 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_ssse3(const std::vector<double> &residues,
238  std::vector<double> &weights,
239  const double NoiseThreshold)
240 {
241 #if VISP_HAVE_SSSE3
242  if (residues.empty()) {
243  return;
244  }
245 
246  m_residues = residues;
247 
248  double med = getMedian(m_residues);
249  m_normres.resize(residues.size());
250 
251 #if HAVE_TRANSFORM
252 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
253  std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
254 #else
255  std::transform(residues.begin(), residues.end(), m_normres.begin(),
256  std::bind(AbsDiff<double>(), std::placeholders::_1, med));
257 #endif
258 #else
259  for (size_t i = 0; i < m_residues.size(); i++) {
260  m_normres[i] = (std::fabs(residues[i] - med));
261  }
262 #endif
263 
264  m_residues = m_normres;
265  double normmedian = getMedian(m_residues);
266 
267  // 1.48 keeps scale estimate consistent for a normal probability dist.
268  double sigma = 1.4826 * normmedian; // median Absolute Deviation
269 
270  // Set a minimum threshold for sigma
271  // (when sigma reaches the level of noise in the image)
272  if (sigma < NoiseThreshold) {
273  sigma = NoiseThreshold;
274  }
275 
276  psiTukey(sigma, m_normres, weights);
277 #else
278  (void)residues;
279  (void)weights;
280  (void)NoiseThreshold;
281 #endif
282 }
283 
287 template <>
288 inline void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
289  const float NoiseThreshold)
290 {
292 #if !VISP_HAVE_SSSE3
293  checkSSSE3 = false;
294 #endif
295 
296  if (checkSSSE3)
297  MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
298  else
299  MEstimator_impl(residues, weights, NoiseThreshold);
300 }
301 
305 template <>
306 inline void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
307  const double NoiseThreshold)
308 {
310 #if !VISP_HAVE_SSSE3
311  checkSSSE3 = false;
312 #endif
313 
314  if (checkSSSE3)
315  MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
316  else
317  MEstimator_impl(residues, weights, NoiseThreshold);
318 }
319 
323 template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
324 {
325  double C = sig * 4.6851;
326 
327  // Here we consider that sig cannot be equal to 0
328  for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
329  double xi = x[i] / C;
330  xi *= xi;
331 
332  if (xi > 1.) {
333  weights[i] = 0;
334  } else {
335  xi = 1 - xi;
336  xi *= xi;
337  weights[i] = xi;
338  }
339  }
340 }
341 
345 template <>
346 inline void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
347  const double NoiseThreshold)
348 {
349  if (residues.size() == 0) {
350  return;
351  }
352 
353  m_residues.resize(0);
354  m_residues.reserve(residues.size());
355  m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
356 
357  double med = getMedian(m_residues);
358 
359  m_normres.resize(residues.size());
360  for (size_t i = 0; i < m_residues.size(); i++) {
361  m_normres[i] = std::fabs(residues[(unsigned int)i] - med);
362  }
363 
364  m_residues = m_normres;
365  double normmedian = getMedian(m_residues);
366 
367  // 1.48 keeps scale estimate consistent for a normal probability dist.
368  double sigma = 1.4826 * normmedian; // median Absolute Deviation
369 
370  // Set a minimum threshold for sigma
371  // (when sigma reaches the level of noise in the image)
372  if (sigma < NoiseThreshold) {
373  sigma = NoiseThreshold;
374  }
375 
376  psiTukey(sigma, m_normres, weights);
377 }
378 
382 template <>
383 inline void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
384  const double NoiseThreshold)
385 {
386  if (residues.size() == 0) {
387  return;
388  }
389 
390  m_residues.resize(0);
391  m_residues.reserve(residues.size());
392  for (unsigned int i = 0; i < residues.size(); i++) {
393  m_residues.push_back((float)residues[i]);
394  }
395 
396  float med = getMedian(m_residues);
397 
398  m_normres.resize(residues.size());
399  for (size_t i = 0; i < m_residues.size(); i++) {
400  m_normres[i] = (float)std::fabs(residues[(unsigned int)i] - med);
401  }
402 
403  m_residues = m_normres;
404  float normmedian = getMedian(m_residues);
405 
406  // 1.48 keeps scale estimate consistent for a normal probability dist.
407  float sigma = 1.4826f * normmedian; // median Absolute Deviation
408 
409  // Set a minimum threshold for sigma
410  // (when sigma reaches the level of noise in the image)
411  if (sigma < NoiseThreshold) {
412  sigma = (float)NoiseThreshold;
413  }
414 
415  psiTukey(sigma, m_normres, weights);
416 }
417 
421 template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
422 {
423  T C = static_cast<T>(4.6851) * sig;
424  weights.resize(x.size());
425 
426  // Here we consider that sig cannot be equal to 0
427  for (size_t i = 0; i < x.size(); i++) {
428  T xi = x[i] / C;
429  xi *= xi;
430 
431  if (xi > 1.) {
432  weights[i] = 0;
433  } else {
434  xi = 1 - xi;
435  xi *= xi;
436  weights[i] = xi;
437  }
438  }
439 }
440 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
441 
442 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
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()