Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpMbtTukeyEstimator.cpp
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 #include <algorithm>
37 #include <cmath>
38 #include <iostream>
39 
40 #include <visp3/core/vpCPUFeatures.h>
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/mbt/vpMbtTukeyEstimator.h>
43 
44 #define USE_TRANSFORM 1
45 #if defined(VISP_HAVE_CPP11_COMPATIBILITY) && USE_TRANSFORM
46 #define HAVE_TRANSFORM 1
47 #endif
48 
49 #if HAVE_TRANSFORM
50 #include <functional>
51 #endif
52 
53 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
54 #include <emmintrin.h>
55 #define VISP_HAVE_SSE2 1
56 
57 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
58 #include <pmmintrin.h>
59 #define VISP_HAVE_SSE3 1
60 #endif
61 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
62 #include <tmmintrin.h>
63 #define VISP_HAVE_SSSE3 1
64 #endif
65 #endif
66 
67 #define USE_ORIGINAL_TUKEY_CODE 1
68 
69 #ifndef DOXYGEN_SHOULD_SKIP_THIS
70 
71 #if HAVE_TRANSFORM
72 namespace
73 {
74 template <typename T> struct AbsDiff : public std::binary_function<T, T, T> {
75  double operator()(const T a, const T b) const { return std::fabs(a - b); }
76 };
77 }
78 #endif
79 
80 template class vpMbtTukeyEstimator<float>;
81 template class vpMbtTukeyEstimator<double>;
82 
83 #if VISP_HAVE_SSSE3
84 namespace
85 {
86 inline __m128 abs_ps(__m128 x)
87 {
88  static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
89  return _mm_andnot_ps(sign_mask, x);
90 }
91 }
92 #endif
93 
94 template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
95 {
96  // Not the exact median when even number of elements
97  int index = (int)(ceil(vec.size() / 2.0)) - 1;
98  std::nth_element(vec.begin(), vec.begin() + index, vec.end());
99  return vec[index];
100 }
101 
102 // Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
103 // 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:
104 // libvisp_mbt.so.3.1.0: undefined reference to
105 // `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
106 // std::allocator<double> > const&, std::vector<double, std::allocator<double>
107 // >&, double)'
108 template <typename T>
109 void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
110  const T NoiseThreshold)
111 {
112  if (residues.empty()) {
113  return;
114  }
115 
116  m_residues = residues;
117 
118  T med = getMedian(m_residues);
119  m_normres.resize(residues.size());
120 
121 #if HAVE_TRANSFORM
122  std::transform(residues.begin(), residues.end(), m_normres.begin(),
123  std::bind(AbsDiff<T>(), std::placeholders::_1, med));
124 #else
125  for (size_t i = 0; i < m_residues.size(); i++) {
126  m_normres[i] = (std::fabs(residues[i] - med));
127  }
128 #endif
129 
130  m_residues = m_normres;
131  T normmedian = getMedian(m_residues);
132 
133  // 1.48 keeps scale estimate consistent for a normal probability dist.
134  T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
135 
136  // Set a minimum threshold for sigma
137  // (when sigma reaches the level of noise in the image)
138  if (sigma < NoiseThreshold) {
139  sigma = NoiseThreshold;
140  }
141 
142  psiTukey(sigma, m_normres, weights);
143 }
144 
145 template <>
146 void vpMbtTukeyEstimator<float>::MEstimator_impl_ssse3(const std::vector<float> &residues, std::vector<float> &weights,
147  const float NoiseThreshold)
148 {
149 #if VISP_HAVE_SSSE3
150  if (residues.empty()) {
151  return;
152  }
153 
154  m_residues = residues;
155 
156  float med = getMedian(m_residues);
157  m_normres.resize(residues.size());
158 
159  size_t i = 0;
160  __m128 med_128 = _mm_set_ps1(med);
161 
162  if (m_residues.size() >= 4) {
163  for (i = 0; i <= m_residues.size() - 4; i += 4) {
164  __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
165  _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
166  }
167  }
168 
169  for (; i < m_residues.size(); i++) {
170  m_normres[i] = (std::fabs(residues[i] - med));
171  }
172 
173  m_residues = m_normres;
174  float normmedian = getMedian(m_residues);
175 
176  // 1.48 keeps scale estimate consistent for a normal probability dist.
177  float sigma = 1.4826f * normmedian; // median Absolute Deviation
178 
179  // Set a minimum threshold for sigma
180  // (when sigma reaches the level of noise in the image)
181  if (sigma < NoiseThreshold) {
182  sigma = NoiseThreshold;
183  }
184 
185  psiTukey(sigma, m_normres, weights);
186 #else
187  (void)residues;
188  (void)weights;
189  (void)NoiseThreshold;
190 #endif
191 }
192 
193 template <>
194 void vpMbtTukeyEstimator<double>::MEstimator_impl_ssse3(const std::vector<double> &residues,
195  std::vector<double> &weights, const double NoiseThreshold)
196 {
197 #if VISP_HAVE_SSSE3
198  if (residues.empty()) {
199  return;
200  }
201 
202  m_residues = residues;
203 
204  double med = getMedian(m_residues);
205  m_normres.resize(residues.size());
206 
207 #if HAVE_TRANSFORM
208  std::transform(residues.begin(), residues.end(), m_normres.begin(),
209  std::bind(AbsDiff<double>(), std::placeholders::_1, med));
210 #else
211  for (size_t i = 0; i < m_residues.size(); i++) {
212  m_normres[i] = (std::fabs(residues[i] - med));
213  }
214 #endif
215 
216  m_residues = m_normres;
217  double normmedian = getMedian(m_residues);
218 
219  // 1.48 keeps scale estimate consistent for a normal probability dist.
220  double sigma = 1.4826 * normmedian; // median Absolute Deviation
221 
222  // Set a minimum threshold for sigma
223  // (when sigma reaches the level of noise in the image)
224  if (sigma < NoiseThreshold) {
225  sigma = NoiseThreshold;
226  }
227 
228  psiTukey(sigma, m_normres, weights);
229 #else
230  (void)residues;
231  (void)weights;
232  (void)NoiseThreshold;
233 #endif
234 }
235 
236 template <>
237 void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
238  const float NoiseThreshold)
239 {
241 #if !VISP_HAVE_SSSE3
242  checkSSSE3 = false;
243 #endif
244 
245  if (checkSSSE3)
246  MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
247  else
248  MEstimator_impl(residues, weights, NoiseThreshold);
249 }
250 
251 template <>
252 void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
253  const double NoiseThreshold)
254 {
255  bool checkSSSE3 = vpCPUFeatures::checkSSSE3();
256 #if !VISP_HAVE_SSSE3
257  checkSSSE3 = false;
258 #endif
259 
260  if (checkSSSE3)
261  MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
262  else
263  MEstimator_impl(residues, weights, NoiseThreshold);
264 }
265 
266 template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
267 {
268  T cst_const = static_cast<T>(4.6851);
269  T inv_cst_const = 1 / cst_const;
270  T inv_sig = 1 / sig;
271 
272  for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
273 #if USE_ORIGINAL_TUKEY_CODE
274  if (std::fabs(sig) <= std::numeric_limits<T>::epsilon() &&
275  std::fabs(weights[i]) > std::numeric_limits<double>::epsilon() // sig should be equal to 0
276  // only if NoiseThreshold
277  // ==
278  // 0
279  ) {
280  weights[i] = 1;
281  continue;
282  }
283 #endif
284 
285  double xi_sig = x[(size_t)i] * inv_sig;
286 
287  if ((std::fabs(xi_sig) <= cst_const)
288 #if USE_ORIGINAL_TUKEY_CODE
289  && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon() // Consider the previous
290  // weights here
291 #endif
292  ) {
293  weights[i] = (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)) *
294  (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)); // vpMath::sqr( 1 -
295  // vpMath::sqr(xi_sig *
296  // inv_cst_const) );
297  } else {
298  // Outlier - could resize list of points tracked here?
299  weights[i] = 0;
300  }
301  }
302 }
303 
304 template <>
305 void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
306  const double NoiseThreshold)
307 {
308  if (residues.size() == 0) {
309  return;
310  }
311 
312  m_residues.resize(0);
313  m_residues.reserve(residues.size());
314  m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
315 
316  double med = getMedian(m_residues);
317 
318  m_normres.resize(residues.size());
319  for (size_t i = 0; i < m_residues.size(); i++) {
320  m_normres[i] = std::fabs(residues[(unsigned int)i] - med);
321  }
322 
323  m_residues = m_normres;
324  double normmedian = getMedian(m_residues);
325 
326  // 1.48 keeps scale estimate consistent for a normal probability dist.
327  double sigma = 1.4826 * normmedian; // median Absolute Deviation
328 
329  // Set a minimum threshold for sigma
330  // (when sigma reaches the level of noise in the image)
331  if (sigma < NoiseThreshold) {
332  sigma = NoiseThreshold;
333  }
334 
335  psiTukey(sigma, m_normres, weights);
336 }
337 
338 template <>
339 void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
340  const double NoiseThreshold)
341 {
342  if (residues.size() == 0) {
343  return;
344  }
345 
346  m_residues.resize(0);
347  m_residues.reserve(residues.size());
348  for (unsigned int i = 0; i < residues.size(); i++) {
349  m_residues.push_back((float)residues[i]);
350  }
351 
352  float med = getMedian(m_residues);
353 
354  m_normres.resize(residues.size());
355  for (size_t i = 0; i < m_residues.size(); i++) {
356  m_normres[i] = (float)std::fabs(residues[(unsigned int)i] - med);
357  }
358 
359  m_residues = m_normres;
360  float normmedian = getMedian(m_residues);
361 
362  // 1.48 keeps scale estimate consistent for a normal probability dist.
363  float sigma = 1.4826f * normmedian; // median Absolute Deviation
364 
365  // Set a minimum threshold for sigma
366  // (when sigma reaches the level of noise in the image)
367  if (sigma < NoiseThreshold) {
368  sigma = (float)NoiseThreshold;
369  }
370 
371  psiTukey(sigma, m_normres, weights);
372 }
373 
374 template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
375 {
376  T cst_const = static_cast<T>(4.6851);
377  T inv_cst_const = 1 / cst_const;
378  T inv_sig = 1 / sig;
379 
380  for (size_t i = 0; i < x.size(); i++) {
381 #if USE_ORIGINAL_TUKEY_CODE
382  if (std::fabs(sig) <= std::numeric_limits<T>::epsilon() &&
383  std::fabs(weights[i]) > std::numeric_limits<T>::epsilon() // sig should be equal to 0 only
384  // if NoiseThreshold == 0
385  ) {
386  weights[i] = 1;
387  continue;
388  }
389 #endif
390 
391  T xi_sig = x[i] * inv_sig;
392 
393  if ((std::fabs(xi_sig) <= cst_const)
394 #if USE_ORIGINAL_TUKEY_CODE
395  && std::fabs(weights[i]) > std::numeric_limits<T>::epsilon() // Consider the previous
396  // weights here
397 #endif
398  ) {
399  // vpMath::sqr( 1 - vpMath::sqr(xi_sig * inv_cst_const) );
400  weights[i] = (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)) *
401  (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const));
402  } else {
403  // Outlier - could resize list of points tracked here?
404  weights[i] = 0;
405  }
406  }
407 }
408 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
VISP_EXPORT bool checkSSSE3()
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244