43 #include <visp3/core/vpImagePoint.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpRobust.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/me/vpMe.h>
48 #include <visp3/me/vpMeLine.h>
49 #include <visp3/me/vpMeSite.h>
50 #include <visp3/me/vpMeTracker.h>
54 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2);
56 static void normalizeAngle(
double &delta)
58 while (delta > M_PI) {
61 while (delta < -M_PI) {
66 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2)
69 double B = double(i1 - i2);
70 double A = double(j1 - j2);
74 normalizeAngle(delta);
77 static void project(
double a,
double b,
double c,
double i,
double j,
double &ip,
double &jp)
79 if (fabs(a) > fabs(b)) {
81 ip = (-c - b * jp) / a;
85 jp = (-c - a * ip) / b;
90 : m_rho(0.), m_theta(0.), m_delta(0.), m_delta_1(0.), m_angle(0.), m_angle_1(90), m_sign(1),
91 m_useIntensityForRho(true), m_a(0.), m_b(0.), m_c(0.)
95 :
vpMeTracker(meline), m_rho(0.), m_theta(0.), m_delta(0.), m_delta_1(0.), m_angle(0.), m_angle_1(90), m_sign(1),
96 m_useIntensityForRho(true), m_a(0.), m_b(0.), m_c(0.)
124 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
128 int nbrows =
static_cast<int>(I.
getHeight());
129 int nbcols =
static_cast<int>(I.
getWidth());
132 if (std::fabs(
m_me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
142 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
147 double stepi = diffsi / (double)n_sample;
148 double stepj = diffsj / (double)n_sample;
164 unsigned int is_uint =
static_cast<unsigned int>(is);
165 unsigned int js_uint =
static_cast<unsigned int>(js);
175 double contrastThreshold = fabs(convolution) * marginRatio;
188 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
189 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
203 std::cout <<
"Click on the line first point..." << std::endl;
208 std::cout <<
"Click on the line second point..." << std::endl;
238 unsigned int iter = 0;
239 unsigned int nos_1 = 0;
240 double distance = 100;
248 if ((fabs(
m_b) >= 0.9))
254 for (std::list<vpMeSite>::const_iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
264 while (iter < 4 && distance > 0.05) {
265 for (
unsigned int i = 0; i < k; i++) {
266 for (
unsigned int j = 0; j < 2; j++) {
267 DA[i][j] = w[i] * A[i][j];
278 for (
unsigned int i = 0; i < nos_1; i++) {
283 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
288 for (std::list<vpMeSite>::iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
317 for (std::list<vpMeSite>::const_iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
327 while (iter < 4 && distance > 0.05) {
336 for (
unsigned int i = 0; i < nos_1; i++) {
341 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
346 for (std::list<vpMeSite>::iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
375 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
377 int i1s, j1s, i2s, j2s;
394 double angle_ = atan2((
double)(i1s - i2s), (
double)(j1s - j2s));
401 computeDelta(
m_delta, i1s, j1s, i2s, j2s);
421 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
427 for (std::list<vpMeSite>::iterator it =
m_meList.begin(); it !=
m_meList.end();) {
445 for (std::list<vpMeSite>::const_iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
463 if (fabs(imin - imax) < 25) {
464 for (std::list<vpMeSite>::const_iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
485 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
488 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
492 int nbrows =
static_cast<int>(I.
getHeight());
493 int nbcols =
static_cast<int>(I.
getWidth());
497 if (std::fabs(
m_me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
509 double di = diffsi / sqrt(s);
510 double dj = diffsj / sqrt(s);
526 for (
int i = 0; i < 3; i++) {
536 unsigned int is_uint =
static_cast<unsigned int>(P.
m_ifloat);
537 unsigned int js_uint =
static_cast<unsigned int>(P.
m_jfloat);
558 for (
int i = 0; i < 3; i++) {
568 unsigned int is_uint =
static_cast<unsigned int>(P.
m_ifloat);
569 unsigned int js_uint =
static_cast<unsigned int>(P.
m_jfloat);
590 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
591 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
596 double i1, j1, i2, j2;
599 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
617 if ((
double)n < 0.9 * expecteddensity) {
631 double angle_ =
m_delta + M_PI / 2;
636 while (angle_ > M_PI)
642 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
653 for (std::list<vpMeSite>::iterator it =
m_meList.begin(); it !=
m_meList.end(); ++it) {
664 vpCDEBUG(1) <<
"begin vpMeLine::track()" << std::endl;
717 vpCDEBUG(1) <<
"end vpMeLine::track()" << std::endl;
720 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int &i1,
int &i2,
int &j1,
int &j2)
722 i1 = (int)(i + cos(theta) * incr);
723 j1 = (int)(j + sin(theta) * incr);
725 i2 = (int)(i - cos(theta) * incr);
726 j2 = (int)(j - sin(theta) * incr);
748 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
749 unsigned char v1 = 0, v2 = 0;
753 update_indices(
m_theta, i, j, incr, i1, i2, j1, j2);
755 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
756 double rho_lim1 = fabs((
double)i / cos(
m_theta));
757 double rho_lim2 = fabs((
double)j / sin(
m_theta));
759 double co_rho_lim1 = fabs(((
double)(height_ - i)) / cos(
m_theta));
760 double co_rho_lim2 = fabs(((
double)(width_ - j)) / sin(
m_theta));
762 double rho_lim = std::min<double>(rho_lim1, rho_lim2);
763 double co_rho_lim = std::min<double>(co_rho_lim1, co_rho_lim2);
764 incr = (int)std::floor(std::min<double>(rho_lim, co_rho_lim));
765 if (incr < INCR_MIN) {
769 update_indices(
m_theta, i, j, incr, i1, i2, j1, j2);
774 unsigned int i1_ =
static_cast<unsigned int>(i1);
775 unsigned int j1_ =
static_cast<unsigned int>(j1);
776 unsigned int i2_ =
static_cast<unsigned int>(i2);
777 unsigned int j2_ =
static_cast<unsigned int>(j2);
780 if (abs(v1 - v2) < 1) {
785 "there is no gray level difference between both "
786 "sides of the line"));
789 update_indices(
m_theta, i, j, incr, i1, i2, j1, j2);
847 double a1 = line1.
m_a;
848 double b1 = line1.
m_b;
849 double c1 = line1.
m_c;
850 double a2 = line2.
m_a;
851 double b2 = line2.
m_b;
852 double c2 = line2.
m_c;
859 denom = (-(a2 / a1) * b1 + b2);
862 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
863 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
868 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
869 j = ((a2 / a1) * c1 - c2) / denom;
870 i = (-b1 * j - c1) / a1;
875 denom = (-(b2 / b1) * a1 + a2);
878 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
879 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
884 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
885 i = ((b2 / b1) * c1 - c2) / denom;
886 j = (-a1 * i - c1) / b1;
899 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
922 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
949 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
978 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
979 const vpColor &color,
unsigned int thickness)
1008 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1009 const vpColor &color,
unsigned int thickness)
1017 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1021 if (fabs(A) < fabs(B)) {
1022 double i1, j1, i2, j2;
1024 j1 = (-A * i1 - C) / B;
1026 j2 = (-A * i2 - C) / B;
1035 double i1, j1, i2, j2;
1037 i1 = -(B * j1 + C) / A;
1039 i2 = -(B * j2 + C) / A;
1058 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1062 if (fabs(A) < fabs(B)) {
1063 double i1, j1, i2, j2;
1065 j1 = (-A * i1 - C) / B;
1067 j2 = (-A * i2 - C) / B;
1076 double i1, j1, i2, j2;
1078 i1 = -(B * j1 + C) / A;
1080 i2 = -(B * j2 + C) / A;
1099 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1100 const vpColor &color,
unsigned int thickness)
1104 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1117 if (fabs(A) < fabs(B)) {
1118 double i1, j1, i2, j2;
1120 j1 = (-A * i1 - C) / B;
1122 j2 = (-A * i2 - C) / B;
1131 double i1, j1, i2, j2;
1133 i1 = -(B * j1 + C) / A;
1135 i2 = -(B * j2 + C) / A;
1154 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1155 const vpColor &color,
unsigned int thickness)
1159 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1172 if (fabs(A) < fabs(B)) {
1173 double i1, j1, i2, j2;
1175 j1 = (-A * i1 - C) / B;
1177 j2 = (-A * i2 - C) / B;
1186 double i1, j1, i2, j2;
1188 i1 = -(B * j1 + C) / A;
1190 i2 = -(B * j2 + C) / A;
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor blue
static const vpColor green
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emitted by ViSP classes.
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)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that tracks in an image a line moving edges.
double m_angle
Angle in deg between the extremities.
double m_angle_1
Angle in deg between the extremities.
double m_theta
theta parameter of the line
double m_delta_1
Angle in rad between the extremities.
virtual ~vpMeLine() vp_override
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) vp_override
void computeRhoTheta(const vpImage< unsigned char > &I)
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
void reSample(const vpImage< unsigned char > &I)
double m_c
Parameter c of the line equation a*i + b*j + c = 0.
bool m_useIntensityForRho
double m_delta
Angle in rad between the extremities.
double m_rho
rho parameter of the line
void seekExtremities(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
static void displayLine(const vpImage< unsigned char > &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, const double &B, const double &C, const vpColor &color=vpColor::green, unsigned int thickness=1)
double m_a
Parameter a of the line equation a*i + b*j + c = 0.
double m_b
Parameter b of the line equation a*i + b*j + c = 0.
vpMeSite m_PExt[2]
Line extremities.
void initTracking(const vpImage< unsigned char > &I)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int m_mask_sign
Mask sign.
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
@ NO_SUPPRESSION
Point successfully tracked.
void setDisplay(vpMeSiteDisplayType select)
double m_ifloat
Subpixel coordinates along i of a site.
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
void setAlpha(const double &a)
vpMeSiteState getState() const
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.
void setContrastThreshold(const double &thresh, const vpMe &me)
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
void setState(const vpMeSiteState &flag)
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
void track(const vpImage< unsigned char > &I)
vpMe * m_me
Moving edges initialisation parameters.
static bool inRoiMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
void display(const vpImage< unsigned char > &I)
bool outOfImage(int i, int j, int border, int nrows, int ncols)
const vpImage< bool > * m_maskCandidates
Mask used to determine candidate points for initialization in an image.
std::list< vpMeSite > m_meList
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void setRange(const unsigned int &range)
double getThresholdMarginRatio() const
double getSampleStep() const
unsigned int getRange() const
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ initializationError
Tracker initialization error.
@ fatalError
Tracker fatal error.
#define vpDEBUG_ENABLE(level)