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)
51 int half_1 = half + 1;
52 int half_3 = half + 3;
55 return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0));
66 m_selectDisplay =
NONE;
82 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
83 weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
87 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
88 weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
97 : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
98 weight(1), m_selectDisplay(NONE), m_state(NO_SUPPRESSION)
108 m_selectDisplay =
NONE;
121 m_selectDisplay =
NONE;
132 void vpMeSite::init(
double ip,
double jp,
double alphap,
double convltp,
int sign)
134 m_selectDisplay =
NONE;
155 m_selectDisplay = m.m_selectDisplay;
163 unsigned int range_ =
static_cast<unsigned int>(range);
170 double salpha = sin(
alpha);
171 double calpha = cos(
alpha);
175 for (
int k = -range; k <= range; k++) {
176 double ii = (
ifloat + k * salpha);
177 double jj = (
jfloat + k * calpha);
192 list_query_pixels[n] = pel;
196 return (list_query_pixels);
203 int height_ =
static_cast<int>(I.
getHeight());
204 int width_ =
static_cast<int>(I.
getWidth());
208 half = (
static_cast<int>(msize) - 1) >> 1;
210 if (horsImage(
i,
j, half + me->
getStrip(), height_, width_)) {
217 double theta =
alpha + M_PI / 2;
228 if (abs(thetadeg) == 180) {
232 unsigned int index_mask = (
unsigned int)(thetadeg / (
double)me->
getAngleStep());
234 unsigned int i_ =
static_cast<unsigned int>(
i);
235 unsigned int j_ =
static_cast<unsigned int>(
j);
236 unsigned int half_ =
static_cast<unsigned int>(half);
238 unsigned int ihalf = i_ - half_;
239 unsigned int jhalf = j_ - half_;
241 for (
unsigned int a = 0; a < msize; a++) {
242 unsigned int ihalfa = ihalf + a;
243 for (
unsigned int b = 0; b < msize; b++) {
255 double max_convolution = 0;
261 unsigned int range = me->
getRange();
265 double contrast_max = 1 + me->
getMu2();
266 double contrast_min = 1 - me->
getMu1();
269 double *likelihood =
new double[2 * range + 1];
277 threshold = me->
getThreshold() / (100.0 * n_d * trunc(n_d / 2.0));
282 for (
unsigned int n = 0; n < 2 * range + 1; n++) {
284 double convolution_ = list_query_pixels[n].
convolution(I, me);
289 likelihood[n] = fabs(convolution_ +
convlt);
290 if (likelihood[n] > threshold) {
291 contrast = convolution_ /
convlt;
292 if ((contrast > contrast_min) && (contrast < contrast_max) && fabs(1 - contrast) < diff) {
293 diff = fabs(1 - contrast);
294 max_convolution = convolution_;
302 for (
unsigned int n = 0; n < 2 * range + 1; n++) {
304 double convolution_ = list_query_pixels[n].
convolution(I, me);
305 likelihood[n] = fabs(2 * convolution_);
306 if (likelihood[n] > max && likelihood[n] > threshold) {
307 max_convolution = convolution_;
318 ip.
set_i(list_query_pixels[max_rank].
i);
319 ip.
set_j(list_query_pixels[max_rank].
j);
323 *
this = list_query_pixels[max_rank];
329 delete[] list_query_pixels;
335 ip.
set_i(list_query_pixels[0].
i);
336 ip.
set_j(list_query_pixels[0].
j);
340 if (std::fabs(contrast) > std::numeric_limits<double>::epsilon())
345 delete[] list_query_pixels;
354 return (os <<
"Alpha: " << vpMeS.
alpha <<
" Convolution: " << vpMeS.
convlt <<
" Weight: " << vpMeS.
weight);
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 j
Coordinates along j of a site.
@ TOO_NEAR
Point removed because too near image borders.
@ THRESHOLD
Point removed due to a threshold problem.
@ CONTRAST
Point removed due to a contrast problem.
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
@ NO_SUPPRESSION
Point used by the tracker.
void setDisplay(vpMeSiteDisplayType select)
double weight
Uncertainty of point given as a probability between 0 and 1.
int operator!=(const vpMeSite &m)
void display(const vpImage< unsigned char > &I)
double ifloat
Floating coordinates along i of a site.
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
int i
Coordinate along i of a site.
vpMeSite & operator=(const vpMeSite &m)
double normGradient
Convolution of Site in previous image.
double alpha
Angle of tangent at site.
double jfloat
Floating coordinates along j of a site.
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
double convlt
Convolution of Site in previous image.
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
vpLikelihoodThresholdType getLikelihoodThresholdType() const
unsigned int getAngleStep() const
double getThreshold() const
unsigned int getMaskSize() const
vpMatrix * getMask() const
unsigned int getRange() const