Visual Servoing Platform version 3.5.0
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
44template <typename T> class vpMbtTukeyEstimator
45{
46public:
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
50private:
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
104namespace
105{
106#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
107auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
108#else
109template <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
116template class vpMbtTukeyEstimator<float>;
117template class vpMbtTukeyEstimator<double>;
118
119#if VISP_HAVE_SSSE3
120namespace
121{
122inline __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
130template <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)'
144template <typename T>
145void 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
185template <>
186inline 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
236template <>
237inline 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
287template <>
288inline 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
305template <>
306inline 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
323template <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
345template <>
346inline 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
382template <>
383inline 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
421template <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:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
VISP_EXPORT bool checkSSSE3()