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,
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;
80 #include <visp3/core/vpCPUFeatures.h>
82 #define USE_TRANSFORM 1
83 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && USE_TRANSFORM
84 #define HAVE_TRANSFORM 1
91 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
92 #include <emmintrin.h>
93 #define VISP_HAVE_SSE2 1
95 #if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
96 #include <pmmintrin.h>
97 #define VISP_HAVE_SSE3 1
99 #if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
100 #include <tmmintrin.h>
101 #define VISP_HAVE_SSSE3 1
105 #ifndef DOXYGEN_SHOULD_SKIP_THIS
110 template <
typename T>
struct AbsDiff :
public std::binary_function<T, T, T> {
111 T operator()(
const T a,
const T b)
const {
return std::fabs(a - b); }
116 template class vpMbtTukeyEstimator<float>;
117 template class vpMbtTukeyEstimator<double>;
122 inline __m128 abs_ps(__m128 x)
124 static const __m128 sign_mask = _mm_set1_ps(-0.f);
125 return _mm_andnot_ps(sign_mask, x);
130 template <
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());
144 template <
typename T>
145 void 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 std::transform(residues.begin(), residues.end(), m_normres.begin(),
159 std::bind(AbsDiff<T>(), std::placeholders::_1, med));
161 for (
size_t i = 0; i < m_residues.size(); i++) {
162 m_normres[i] = (std::fabs(residues[i] - med));
166 m_residues = m_normres;
167 T normmedian = getMedian(m_residues);
170 T sigma =
static_cast<T
>(1.4826 * normmedian);
174 if (sigma < NoiseThreshold) {
175 sigma = NoiseThreshold;
178 psiTukey(sigma, m_normres, weights);
182 inline void vpMbtTukeyEstimator<float>::MEstimator_impl_ssse3(
const std::vector<float> &residues, std::vector<float> &weights,
183 const float NoiseThreshold)
186 if (residues.empty()) {
190 m_residues = residues;
192 float med = getMedian(m_residues);
193 m_normres.resize(residues.size());
196 __m128 med_128 = _mm_set_ps1(med);
198 if (m_residues.size() >= 4) {
199 for (i = 0; i <= m_residues.size() - 4; i += 4) {
200 __m128 residues_128 = _mm_loadu_ps(residues.data() + i);
201 _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128)));
205 for (; i < m_residues.size(); i++) {
206 m_normres[i] = (std::fabs(residues[i] - med));
209 m_residues = m_normres;
210 float normmedian = getMedian(m_residues);
213 float sigma = 1.4826f * normmedian;
217 if (sigma < NoiseThreshold) {
218 sigma = NoiseThreshold;
221 psiTukey(sigma, m_normres, weights);
225 (void)NoiseThreshold;
233 inline void vpMbtTukeyEstimator<double>::MEstimator_impl_ssse3(
const std::vector<double> &residues,
234 std::vector<double> &weights,
const double NoiseThreshold)
237 if (residues.empty()) {
241 m_residues = residues;
243 double med = getMedian(m_residues);
244 m_normres.resize(residues.size());
247 std::transform(residues.begin(), residues.end(), m_normres.begin(),
248 std::bind(AbsDiff<double>(), std::placeholders::_1, med));
250 for (
size_t i = 0; i < m_residues.size(); i++) {
251 m_normres[i] = (std::fabs(residues[i] - med));
255 m_residues = m_normres;
256 double normmedian = getMedian(m_residues);
259 double sigma = 1.4826 * normmedian;
263 if (sigma < NoiseThreshold) {
264 sigma = NoiseThreshold;
267 psiTukey(sigma, m_normres, weights);
271 (void)NoiseThreshold;
279 inline void vpMbtTukeyEstimator<float>::MEstimator(
const std::vector<float> &residues, std::vector<float> &weights,
280 const float NoiseThreshold)
288 MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
290 MEstimator_impl(residues, weights, NoiseThreshold);
297 inline void vpMbtTukeyEstimator<double>::MEstimator(
const std::vector<double> &residues, std::vector<double> &weights,
298 const double NoiseThreshold)
306 MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
308 MEstimator_impl(residues, weights, NoiseThreshold);
314 template <
typename T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x,
vpColVector &weights)
316 double C = sig * 4.6851;
319 for (
unsigned int i = 0; i < (
unsigned int)x.size(); i++) {
320 double xi = x[i] / C;
338 const double NoiseThreshold)
340 if (residues.
size() == 0) {
345 m_residues.reserve(residues.
size());
346 m_residues.insert(m_residues.end(), &residues.
data[0], &residues.
data[residues.
size()]);
348 double med = getMedian(m_residues);
350 m_normres.resize(residues.
size());
351 for (
size_t i = 0; i < m_residues.size(); i++) {
352 m_normres[i] = std::fabs(residues[(
unsigned int)i] - med);
355 m_residues = m_normres;
356 double normmedian = getMedian(m_residues);
359 double sigma = 1.4826 * normmedian;
363 if (sigma < NoiseThreshold) {
364 sigma = NoiseThreshold;
367 psiTukey(sigma, m_normres, weights);
375 const double NoiseThreshold)
377 if (residues.
size() == 0) {
381 m_residues.resize(0);
382 m_residues.reserve(residues.
size());
383 for (
unsigned int i = 0; i < residues.
size(); i++) {
384 m_residues.push_back((
float)residues[i]);
387 float med = getMedian(m_residues);
389 m_normres.resize(residues.
size());
390 for (
size_t i = 0; i < m_residues.size(); i++) {
391 m_normres[i] = (float)std::fabs(residues[(
unsigned int)i] - med);
394 m_residues = m_normres;
395 float normmedian = getMedian(m_residues);
398 float sigma = 1.4826f * normmedian;
402 if (sigma < NoiseThreshold) {
403 sigma = (float)NoiseThreshold;
406 psiTukey(sigma, m_normres, weights);
412 template <
class T>
void vpMbtTukeyEstimator<T>::psiTukey(
const T sig, std::vector<T> &x, std::vector<T> &weights)
414 T C =
static_cast<T
>(4.6851) * sig;
415 weights.resize(x.size());
418 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()