Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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/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, T NoiseThreshold);
48 void MEstimator(const vpColVector &residues, vpColVector &weights, double NoiseThreshold);
49
50private:
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
114namespace
115{
116#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
117auto AbsDiff = [](const auto &a, const auto &b) { return std::fabs(a - b); };
118#else
119template <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
126template class vpMbtTukeyEstimator<float>;
127template class vpMbtTukeyEstimator<double>;
128
129#if VISP_HAVE_SSSE3
130namespace
131{
132inline __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
140template <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)'
154template <typename T>
155void 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
195template <>
196inline 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
256template <>
257inline 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
307template <>
308inline void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
309 float NoiseThreshold)
310{
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
325template <>
326inline void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
327 double NoiseThreshold)
328{
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
343template <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
365template <>
366inline 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
402template <>
403inline 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
441template <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:144
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()