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 : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.),
95 :
vpMeTracker(meline), rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
96 _useIntensityForRho(true), a(0.), b(0.), c(0.)
121 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
129 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
139 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
144 double stepi = diffsi / (double)n_sample;
145 double stepj = diffsj / (double)n_sample;
177 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
178 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
192 std::cout <<
"Click on the line first point..." << std::endl;
197 std::cout <<
"Click on the line second point..." << std::endl;
227 unsigned int iter = 0;
228 unsigned int nos_1 = 0;
229 double distance = 100;
237 if ((fabs(
b) >= 0.9))
243 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
253 while (iter < 4 && distance > 0.05) {
254 for (
unsigned int i = 0; i < k; i++) {
255 for (
unsigned int j = 0; j < 2; j++) {
256 DA[i][j] = w[i] * A[i][j];
267 for (
unsigned int i = 0; i < nos_1; i++) {
272 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
277 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
306 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
316 while (iter < 4 && distance > 0.05) {
325 for (
unsigned int i = 0; i < nos_1; i++) {
330 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
335 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
359 normalizeAngle(
delta);
364 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
366 int i1s, j1s, i2s, j2s;
383 double angle_ = atan2((
double)(i1s - i2s), (
double)(j1s - j2s));
390 computeDelta(
delta, i1s, j1s, i2s, j2s);
410 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
416 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end();) {
434 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
452 if (fabs(imin - imax) < 25) {
453 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
474 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
477 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
486 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
498 double di = diffsi / sqrt(s);
499 double dj = diffsj / sqrt(s);
516 for (
int i = 0; i < 3; i++) {
546 for (
int i = 0; i < 3; i++) {
575 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
576 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
581 double i1, j1, i2, j2;
584 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
588 project(
a,
b,
c,
PExt[0].ifloat,
PExt[0].jfloat, i1, j1);
589 project(
a,
b,
c,
PExt[1].ifloat,
PExt[1].jfloat, i2, j2);
602 if ((
double)n < 0.9 * expecteddensity) {
603 double delta_new =
delta;
618 double angle_ =
delta + M_PI / 2;
623 while (angle_ > M_PI)
629 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
640 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
651 vpCDEBUG(1) <<
"begin vpMeLine::track()" << std::endl;
708 vpCDEBUG(1) <<
"end vpMeLine::track()" << std::endl;
711 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int &i1,
int &i2,
int &j1,
int &j2)
713 i1 = (int)(i + cos(
theta) * incr);
714 j1 = (int)(j + sin(
theta) * incr);
716 i2 = (int)(i - cos(
theta) * incr);
717 j2 = (int)(j - sin(
theta) * incr);
727 while (
theta >= M_PI)
741 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
742 unsigned char v1 = 0, v2 = 0;
746 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
748 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
749 double rho_lim1 = fabs((
double)i / cos(
theta));
750 double rho_lim2 = fabs((
double)j / sin(
theta));
752 double co_rho_lim1 = fabs(((
double)(height_ - i)) / cos(
theta));
753 double co_rho_lim2 = fabs(((
double)(width_ - j)) / sin(
theta));
755 double rho_lim = (std::min)(rho_lim1, rho_lim2);
756 double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
757 incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
758 if (incr < INCR_MIN) {
762 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
767 unsigned int i1_ =
static_cast<unsigned int>(i1);
768 unsigned int j1_ =
static_cast<unsigned int>(j1);
769 unsigned int i2_ =
static_cast<unsigned int>(i2);
770 unsigned int j2_ =
static_cast<unsigned int>(j2);
773 if (abs(v1 - v2) < 1) {
778 "there is no gray level difference between both "
779 "sides of the line"));
782 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
852 denom = (-(a2 / a1) * b1 + b2);
855 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
856 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
861 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
862 j = ((a2 / a1) * c1 - c2) / denom;
863 i = (-b1 * j - c1) / a1;
868 denom = (-(b2 / b1) * a1 + a2);
871 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
872 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
877 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
878 i = ((b2 / b1) * c1 - c2) / denom;
879 j = (-a1 * i - c1) / b1;
892 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
915 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
942 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
971 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
972 const vpColor &color,
unsigned int thickness)
1001 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1002 const vpColor &color,
unsigned int thickness)
1010 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1014 if (fabs(A) < fabs(B)) {
1015 double i1, j1, i2, j2;
1017 j1 = (-A * i1 - C) / B;
1019 j2 = (-A * i2 - C) / B;
1028 double i1, j1, i2, j2;
1030 i1 = -(B * j1 + C) / A;
1032 i2 = -(B * j2 + C) / A;
1051 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1055 if (fabs(A) < fabs(B)) {
1056 double i1, j1, i2, j2;
1058 j1 = (-A * i1 - C) / B;
1060 j2 = (-A * i2 - C) / B;
1069 double i1, j1, i2, j2;
1071 i1 = -(B * j1 + C) / A;
1073 i2 = -(B * j2 + C) / A;
1092 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1093 const vpColor &color,
unsigned int thickness)
1097 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1110 if (fabs(A) < fabs(B)) {
1111 double i1, j1, i2, j2;
1113 j1 = (-A * i1 - C) / B;
1115 j2 = (-A * i2 - C) / B;
1124 double i1, j1, i2, j2;
1126 i1 = -(B * j1 + C) / A;
1128 i2 = -(B * j2 + C) / A;
1147 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1148 const vpColor &color,
unsigned int thickness)
1152 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1165 if (fabs(A) < fabs(B)) {
1166 double i1, j1, i2, j2;
1168 j1 = (-A * i1 - C) / B;
1170 j2 = (-A * i2 - C) / B;
1179 double i1, j1, i2, j2;
1181 i1 = -(B * j1 + C) / A;
1183 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 theta
theta parameter of the line
double a
Parameter a of the line equation a*i + b*j + c = 0.
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 angle
Angle in deg between the extremities.
double c
Parameter c of the line equation a*i + b*j + c = 0.
double rho
rho parameter of the line
vpMeSite PExt[2]
Line extremities.
void seekExtremities(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
double angle_1
Angle in deg between the extremities.
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 delta_1
Angle in rad between the extremities.
double delta
Angle in rad between the extremities.
double b
Parameter b of the line equation a*i + b*j + c = 0.
void initTracking(const vpImage< unsigned char > &I)
virtual ~vpMeLine() override
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int j
Coordinates along j of a site.
@ 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 ifloat
Floating coordinates along i of a site.
int i
Coordinate along i of a site.
double alpha
Angle of tangent at site.
double jfloat
Floating coordinates along j of a site.
vpMeSiteState getState() const
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
void setState(const vpMeSiteState &flag)
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
std::list< vpMeSite > list
void display(const vpImage< unsigned char > &I)
vpMe * me
Moving edges initialisation parameters.
void setRange(const unsigned int &range)
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)