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
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)
316 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
321 MEstimator_impl_simd(residues, weights, NoiseThreshold);
323 MEstimator_impl(residues, weights, NoiseThreshold);
330 inline void vpMbtTukeyEstimator<double>::MEstimator(
const std::vector<double> &residues, std::vector<double> &weights,
331 double NoiseThreshold)
334 #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON
339 MEstimator_impl_simd(residues, weights, NoiseThreshold);
341 MEstimator_impl(residues, weights, NoiseThreshold);
347 template <
typename T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights)
349 double C = sig * 4.6851;
352 for (
unsigned int i = 0; i < (
unsigned int)x.size(); i++) {
353 double xi = x[i] / C;
372 double NoiseThreshold)
374 if (residues.
size() == 0) {
379 m_residues.reserve(residues.
size());
380 m_residues.insert(m_residues.end(), &residues.
data[0], &residues.
data[residues.
size()]);
382 double med = getMedian(m_residues);
384 m_normres.resize(residues.
size());
385 for (
size_t i = 0; i < m_residues.size(); i++) {
386 m_normres[i] = std::fabs(residues[(
unsigned int)i] - med);
389 m_residues = m_normres;
390 double normmedian = getMedian(m_residues);
393 double sigma = 1.4826 * normmedian;
397 if (sigma < NoiseThreshold) {
398 sigma = NoiseThreshold;
401 psiTukey(sigma, m_normres, weights);
409 double NoiseThreshold)
411 if (residues.
size() == 0) {
415 m_residues.resize(0);
416 m_residues.reserve(residues.
size());
417 for (
unsigned int i = 0; i < residues.
size(); i++) {
418 m_residues.push_back((
float)residues[i]);
421 float med = getMedian(m_residues);
423 m_normres.resize(residues.
size());
424 for (
size_t i = 0; i < m_residues.size(); i++) {
425 m_normres[i] = (float)std::fabs(residues[(
unsigned int)i] - med);
428 m_residues = m_normres;
429 float normmedian = getMedian(m_residues);
432 float sigma = 1.4826f * normmedian;
436 if (sigma < NoiseThreshold) {
437 sigma = (float)NoiseThreshold;
440 psiTukey(sigma, m_normres, weights);
446 template <
class T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights)
448 T C =
static_cast<T
>(4.6851) * sig;
449 weights.resize(x.size());
452 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()