45 #include <visp3/core/vpImagePoint.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpRobust.h>
48 #include <visp3/core/vpTrackingException.h>
49 #include <visp3/me/vpMe.h>
50 #include <visp3/me/vpMeLine.h>
51 #include <visp3/me/vpMeSite.h>
52 #include <visp3/me/vpMeTracker.h>
56 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2);
58 static void normalizeAngle(
double &delta)
60 while (delta > M_PI) {
63 while (delta < -M_PI) {
68 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2)
71 double B = double(i1 - i2);
72 double A = double(j1 - j2);
76 normalizeAngle(delta);
79 static void project(
double a,
double b,
double c,
double i,
double j,
double &ip,
double &jp)
81 if (fabs(a) > fabs(b)) {
83 ip = (-c - b * jp) / a;
86 jp = (-c - a * ip) / b;
96 : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.),
106 :
vpMeTracker(meline), rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
107 _useIntensityForRho(true), a(0.), b(0.), c(0.)
149 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
157 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
167 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
172 double stepi = diffsi / (double)n_sample;
173 double stepj = diffsj / (double)n_sample;
205 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
206 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
239 std::cout <<
"Click on the line first point..." << std::endl;
244 std::cout <<
"Click on the line second point..." << std::endl;
279 unsigned int iter = 0;
280 unsigned int nos_1 = 0;
281 double distance = 100;
289 if ((fabs(
b) >= 0.9))
295 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
305 while (iter < 4 && distance > 0.05) {
306 for (
unsigned int i = 0; i < k; i++) {
307 for (
unsigned int j = 0; j < 2; j++) {
308 DA[i][j] = w[i] * A[i][j];
319 for (
unsigned int i = 0; i < nos_1; i++) {
324 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
329 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
358 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
368 while (iter < 4 && distance > 0.05) {
377 for (
unsigned int i = 0; i < nos_1; i++) {
382 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
387 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
411 normalizeAngle(
delta);
425 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
427 int i1s, j1s, i2s, j2s;
444 double angle_ = atan2((
double)(i1s - i2s), (
double)(j1s - j2s));
451 computeDelta(
delta, i1s, j1s, i2s, j2s);
470 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
479 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end();) {
500 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
518 if (fabs(imin - imax) < 25) {
519 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
551 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
554 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
563 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
575 double di = diffsi / sqrt(s);
576 double dj = diffsj / sqrt(s);
593 for (
int i = 0; i < 3; i++) {
622 for (
int i = 0; i < 3; i++) {
650 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
651 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
667 double i1, j1, i2, j2;
670 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
674 project(
a,
b,
c,
PExt[0].ifloat,
PExt[0].jfloat, i1, j1);
675 project(
a,
b,
c,
PExt[1].ifloat,
PExt[1].jfloat, i2, j2);
688 if ((
double)n < 0.9 * expecteddensity) {
689 double delta_new =
delta;
708 double angle_ =
delta + M_PI / 2;
713 while (angle_ > M_PI)
719 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
730 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
747 vpCDEBUG(1) <<
"begin vpMeLine::track()" << std::endl;
802 vpCDEBUG(1) <<
"end vpMeLine::track()" << std::endl;
805 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int &i1,
int &i2,
int &j1,
int &j2)
807 i1 = (int)(i + cos(
theta) * incr);
808 j1 = (int)(j + sin(
theta) * incr);
810 i2 = (int)(i - cos(
theta) * incr);
811 j2 = (int)(j - sin(
theta) * incr);
827 while (
theta >= M_PI)
860 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
861 unsigned char v1 = 0, v2 = 0;
865 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
867 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
868 double rho_lim1 = fabs((
double)i / cos(
theta));
869 double rho_lim2 = fabs((
double)j / sin(
theta));
871 double co_rho_lim1 = fabs(((
double)(height_ - i)) / cos(
theta));
872 double co_rho_lim2 = fabs(((
double)(width_ - j)) / sin(
theta));
874 double rho_lim = (std::min)(rho_lim1, rho_lim2);
875 double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
876 incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
877 if (incr < INCR_MIN) {
881 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
886 unsigned int i1_ =
static_cast<unsigned int>(i1);
887 unsigned int j1_ =
static_cast<unsigned int>(j1);
888 unsigned int i2_ =
static_cast<unsigned int>(i2);
889 unsigned int j2_ =
static_cast<unsigned int>(j2);
892 if (abs(v1 - v2) < 1) {
897 "there is no gray level difference between both "
898 "sides of the line"));
901 update_indices(
theta, i, j, incr, i1, i2, j1, j2);
1003 denom = (-(a2 / a1) * b1 + b2);
1006 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1007 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1012 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1013 j = ((a2 / a1) * c1 - c2) / denom;
1014 i = (-b1 * j - c1) / a1;
1019 denom = (-(b2 / b1) * a1 + a2);
1022 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1023 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1028 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1029 i = ((b2 / b1) * c1 - c2) / denom;
1030 j = (-a1 * i - c1) / b1;
1063 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1067 if (fabs(A) < fabs(B)) {
1068 double i1, j1, i2, j2;
1070 j1 = (-A * i1 - C) / B;
1072 j2 = (-A * i2 - C) / B;
1082 double i1, j1, i2, j2;
1084 i1 = -(B * j1 + C) / A;
1086 i2 = -(B * j2 + C) / A;
1126 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1130 if (fabs(A) < fabs(B)) {
1131 double i1, j1, i2, j2;
1133 j1 = (-A * i1 - C) / B;
1135 j2 = (-A * i2 - C) / B;
1145 double i1, j1, i2, j2;
1147 i1 = -(B * j1 + C) / A;
1149 i2 = -(B * j2 + C) / A;
1191 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1192 const vpColor &color,
unsigned int thickness)
1196 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1211 if (fabs(A) < fabs(B)) {
1212 double i1, j1, i2, j2;
1214 j1 = (-A * i1 - C) / B;
1216 j2 = (-A * i2 - C) / B;
1226 double i1, j1, i2, j2;
1228 i1 = -(B * j1 + C) / A;
1230 i2 = -(B * j2 + C) / A;
1272 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1273 const vpColor &color,
unsigned int thickness)
1277 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1292 if (fabs(A) < fabs(B)) {
1293 double i1, j1, i2, j2;
1295 j1 = (-A * i1 - C) / B;
1297 j2 = (-A * i2 - C) / B;
1307 double i1, j1, i2, j2;
1309 i1 = -(B * j1 + C) / A;
1311 i2 = -(B * j2 + C) / A;
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionnalities.
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 emited 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.
virtual void sample(const vpImage< unsigned char > &image, bool doNotTrack=false)
void track(const vpImage< unsigned char > &Im)
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 c
Parameter c of the line equation a*i + b*j + c = 0.
void seekExtremities(const vpImage< unsigned char > &I)
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
double b
Parameter b of the line equation a*i + b*j + c = 0.
void initTracking(const vpImage< unsigned char > &I)
Performs search in a given direction(normal) for a given distance(pixels) for a given '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)
vpMeSiteState getState() const
void track(const vpImage< unsigned char > &im, const vpMe *me, 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)
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)
Track sampled pixels.
std::list< vpMeSite > list
virtual void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness)=0
vpMe * me
Moving edges initialisation parameters.
void setRange(const unsigned int &r)
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 emited by the vpTracker class and its derivates.
#define vpDEBUG_ENABLE(level)