42 #include <visp3/core/vpTrackingException.h>
43 #include <visp3/me/vpMe.h>
44 #include <visp3/me/vpMeSite.h>
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 static bool horsImage(
int i,
int j,
int half,
int rows,
int cols)
49 int half_1 = half + 1;
50 int half_3 = half + 3;
51 return ((0 < (half_1 - i)) || (((i - rows) + half_3) > 0) || (0 < (half_1 - j)) || (((j - cols) + half_3) > 0));
63 m_selectDisplay =
NONE;
79 : 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),
80 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
84 : 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),
85 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
94 : 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),
95 m_weight(1), m_contrastThreshold(10000.0), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
105 m_selectDisplay =
NONE;
116 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp)
118 m_selectDisplay =
NONE;
120 m_i =
static_cast<int>(ip);
122 m_j =
static_cast<int>(jp);
129 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp,
const int &sign)
131 m_selectDisplay =
NONE;
133 m_i =
static_cast<int>(ip);
135 m_j =
static_cast<int>(jp);
142 void vpMeSite::init(
const double &ip,
const double &jp,
const double &alphap,
const double &convltp,
const int &sign,
const double &contrastThreshold)
144 m_selectDisplay =
NONE;
146 m_i =
static_cast<int>(ip);
148 m_j =
static_cast<int>(jp);
167 m_selectDisplay = m.m_selectDisplay;
175 unsigned int range_ =
static_cast<unsigned int>(range);
187 for (
int k = -range; k <= range; ++k) {
188 double ii =
m_ifloat + (k * salpha);
189 double jj =
m_jfloat + (k * calpha);
204 list_query_pixels[n] = pel;
208 return list_query_pixels;
215 int height_ =
static_cast<int>(I.
getHeight());
216 int width_ =
static_cast<int>(I.
getWidth());
220 half =
static_cast<int>((msize - 1) >> 1);
222 if (horsImage(
m_i,
m_j, half + me->
getStrip(), height_, width_)) {
229 double theta =
m_alpha + (M_PI / 2);
235 while (theta > M_PI) {
242 const int flatAngle = 180;
243 if (abs(thetadeg) == flatAngle) {
247 unsigned int index_mask =
static_cast<unsigned int>(thetadeg /
static_cast<double>(me->
getAngleStep()));
249 unsigned int i_ =
static_cast<unsigned int>(
m_i);
250 unsigned int j_ =
static_cast<unsigned int>(
m_j);
251 unsigned int half_ =
static_cast<unsigned int>(half);
253 unsigned int ihalf = i_ - half_;
254 unsigned int jhalf = j_ - half_;
256 for (
unsigned int a = 0; a < msize; ++a) {
257 unsigned int ihalfa = ihalf + a;
258 for (
unsigned int b = 0; b < msize; ++b) {
270 double max_convolution = 0;
276 unsigned int range = me->
getRange();
280 double contrast_max = 1 + me->
getMu2();
281 double contrast_min = 1 - me->
getMu1();
284 double *likelihood =
new double[(2 * range) + 1];
288 for (
unsigned int n = 0; n < ((2 * range) + 1); ++n) {
290 double convolution_ = list_query_pixels[n].
convolution(I, me);
294 threshold = 2.0 * threshold;
298 threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
304 likelihood[n] = fabs(convolution_ +
m_convlt);
306 if (likelihood[n] > threshold) {
308 if ((contrast > contrast_min) && (contrast < contrast_max) && (fabs(1 - contrast) < diff)) {
309 diff = fabs(1 - contrast);
310 max_convolution = convolution_;
312 max_rank =
static_cast<int>(n);
318 for (
unsigned int n = 0; n < ((2 * range) + 1); ++n) {
322 threshold = 2.0 * threshold;
326 threshold = threshold / (100.0 * n_d * trunc(n_d / 2.0));
330 double convolution_ = list_query_pixels[n].
convolution(I, me);
331 likelihood[n] = fabs(2 * convolution_);
332 if ((likelihood[n] > max) && (likelihood[n] > threshold)) {
333 max_convolution = convolution_;
335 max_rank =
static_cast<int>(n);
344 ip.
set_i(list_query_pixels[max_rank].
m_i);
345 ip.
set_j(list_query_pixels[max_rank].
m_j);
349 *
this = list_query_pixels[max_rank];
355 delete[] list_query_pixels;
366 if (std::fabs(contrast) > std::numeric_limits<double>::epsilon()) {
373 delete[] list_query_pixels;
393 const unsigned int crossSize = 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