48 #include <visp3/core/vpImagePoint.h> 49 #include <visp3/core/vpMath.h> 50 #include <visp3/core/vpRobust.h> 51 #include <visp3/core/vpTrackingException.h> 52 #include <visp3/me/vpMe.h> 53 #include <visp3/me/vpMeLine.h> 54 #include <visp3/me/vpMeSite.h> 55 #include <visp3/me/vpMeTracker.h> 59 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2);
61 static void normalizeAngle(
double &delta)
63 while (delta > M_PI) {
66 while (delta < -M_PI) {
71 void computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2)
74 double B = double(i1 - i2);
75 double A = double(j1 - j2);
79 normalizeAngle(delta);
82 static void project(
double a,
double b,
double c,
double i,
double j,
double &ip,
double &jp)
84 if (fabs(a) > fabs(b)) {
86 ip = (-c - b * jp) / a;
89 jp = (-c - a * ip) / b;
99 : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.),
115 delta = meline.
delta;
152 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
160 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
170 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
175 double stepi = diffsi / (double)n_sample;
176 double stepj = diffsj / (double)n_sample;
193 pix.
init((
int)is, (
int)js, delta, 0,
sign);
208 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
209 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
240 std::cout <<
"Click on the line first point..." << std::endl;
245 std::cout <<
"Click on the line second point..." << std::endl;
284 unsigned int iter = 0;
285 unsigned int nos_1 = 0;
286 double distance = 100;
294 if ((fabs(b) >= 0.9))
300 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
310 while (iter < 4 && distance > 0.05) {
320 for (i = 0; i < nos_1; i++) {
325 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
330 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
359 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
369 while (iter < 4 && distance > 0.05) {
379 for (i = 0; i < nos_1; i++) {
384 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
389 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
413 normalizeAngle(delta);
427 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
429 int i1s, j1s, i2s, j2s;
446 double angle_ = atan2((
double)(i1s - i2s), (
double)(j1s - j2s));
453 computeDelta(delta, i1s, j1s, i2s, j2s);
472 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
481 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end();) {
502 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
520 if (fabs(imin - imax) < 25) {
521 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
553 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
556 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
565 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
577 double di = diffsi / sqrt(s);
578 double dj = diffsj / sqrt(s);
595 for (
int i = 0; i < 3; i++) {
624 for (
int i = 0; i < 3; i++) {
652 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
653 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
669 double i1, j1, i2, j2;
672 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
676 project(a, b, c,
PExt[0].ifloat,
PExt[0].jfloat, i1, j1);
677 project(a, b, c,
PExt[1].ifloat,
PExt[1].jfloat, i2, j2);
690 if ((
double)n < 0.9 * expecteddensity) {
691 double delta_new =
delta;
710 double angle_ = delta + M_PI / 2;
715 while (angle_ > M_PI)
721 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
732 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
749 vpCDEBUG(1) <<
"begin vpMeLine::track()" << std::endl;
804 vpCDEBUG(1) <<
"end vpMeLine::track()" << std::endl;
807 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int &i1,
int &i2,
int &j1,
int &j2)
809 i1 = (int)(i + cos(theta) * incr);
810 j1 = (int)(j + sin(theta) * incr);
812 i2 = (int)(i - cos(theta) * incr);
813 j2 = (int)(j - sin(theta) * incr);
829 while (theta >= M_PI)
862 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
863 unsigned char v1 = 0, v2 = 0;
867 update_indices(theta, i, j, incr, i1, i2, j1, j2);
869 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
870 double rho_lim1 = fabs((
double)i / cos(theta));
871 double rho_lim2 = fabs((
double)j / sin(theta));
873 double co_rho_lim1 = fabs(((
double)(height_ - i)) / cos(theta));
874 double co_rho_lim2 = fabs(((
double)(width_ - j)) / sin(theta));
876 double rho_lim = (std::min)(rho_lim1, rho_lim2);
877 double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
878 incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
879 if (incr < INCR_MIN) {
883 update_indices(theta, i, j, incr, i1, i2, j1, j2);
888 unsigned int i1_ =
static_cast<unsigned int>(i1);
889 unsigned int j1_ =
static_cast<unsigned int>(j1);
890 unsigned int i2_ =
static_cast<unsigned int>(i2);
891 unsigned int j2_ =
static_cast<unsigned int>(j2);
894 if (abs(v1 - v2) < 1) {
899 "there is no gray level difference between both " 900 "sides of the line"));
903 update_indices(theta, i, j, incr, i1, i2, j1, j2);
906 if (theta >= 0 && theta <= M_PI / 2) {
1001 double i = 0, j = 0;
1005 denom = (-(a2 / a1) * b1 + b2);
1008 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1009 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1014 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1015 j = ((a2 / a1) * c1 - c2) / denom;
1016 i = (-b1 * j - c1) / a1;
1021 denom = (-(b2 / b1) * a1 + a2);
1024 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1025 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1030 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1031 i = ((b2 / b1) * c1 - c2) / denom;
1032 j = (-a1 * i - c1) / b1;
1065 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1069 if (fabs(A) < fabs(B)) {
1070 double i1, j1, i2, j2;
1072 j1 = (-A * i1 - C) / B;
1074 j2 = (-A * i2 - C) / B;
1084 double i1, j1, i2, j2;
1086 i1 = -(B * j1 + C) / A;
1088 i2 = -(B * j2 + C) / A;
1128 const double &B,
const double &C,
const vpColor &color,
unsigned int thickness)
1132 if (fabs(A) < fabs(B)) {
1133 double i1, j1, i2, j2;
1135 j1 = (-A * i1 - C) / B;
1137 j2 = (-A * i2 - C) / B;
1147 double i1, j1, i2, j2;
1149 i1 = -(B * j1 + C) / A;
1151 i2 = -(B * j2 + C) / A;
1193 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1194 const vpColor &color,
unsigned int thickness)
1198 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1213 if (fabs(A) < fabs(B)) {
1214 double i1, j1, i2, j2;
1216 j1 = (-A * i1 - C) / B;
1218 j2 = (-A * i2 - C) / B;
1228 double i1, j1, i2, j2;
1230 i1 = -(B * j1 + C) / A;
1232 i2 = -(B * j2 + C) / A;
1274 const std::list<vpMeSite> &site_list,
const double &A,
const double &B,
const double &C,
1275 const vpColor &color,
unsigned int thickness)
1279 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1294 if (fabs(A) < fabs(B)) {
1295 double i1, j1, i2, j2;
1297 j1 = (-A * i1 - C) / B;
1299 j2 = (-A * i2 - C) / B;
1309 double i1, j1, i2, j2;
1311 i1 = -(B * j1 + C) / A;
1313 i2 = -(B * j2 + C) / A;
void reSample(const vpImage< unsigned char > &I)
unsigned int getRange() const
Implementation of a matrix and operations on matrices.
double c
Parameter c of the line equation a*i + b*j + c = 0.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
unsigned int getWidth() const
#define vpDEBUG_ENABLE(level)
unsigned int numberOfSignal()
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Class to define colors available for display functionnalities.
error that can be emited by ViSP classes.
void track(const vpImage< unsigned char > &Im)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static int round(const double x)
vpMeSiteState getState() const
void display(const vpImage< unsigned char > &I, vpColor col)
void set_i(const double ii)
void computeRhoTheta(const vpImage< unsigned char > &I)
std::list< vpMeSite > list
Error that can be emited by the vpTracker class and its derivates.
int outOfImage(int i, int j, int half, int row, int cols)
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)
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
static double sqr(double x)
Class that tracks in an image a line moving edges.
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
double a
Parameter a of the line equation a*i + b*j + c = 0.
void setState(const vpMeSiteState &flag)
void set_j(const double jj)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Implementation of column vector and the associated operations.
double b
Parameter b of the line equation a*i + b*j + c = 0.
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
virtual void sample(const vpImage< unsigned char > &image, const bool doNotTrack=false)
vpMe * me
Moving edges initialisation parameters.
Contains an M-Estimator and various influence function.
void initTracking(const vpImage< unsigned char > &I)
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
unsigned int getHeight() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void setRange(const unsigned int &r)
void setThreshold(const double noise_threshold)
double getSampleStep() const
vpMeSite::vpMeSiteDisplayType selectDisplay
void setIteration(const unsigned int iter)
Set iteration.
void seekExtremities(const vpImage< unsigned char > &I)
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
static const vpColor blue