36 #ifndef _vpMbtTukeyEstimator_h_
37 #define _vpMbtTukeyEstimator_h_
40 #include <visp3/core/vpColVector.h>
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
44 template <
typename T>
class vpMbtTukeyEstimator
47 void MEstimator(
const std::vector<T> &residues, std::vector<T> &weights, T NoiseThreshold);
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);
57 std::vector<T> m_normres;
58 std::vector<T> m_residues;
79 #include <visp3/core/vpCPUFeatures.h>
81 #define USE_TRANSFORM 1
82 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) && USE_TRANSFORM
83 #define HAVE_TRANSFORM 1
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
91 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
92 #include <pmmintrin.h>
93 #define VISP_HAVE_SSE3 1
95 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
96 #include <tmmintrin.h>
97 #define VISP_HAVE_SSSE3 1
101 #if defined _WIN32 && defined(_M_ARM64)
102 # define _ARM64_DISTINCT_NEON_TYPES
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
111 #ifndef DOXYGEN_SHOULD_SKIP_THIS
117 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
118 auto AbsDiff = [](
const auto &a,
const auto &b) {
return std::fabs(a - b); };
120 template <
typename T>
struct AbsDiff :
public std::binary_function<T, T, T>
122 T operator()(
const T a,
const T b)
const {
return std::fabs(a - b); }
128 template class vpMbtTukeyEstimator<float>;
129 template class vpMbtTukeyEstimator<double>;
134 inline __m128 abs_ps(__m128 x)
136 static const __m128 sign_mask = _mm_set1_ps(-0.f);
137 return _mm_andnot_ps(sign_mask, x);
142 template <
typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
145 int index = (int)(ceil(vec.size() / 2.0)) - 1;
146 std::nth_element(vec.begin(), vec.begin() + index, vec.end());
156 template <
typename T>
157 void vpMbtTukeyEstimator<T>::MEstimator_impl(
const std::vector<T> &residues, std::vector<T> &weights,
158 const T NoiseThreshold)
160 if (residues.empty()) {
164 m_residues = residues;
166 T med = getMedian(m_residues);
167 m_normres.resize(residues.size());
171 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
172 std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
174 std::transform(residues.begin(), residues.end(), m_normres.begin(),
175 std::bind(AbsDiff<T>(), std::placeholders::_1, med));
178 for (
size_t i = 0; i < m_residues.size(); i++) {
179 m_normres[i] = (std::fabs(residues[i] - med));
183 m_residues = m_normres;
184 T normmedian = getMedian(m_residues);
187 T sigma =
static_cast<T
>(1.4826 * normmedian);
191 if (sigma < NoiseThreshold) {
192 sigma = NoiseThreshold;
195 psiTukey(sigma, m_normres, weights);
199 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_simd(
const std::vector<float> &residues,
200 std::vector<float> &weights,
201 float NoiseThreshold)
203 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
204 if (residues.empty()) {
208 m_residues = residues;
210 float med = getMedian(m_residues);
211 m_normres.resize(residues.size());
215 __m128 med_128 = _mm_set_ps1(med);
217 float32x4_t med_128 = vdupq_n_f32(med);
220 if (m_residues.size() >= 4) {
221 for (i = 0; i <= m_residues.size() - 4; i += 4) {
223 __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
224 _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
226 float32x4_t residues_128 = vld1q_f32(residues.data() + i);
227 vst1q_f32(m_normres.data() + i, vabsq_f32(vsubq_f32(residues_128, med_128)));
232 for (; i < m_residues.size(); i++) {
233 m_normres[i] = (std::fabs(residues[i] - med));
236 m_residues = m_normres;
237 float normmedian = getMedian(m_residues);
240 float sigma = 1.4826f * normmedian;
244 if (sigma < NoiseThreshold) {
245 sigma = NoiseThreshold;
248 psiTukey(sigma, m_normres, weights);
252 (void)NoiseThreshold;
260 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_simd(
const std::vector<double> &residues,
261 std::vector<double> &weights,
262 double NoiseThreshold)
264 #if VISP_HAVE_SSSE3 || VISP_HAVE_NEON
265 if (residues.empty()) {
269 m_residues = residues;
271 double med = getMedian(m_residues);
272 m_normres.resize(residues.size());
276 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
277 std::transform(residues.begin(), residues.end(), m_normres.begin(), std::bind(AbsDiff, std::placeholders::_1, med));
279 std::transform(residues.begin(), residues.end(), m_normres.begin(),
280 std::bind(AbsDiff<double>(), std::placeholders::_1, med));
283 for (
size_t i = 0; i < m_residues.size(); i++) {
284 m_normres[i] = (std::fabs(residues[i] - med));
288 m_residues = m_normres;
289 double normmedian = getMedian(m_residues);
292 double sigma = 1.4826 * normmedian;
296 if (sigma < NoiseThreshold) {
297 sigma = NoiseThreshold;
300 psiTukey(sigma, m_normres, weights);
304 (void)NoiseThreshold;
312 inline void vpMbtTukeyEstimator<float>::MEstimator(
const std::vector<float> &residues, std::vector<float> &weights,
313 float NoiseThreshold)
315 #if defined(VISP_HAVE_SIMDLIB)
320 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
325 MEstimator_impl_simd(residues, weights, NoiseThreshold);
327 MEstimator_impl(residues, weights, NoiseThreshold);
334 inline void vpMbtTukeyEstimator<double>::MEstimator(
const std::vector<double> &residues, std::vector<double> &weights,
335 double NoiseThreshold)
337 #if defined(VISP_HAVE_SIMDLIB)
342 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
347 MEstimator_impl_simd(residues, weights, NoiseThreshold);
349 MEstimator_impl(residues, weights, NoiseThreshold);
355 template <
typename T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights)
357 double C = sig * 4.6851;
360 for (
unsigned int i = 0; i < (
unsigned int)x.size(); i++) {
361 double xi = x[i] / C;
380 double NoiseThreshold)
382 if (residues.
size() == 0) {
387 m_residues.reserve(residues.
size());
388 m_residues.insert(m_residues.end(), &residues.
data[0], &residues.
data[residues.
size()]);
390 double med = getMedian(m_residues);
392 m_normres.resize(residues.
size());
393 for (
size_t i = 0; i < m_residues.size(); i++) {
394 m_normres[i] = std::fabs(residues[(
unsigned int)i] - med);
397 m_residues = m_normres;
398 double normmedian = getMedian(m_residues);
401 double sigma = 1.4826 * normmedian;
405 if (sigma < NoiseThreshold) {
406 sigma = NoiseThreshold;
409 psiTukey(sigma, m_normres, weights);
417 double NoiseThreshold)
419 if (residues.
size() == 0) {
423 m_residues.resize(0);
424 m_residues.reserve(residues.
size());
425 for (
unsigned int i = 0; i < residues.
size(); i++) {
426 m_residues.push_back((
float)residues[i]);
429 float med = getMedian(m_residues);
431 m_normres.resize(residues.
size());
432 for (
size_t i = 0; i < m_residues.size(); i++) {
433 m_normres[i] = (float)std::fabs(residues[(
unsigned int)i] - med);
436 m_residues = m_normres;
437 float normmedian = getMedian(m_residues);
440 float sigma = 1.4826f * normmedian;
444 if (sigma < NoiseThreshold) {
445 sigma = (float)NoiseThreshold;
448 psiTukey(sigma, m_normres, weights);
454 template <
class T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights)
456 T C =
static_cast<T
>(4.6851) * sig;
457 weights.resize(x.size());
460 for (
size_t i = 0; i < x.size(); i++) {
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
VISP_EXPORT bool checkSSSE3()
VISP_EXPORT bool checkNeon()