36#ifndef _vpMbtTukeyEstimator_h_
37#define _vpMbtTukeyEstimator_h_
40#include <visp3/core/vpColVector.h>
42#ifndef DOXYGEN_SHOULD_SKIP_THIS
44template <
typename T>
class vpMbtTukeyEstimator
47 void MEstimator(
const std::vector<T> &residues, std::vector<T> &weights,
const T NoiseThreshold);
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);
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 (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && USE_TRANSFORM
83#define HAVE_TRANSFORM 1
87#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
89#define VISP_HAVE_SSE2 1
91#if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
93#define VISP_HAVE_SSE3 1
95#if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
97#define VISP_HAVE_SSSE3 1
101#ifndef DOXYGEN_SHOULD_SKIP_THIS
106#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
107auto AbsDiff = [](
const auto &a,
const auto &b) {
return std::fabs(a - b); };
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); }
116template class vpMbtTukeyEstimator<float>;
117template class vpMbtTukeyEstimator<double>;
122inline __m128 abs_ps(__m128 x)
124 static const __m128 sign_mask = _mm_set1_ps(-0.f);
125 return _mm_andnot_ps(sign_mask, x);
130template <
typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
133 int index = (int)(ceil(vec.size() / 2.0)) - 1;
134 std::nth_element(vec.begin(), vec.begin() + index, vec.end());
145void vpMbtTukeyEstimator<T>::MEstimator_impl(
const std::vector<T> &residues, std::vector<T> &weights,
146 const T NoiseThreshold)
148 if (residues.empty()) {
152 m_residues = residues;
154 T med = getMedian(m_residues);
155 m_normres.resize(residues.size());
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));
161 std::transform(residues.begin(), residues.end(), m_normres.begin(),
162 std::bind(AbsDiff<T>(), std::placeholders::_1, med));
165 for (
size_t i = 0; i < m_residues.size(); i++) {
166 m_normres[i] = (std::fabs(residues[i] - med));
170 m_residues = m_normres;
171 T normmedian = getMedian(m_residues);
174 T sigma =
static_cast<T
>(1.4826 * normmedian);
178 if (sigma < NoiseThreshold) {
179 sigma = NoiseThreshold;
182 psiTukey(sigma, m_normres, weights);
186inline void vpMbtTukeyEstimator<float>::MEstimator_impl_ssse3(
const std::vector<float> &residues,
187 std::vector<float> &weights,
const float NoiseThreshold)
190 if (residues.empty()) {
194 m_residues = residues;
196 float med = getMedian(m_residues);
197 m_normres.resize(residues.size());
200 __m128 med_128 = _mm_set_ps1(med);
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)));
209 for (; i < m_residues.size(); i++) {
210 m_normres[i] = (std::fabs(residues[i] - med));
213 m_residues = m_normres;
214 float normmedian = getMedian(m_residues);
217 float sigma = 1.4826f * normmedian;
221 if (sigma < NoiseThreshold) {
222 sigma = NoiseThreshold;
225 psiTukey(sigma, m_normres, weights);
229 (void)NoiseThreshold;
237inline void vpMbtTukeyEstimator<double>::MEstimator_impl_ssse3(
const std::vector<double> &residues,
238 std::vector<double> &weights,
239 const double NoiseThreshold)
242 if (residues.empty()) {
246 m_residues = residues;
248 double med = getMedian(m_residues);
249 m_normres.resize(residues.size());
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));
255 std::transform(residues.begin(), residues.end(), m_normres.begin(),
256 std::bind(AbsDiff<double>(), std::placeholders::_1, med));
259 for (
size_t i = 0; i < m_residues.size(); i++) {
260 m_normres[i] = (std::fabs(residues[i] - med));
264 m_residues = m_normres;
265 double normmedian = getMedian(m_residues);
268 double sigma = 1.4826 * normmedian;
272 if (sigma < NoiseThreshold) {
273 sigma = NoiseThreshold;
276 psiTukey(sigma, m_normres, weights);
280 (void)NoiseThreshold;
288inline void vpMbtTukeyEstimator<float>::MEstimator(
const std::vector<float> &residues, std::vector<float> &weights,
289 const float NoiseThreshold)
297 MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
299 MEstimator_impl(residues, weights, NoiseThreshold);
306inline void vpMbtTukeyEstimator<double>::MEstimator(
const std::vector<double> &residues, std::vector<double> &weights,
307 const double NoiseThreshold)
315 MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
317 MEstimator_impl(residues, weights, NoiseThreshold);
323template <
typename T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights)
325 double C = sig * 4.6851;
328 for (
unsigned int i = 0; i < (
unsigned int)x.size(); i++) {
329 double xi = x[i] / C;
347 const double NoiseThreshold)
349 if (residues.
size() == 0) {
354 m_residues.reserve(residues.
size());
355 m_residues.insert(m_residues.end(), &residues.
data[0], &residues.
data[residues.
size()]);
357 double med = getMedian(m_residues);
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);
364 m_residues = m_normres;
365 double normmedian = getMedian(m_residues);
368 double sigma = 1.4826 * normmedian;
372 if (sigma < NoiseThreshold) {
373 sigma = NoiseThreshold;
376 psiTukey(sigma, m_normres, weights);
384 const double NoiseThreshold)
386 if (residues.
size() == 0) {
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]);
396 float med = getMedian(m_residues);
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);
403 m_residues = m_normres;
404 float normmedian = getMedian(m_residues);
407 float sigma = 1.4826f * normmedian;
411 if (sigma < NoiseThreshold) {
412 sigma = (float)NoiseThreshold;
415 psiTukey(sigma, m_normres, weights);
421template <
class T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights)
423 T C =
static_cast<T
>(4.6851) * sig;
424 weights.resize(x.size());
427 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()