42 #include <visp3/core/vpTrackingException.h>
43 #include <visp3/me/vpMe.h>
44 #include <visp3/me/vpMeSite.h>
48 #ifndef DOXYGEN_SHOULD_SKIP_THIS
49 static bool horsImage(
int i,
int j,
int half,
int rows,
int cols)
51 int half_1 = half + 1;
52 int half_3 = half + 3;
53 return ((0 < (half_1 - i)) || (((i - rows) + half_3) > 0) || (0 < (half_1 - j)) || (((j - cols) + half_3) > 0));
65 m_selectDisplay =
NONE;
81 : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
82 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
86 : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
87 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
96 : m_i(0), m_j(0), m_ifloat(0), m_jfloat(0), m_mask_sign(1), m_alpha(0.), m_convlt(0.), m_normGradient(0),
97 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
107 m_selectDisplay =
NONE;
118 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp)
120 m_selectDisplay =
NONE;
122 m_i =
static_cast<int>(ip);
124 m_j =
static_cast<int>(jp);
131 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp,
const int &sign)
133 m_selectDisplay =
NONE;
135 m_i =
static_cast<int>(ip);
137 m_j =
static_cast<int>(jp);
144 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp,
const int &sign,
const double &contrastThreshold)
146 m_selectDisplay =
NONE;
148 m_i =
static_cast<int>(ip);
150 m_j =
static_cast<int>(jp);
169 m_selectDisplay = m.m_selectDisplay;
177 unsigned int range_ =
static_cast<unsigned int>(range);
189 for (
int k = -range; k <= range; ++k) {
190 double ii =
m_ifloat + (k * salpha);
191 double jj =
m_jfloat + (k * calpha);
206 list_query_pixels[n] = pel;
210 return list_query_pixels;
217 int height_ =
static_cast<int>(I.
getHeight());
218 int width_ =
static_cast<int>(I.
getWidth());
222 half =
static_cast<int>((msize - 1) >> 1);
224 if (horsImage(
m_i,
m_j, half + me->
getStrip(), height_, width_)) {
231 double theta =
m_alpha + (M_PI / 2);
237 while (theta > M_PI) {
244 const int flatAngle = 180;
245 if (abs(thetadeg) == flatAngle) {
249 unsigned int index_mask =
static_cast<unsigned int>(thetadeg /
static_cast<double>(me->
getAngleStep()));
251 unsigned int i_ =
static_cast<unsigned int>(
m_i);
252 unsigned int j_ =
static_cast<unsigned int>(
m_j);
253 unsigned int half_ =
static_cast<unsigned int>(half);
255 unsigned int ihalf = i_ - half_;
256 unsigned int jhalf = j_ - half_;
258 for (
unsigned int a = 0; a < msize; ++a) {
259 unsigned int ihalfa = ihalf + a;
260 for (
unsigned int b = 0; b < msize; ++b) {
272 double max_convolution = 0;
278 unsigned int range = me->
getRange();
282 double contrast_max = 1 + me->
getMu2();
283 double contrast_min = 1 - me->
getMu1();
286 double *likelihood =
new double[(2 * range) + 1];
287 const unsigned int val_2 = 2;
291 for (
unsigned int n = 0; n < ((val_2 * range) + 1); ++n) {
293 double convolution_ = list_query_pixels[n].
convolution(I, me);
297 threshold = 2.0 * threshold;
301 threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
307 likelihood[n] = fabs(convolution_ +
m_convlt);
309 if (likelihood[n] > threshold) {
311 if ((contrast > contrast_min) && (contrast < contrast_max) && (fabs(1 - contrast) < diff)) {
312 diff = fabs(1 - contrast);
313 max_convolution = convolution_;
315 max_rank =
static_cast<int>(n);
321 for (
unsigned int n = 0; n < ((val_2 * range) + 1); ++n) {
325 threshold = 2.0 * threshold;
329 threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
333 double convolution_ = list_query_pixels[n].
convolution(I, me);
334 likelihood[n] = fabs(val_2 * convolution_);
335 if ((likelihood[n] > max) && (likelihood[n] > threshold)) {
336 max_convolution = convolution_;
338 max_rank =
static_cast<int>(n);
347 ip.
set_i(list_query_pixels[max_rank].
m_i);
348 ip.
set_j(list_query_pixels[max_rank].
m_j);
352 *
this = list_query_pixels[max_rank];
358 delete[] list_query_pixels;
369 if (std::fabs(contrast) > std::numeric_limits<double>::epsilon()) {
376 delete[] list_query_pixels;
391 const unsigned int crossSize = 3;
420 const unsigned int cross_size = 3;
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
static const vpColor cyan
static const vpColor blue
static const vpColor purple
static const vpColor yellow
static const vpColor green
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
static double sqr(double x)
static int round(double x)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int m_mask_sign
Mask sign.
@ OUTSIDE_ROI_MASK
Point is outside the region of interest mask, but retained in the ME list.
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
@ CONTRAST
Point not tracked due to a contrast problem, but retained in the ME list.
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
@ NO_SUPPRESSION
Point successfully tracked.
void setDisplay(vpMeSiteDisplayType select)
int operator!=(const vpMeSite &m)
void display(const vpImage< unsigned char > &I)
double m_ifloat
Subpixel coordinates along i of a site.
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int &range) const
double m_normGradient
Convolution of Site in previous image.
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
double getContrastThreshold() const
double m_convlt
Convolution of Site in previous image.
double m_alpha
Angle of tangent at site.
vpMeSite & operator=(const vpMeSite &m)
double m_contrastThreshold
Old likelihood ratio threshold (to be avoided) or easy-to-use normalized threshold: minimal contrast.
int m_j
Integer coordinates along j of a site.
int m_i
Integer coordinate along i of a site.
double m_jfloat
Subpixel coordinates along j of a site.
double m_weight
Uncertainty of point given as a probability between 0 and 1.
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
vpLikelihoodThresholdType getLikelihoodThresholdType() const
unsigned int getAngleStep() const
unsigned int getMaskSize() const
vpMatrix * getMask() const
unsigned int getRange() const