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;
60 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS 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) 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 #ifndef DOXYGEN_SHOULD_SKIP_THIS 106 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14) 107 auto AbsDiff = [](
const auto &a,
const auto &b) {
return std::fabs(a - b); };
109 template <
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); }
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 #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);
186 inline 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;
237 inline 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;
288 inline 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);
306 inline 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);
323 template <
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);
421 template <
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++) {
440 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
VISP_EXPORT bool checkSSSE3()
void resize(unsigned int i, bool flagNullify=true)
Implementation of column vector and the associated operations.