Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
vpMeLine.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Moving edges.
32  */
33 
39 #include <algorithm> // (std::min)
40 #include <cmath> // std::fabs
41 #include <limits> // numeric_limits
42 #include <stdlib.h>
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>
51 
52 #define INCR_MIN 1
53 
54 void computeDelta(double &delta, int i1, int j1, int i2, int j2);
55 
56 static void normalizeAngle(double &delta)
57 {
58  while (delta > M_PI) {
59  delta -= M_PI;
60  }
61  while (delta < -M_PI) {
62  delta += M_PI;
63  }
64 }
65 
66 void computeDelta(double &delta, int i1, int j1, int i2, int j2)
67 {
68 
69  double B = double(i1 - i2);
70  double A = double(j1 - j2);
71 
72  delta = atan2(B, A);
73  delta -= M_PI / 2.0;
74  normalizeAngle(delta);
75 }
76 
77 static void project(double a, double b, double c, double i, double j, double &ip, double &jp)
78 {
79  if (fabs(a) > fabs(b)) {
80  jp = (vpMath::sqr(a) * j - a * b * i - c * b) / (vpMath::sqr(a) + vpMath::sqr(b));
81  ip = (-c - b * jp) / a;
82  }
83  else {
84  ip = (vpMath::sqr(b) * i - a * b * j - c * a) / (vpMath::sqr(a) + vpMath::sqr(b));
85  jp = (-c - a * ip) / b;
86  }
87 }
88 
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.)
92 { }
93 
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.)
97 
98 {
99  m_rho = meline.m_rho;
100  m_theta = meline.m_theta;
101  m_delta = meline.m_delta;
102  m_delta_1 = meline.m_delta_1;
103  m_angle = meline.m_angle;
104  m_angle_1 = meline.m_angle_1;
105  m_sign = meline.m_sign;
106 
107  m_a = meline.m_a;
108  m_b = meline.m_b;
109  m_c = meline.m_c;
111  m_PExt[0] = meline.m_PExt[0];
112  m_PExt[1] = meline.m_PExt[1];
113 }
114 
116 {
117  m_meList.clear();
118 }
119 
120 void vpMeLine::sample(const vpImage<unsigned char> &I, bool doNotTrack)
121 {
122  (void)doNotTrack;
123  if (!m_me) {
124  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
125  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
126  }
127 
128  int nbrows = static_cast<int>(I.getHeight());
129  int nbcols = static_cast<int>(I.getWidth());
130  double n_sample;
131 
132  if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
133  vpERROR_TRACE("function called with sample step = 0");
134  throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
135  }
136 
137  // i, j portions of the line_p
138  double diffsi = m_PExt[0].m_ifloat - m_PExt[1].m_ifloat;
139  double diffsj = m_PExt[0].m_jfloat - m_PExt[1].m_jfloat;
140 
141  double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
142  if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
143  throw(vpTrackingException(vpTrackingException::fatalError, "points too close of each other to define a line"));
144  // number of samples along line_p
145  n_sample = length_p / (double)m_me->getSampleStep();
146 
147  double stepi = diffsi / (double)n_sample;
148  double stepj = diffsj / (double)n_sample;
149 
150  // Choose starting point
151  double is = m_PExt[1].m_ifloat;
152  double js = m_PExt[1].m_jfloat;
153 
154  // Delete old list
155  m_meList.clear();
156 
157  // sample positions at i*m_me->getSampleStep() interval along the
158  // line_p, starting at PSiteExt[0]
159 
160  for (int i = 0; i <= vpMath::round(n_sample); i++) {
161  vpImagePoint iP;
162  iP.set_i(is);
163  iP.set_j(js);
164  unsigned int is_uint = static_cast<unsigned int>(is);
165  unsigned int js_uint = static_cast<unsigned int>(js);
166  // If point is in the image, add to the sample list
167  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
168  && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
169  vpMeSite pix;
170  pix.init(iP.get_i(), iP.get_j(), m_delta, 0, m_sign);
173  const double marginRatio = m_me->getThresholdMarginRatio();
174  double convolution = pix.convolution(I, m_me);
175  double contrastThreshold = fabs(convolution) * marginRatio;
176  pix.setContrastThreshold(contrastThreshold, *m_me);
177 
178  if (vpDEBUG_ENABLE(3)) {
180  }
181 
182  m_meList.push_back(pix);
183  }
184  is += stepi;
185  js += stepj;
186  }
187 
188  vpCDEBUG(1) << "end vpMeLine::sample() : ";
189  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
190 }
191 
192 void vpMeLine::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
193 {
194  vpMeLine::displayLine(I, m_PExt[0], m_PExt[1], m_meList, m_a, m_b, m_c, color, thickness);
195 }
196 
198 {
199  vpImagePoint ip1, ip2;
200 
201  vpDisplay::flush(I);
202 
203  std::cout << "Click on the line first point..." << std::endl;
204  while (vpDisplay::getClick(I, ip1) != true) { }
205 
207  vpDisplay::flush(I);
208  std::cout << "Click on the line second point..." << std::endl;
209  while (vpDisplay::getClick(I, ip2) != true) { }
210 
212  vpDisplay::flush(I);
213 
214  try {
215  initTracking(I, ip1, ip2);
216  }
217  catch (...) {
218  vpERROR_TRACE("Error caught");
219  throw;
220  }
221 }
222 
224 {
225  vpMatrix A(numberOfSignal(), 2);
226  vpColVector x(2), x_1(2);
227  x_1 = 0;
228 
229  vpRobust r;
232  D.eye();
233  vpMatrix DA(numberOfSignal(), 2);
236  w = 1;
237  vpMeSite p_me;
238  unsigned int iter = 0;
239  unsigned int nos_1 = 0;
240  double distance = 100;
241 
242  if (m_meList.size() <= 2 || numberOfSignal() <= 2) {
243  // vpERROR_TRACE("Not enough point") ;
244  vpCDEBUG(1) << "Not enough point";
246  }
247 
248  if ((fabs(m_b) >= 0.9)) // Construction du systeme Ax=B
249  // a i + j + c = 0
250  // A = (i 1) B = (-j)
251  {
252  nos_1 = numberOfSignal();
253  unsigned int k = 0;
254  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
255  p_me = *it;
256  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
257  A[k][0] = p_me.m_ifloat;
258  A[k][1] = 1;
259  B[k] = -p_me.m_jfloat;
260  k++;
261  }
262  }
263 
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];
268  }
269  }
270 
271  x = DA.pseudoInverse(1e-26) * D * B;
272 
273  vpColVector residu(nos_1);
274  residu = B - A * x;
275  r.MEstimator(vpRobust::TUKEY, residu, w);
276 
277  k = 0;
278  for (unsigned int i = 0; i < nos_1; i++) {
279  D[k][k] = w[k];
280  k++;
281  }
282  iter++;
283  distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
284  x_1 = x;
285  }
286 
287  k = 0;
288  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
289  p_me = *it;
290  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
291  if (w[k] < 0.2) {
293 
294  *it = p_me;
295  }
296  k++;
297  }
298  }
299 
300  // mise a jour de l'equation de la droite
301  m_a = x[0];
302  m_b = 1;
303  m_c = x[1];
304 
305  double s = sqrt(vpMath::sqr(m_a) + vpMath::sqr(m_b));
306  m_a /= s;
307  m_b /= s;
308  m_c /= s;
309  }
310 
311  else // Construction du systeme Ax=B
312  // i + bj + c = 0
313  // A = (j 1) B = (-i)
314  {
315  nos_1 = numberOfSignal();
316  unsigned int k = 0;
317  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
318  p_me = *it;
319  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
320  A[k][0] = p_me.m_jfloat;
321  A[k][1] = 1;
322  B[k] = -p_me.m_ifloat;
323  k++;
324  }
325  }
326 
327  while (iter < 4 && distance > 0.05) {
328  DA = D * A;
329  x = DA.pseudoInverse(1e-26) * D * B;
330 
331  vpColVector residu(nos_1);
332  residu = B - A * x;
333  r.MEstimator(vpRobust::TUKEY, residu, w);
334 
335  k = 0;
336  for (unsigned int i = 0; i < nos_1; i++) {
337  D[k][k] = w[k];
338  k++;
339  }
340  iter++;
341  distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
342  x_1 = x;
343  }
344 
345  k = 0;
346  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
347  p_me = *it;
348  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
349  if (w[k] < 0.2) {
351 
352  *it = p_me;
353  }
354  k++;
355  }
356  }
357  m_a = 1;
358  m_b = x[0];
359  m_c = x[1];
360 
361  double s = sqrt(vpMath::sqr(m_a) + vpMath::sqr(m_b));
362  m_a /= s;
363  m_b /= s;
364  m_c /= s;
365  }
366 
367  // mise a jour du delta
368  m_delta = atan2(m_a, m_b);
369 
370  normalizeAngle(m_delta);
371 }
372 
374 {
375  vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
376 
377  int i1s, j1s, i2s, j2s;
378 
379  i1s = vpMath::round(ip1.get_i());
380  i2s = vpMath::round(ip2.get_i());
381  j1s = vpMath::round(ip1.get_j());
382  j2s = vpMath::round(ip2.get_j());
383 
384  try {
385 
386  // 1. On fait ce qui concerne les droites (peut etre vide)
387  {
388  // Points extremites
389  m_PExt[0].m_ifloat = (float)ip1.get_i();
390  m_PExt[0].m_jfloat = (float)ip1.get_j();
391  m_PExt[1].m_ifloat = (float)ip2.get_i();
392  m_PExt[1].m_jfloat = (float)ip2.get_j();
393 
394  double angle_ = atan2((double)(i1s - i2s), (double)(j1s - j2s));
395  m_a = cos(angle_);
396  m_b = sin(angle_);
397 
398  // Real values of a, b can have an other sign. So to get the good values
399  // of a and b in order to initialise then c, we call track(I) just below
400 
401  computeDelta(m_delta, i1s, j1s, i2s, j2s);
402  m_delta_1 = m_delta;
403 
404  // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a),
405  // delta);
406 
407  sample(I);
408  }
409  // 2. On appelle ce qui n'est pas specifique
410  {
412  }
413  // Call track(I) to give the good sign to a and b and to initialise c
414  // which can be used for the display
415  track(I);
416  }
417  catch (...) {
418  vpERROR_TRACE("Error caught");
419  throw;
420  }
421  vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
422 }
423 
425 {
426  // Loop through list of sites to track
427  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end();) {
428  vpMeSite s = *it; // current reference pixel
429 
431  it = m_meList.erase(it);
432  else
433  ++it;
434  }
435 }
436 
438 {
439  double imin = +1e6;
440  double jmin = +1e6;
441  double imax = -1;
442  double jmax = -1;
443 
444  // Loop through list of sites to track
445  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
446  vpMeSite s = *it; // current reference pixel
447  if (s.m_ifloat < imin) {
448  imin = s.m_ifloat;
449  jmin = s.m_jfloat;
450  }
451 
452  if (s.m_ifloat > imax) {
453  imax = s.m_ifloat;
454  jmax = s.m_jfloat;
455  }
456  }
457 
458  m_PExt[0].m_ifloat = imin;
459  m_PExt[0].m_jfloat = jmin;
460  m_PExt[1].m_ifloat = imax;
461  m_PExt[1].m_jfloat = jmax;
462 
463  if (fabs(imin - imax) < 25) {
464  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
465  vpMeSite s = *it; // current reference pixel
466  if (s.m_jfloat < jmin) {
467  imin = s.m_ifloat;
468  jmin = s.m_jfloat;
469  }
470 
471  if (s.m_jfloat > jmax) {
472  imax = s.m_ifloat;
473  jmax = s.m_jfloat;
474  }
475  }
476  m_PExt[0].m_ifloat = imin;
477  m_PExt[0].m_jfloat = jmin;
478  m_PExt[1].m_ifloat = imax;
479  m_PExt[1].m_jfloat = jmax;
480  }
481 }
482 
484 {
485  vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
486 
487  if (!m_me) {
488  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
489  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
490  }
491 
492  int nbrows = static_cast<int>(I.getHeight());
493  int nbcols = static_cast<int>(I.getWidth());
494  double n_sample;
495 
496  // if (m_me->getSampleStep()==0)
497  if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
498 
499  vpERROR_TRACE("function called with sample step = 0");
500  throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
501  }
502 
503  // i, j portions of the line_p
504  double diffsi = m_PExt[0].m_ifloat - m_PExt[1].m_ifloat;
505  double diffsj = m_PExt[0].m_jfloat - m_PExt[1].m_jfloat;
506 
507  double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
508 
509  double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
510  double dj = diffsj / sqrt(s);
511 
512  double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
513 
514  // number of samples along line_p
515  n_sample = length_p / (double)m_me->getSampleStep();
516  double sample_step = (double)m_me->getSampleStep();
517 
518  vpMeSite P;
519  P.init((int)m_PExt[0].m_ifloat, (int)m_PExt[0].m_jfloat, m_delta_1, 0, m_sign);
521 
522  unsigned int memory_range = m_me->getRange();
523  m_me->setRange(1);
524 
525 
526  for (int i = 0; i < 3; i++) {
527  P.m_ifloat = P.m_ifloat + di * sample_step;
528  P.m_i = static_cast<int>(P.m_ifloat);
529  P.m_jfloat = P.m_jfloat + dj * sample_step;
530  P.m_j = static_cast<int>(P.m_jfloat);
531 
532  vpImagePoint iP;
533  iP.set_i(P.m_ifloat);
534  iP.set_j(P.m_jfloat);
535 
536  unsigned int is_uint = static_cast<unsigned int>(P.m_ifloat);
537  unsigned int js_uint = static_cast<unsigned int>(P.m_jfloat);
538  if ((!outOfImage(iP, 5, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
539  && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
540  P.track(I, m_me, false);
541 
542  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
543  m_meList.push_back(P);
544  if (vpDEBUG_ENABLE(3)) {
546  }
547  }
548  else {
549  if (vpDEBUG_ENABLE(3)) {
551  }
552  }
553  }
554  }
555 
556  P.init((int)m_PExt[1].m_ifloat, (int)m_PExt[1].m_jfloat, m_delta_1, 0, m_sign);
558  for (int i = 0; i < 3; i++) {
559  P.m_ifloat = P.m_ifloat - di * sample_step;
560  P.m_i = static_cast<int>(P.m_ifloat);
561  P.m_jfloat = P.m_jfloat - dj * sample_step;
562  P.m_j = static_cast<int>(P.m_jfloat);
563 
564  vpImagePoint iP;
565  iP.set_i(P.m_ifloat);
566  iP.set_j(P.m_jfloat);
567 
568  unsigned int is_uint = static_cast<unsigned int>(P.m_ifloat);
569  unsigned int js_uint = static_cast<unsigned int>(P.m_jfloat);
570  if ((!outOfImage(iP, 5, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
571  && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
572  P.track(I, m_me, false);
573 
574  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
575  m_meList.push_back(P);
576  if (vpDEBUG_ENABLE(3)) {
578  }
579  }
580  else {
581  if (vpDEBUG_ENABLE(3)) {
583  }
584  }
585  }
586  }
587 
588  m_me->setRange(memory_range);
589 
590  vpCDEBUG(1) << "end vpMeLine::sample() : ";
591  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
592 }
593 
595 {
596  double i1, j1, i2, j2;
597 
598  if (!m_me) {
599  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
600  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
601  }
602 
603  project(m_a, m_b, m_c, m_PExt[0].m_ifloat, m_PExt[0].m_jfloat, i1, j1);
604  project(m_a, m_b, m_c, m_PExt[1].m_ifloat, m_PExt[1].m_jfloat, i2, j2);
605 
606  // Points extremites
607  m_PExt[0].m_ifloat = i1;
608  m_PExt[0].m_jfloat = j1;
609  m_PExt[1].m_ifloat = i2;
610  m_PExt[1].m_jfloat = j2;
611 
612  double d = sqrt(vpMath::sqr(i1 - i2) + vpMath::sqr(j1 - j2));
613 
614  unsigned int n = numberOfSignal();
615  double expecteddensity = d / (double)m_me->getSampleStep();
616 
617  if ((double)n < 0.9 * expecteddensity) {
618  double delta_new = m_delta;
619  m_delta = m_delta_1;
620  sample(I);
621  m_delta = delta_new;
622  // 2. On appelle ce qui n'est pas specifique
624  }
625 }
626 
628 {
629  vpMeSite p_me;
630 
631  double angle_ = m_delta + M_PI / 2;
632  double diff = 0;
633 
634  while (angle_ < 0)
635  angle_ += M_PI;
636  while (angle_ > M_PI)
637  angle_ -= M_PI;
638 
639  angle_ = vpMath::round(angle_ * 180 / M_PI);
640 
641  // if(fabs(angle_) == 180 )
642  if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
643  angle_ = 0;
644  }
645 
646  // std::cout << "angle theta : " << theta << std::endl ;
647  diff = fabs(angle_ - m_angle_1);
648  if (diff > 90)
649  m_sign *= -1;
650 
651  m_angle_1 = angle_;
652 
653  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
654  p_me = *it;
655  p_me.setAlpha(m_delta);
656  p_me.m_mask_sign = m_sign;
657  *it = p_me;
658  }
659  m_delta_1 = m_delta;
660 }
661 
663 {
664  vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl;
665 
667 
668  // 3. On revient aux droites
669  {
670  // supression des points rejetes par les ME
671  suppressPoints();
672  setExtremities();
673 
674  // Estimation des parametres de la droite aux moindres carre
675  try {
676  leastSquare();
677  }
678  catch (...) {
679  vpERROR_TRACE("Error caught");
680  throw;
681  }
682 
683  // recherche de point aux extremite de la droites
684  // dans le cas d'un glissement
685  seekExtremities(I);
686 
687  setExtremities();
688  try {
689  leastSquare();
690  }
691  catch (...) {
692  vpERROR_TRACE("Error caught");
693  throw;
694  }
695 
696  // suppression des points rejetes par la regression robuste
697  suppressPoints();
698  setExtremities();
699 
700  // reechantillonage si necessaire
701  reSample(I);
702 
703  // remet a jour l'angle delta pour chaque point de la liste
704 
705  updateDelta();
706 
707  // Remise a jour de delta dans la liste de site me
708  if (vpDEBUG_ENABLE(2)) {
709  display(I, vpColor::red);
711  vpDisplay::flush(I);
712  }
713  }
714 
715  computeRhoTheta(I);
716 
717  vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
718 }
719 
720 void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int &i2, int &j1, int &j2)
721 {
722  i1 = (int)(i + cos(theta) * incr);
723  j1 = (int)(j + sin(theta) * incr);
724 
725  i2 = (int)(i - cos(theta) * incr);
726  j2 = (int)(j - sin(theta) * incr);
727 }
728 
730 {
731  m_rho = fabs(m_c);
732  m_theta = atan2(m_b, m_a);
733 
734  while (m_theta >= M_PI)
735  m_theta -= M_PI;
736  while (m_theta < 0)
737  m_theta += M_PI;
738 
739  if (m_useIntensityForRho) {
740  // convention pour choisir le signe de rho
741  int i, j;
742  i = vpMath::round((m_PExt[0].m_ifloat + m_PExt[1].m_ifloat) / 2);
743  j = vpMath::round((m_PExt[0].m_jfloat + m_PExt[1].m_jfloat) / 2);
744 
745  int end = false;
746  int incr = 10;
747 
748  int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
749  unsigned char v1 = 0, v2 = 0;
750 
751  int width_ = (int)I.getWidth();
752  int height_ = (int)I.getHeight();
753  update_indices(m_theta, i, j, incr, i1, i2, j1, j2);
754 
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));
758 
759  double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(m_theta));
760  double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(m_theta));
761 
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) {
766  vpERROR_TRACE("increment is too small");
767  throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small"));
768  }
769  update_indices(m_theta, i, j, incr, i1, i2, j1, j2);
770  }
771 
772  while (!end) {
773  end = true;
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);
778  v1 = I[i1_][j1_];
779  v2 = I[i2_][j2_];
780  if (abs(v1 - v2) < 1) {
781  incr--;
782  end = false;
783  if (incr == 1) {
784  throw(vpException(vpException::fatalError, "In vpMeLine cannot determine rho sign, since "
785  "there is no gray level difference between both "
786  "sides of the line"));
787  }
788  }
789  update_indices(m_theta, i, j, incr, i1, i2, j1, j2);
790  }
791 
792  if (m_theta >= 0 && m_theta <= M_PI / 2) {
793  if (v2 < v1) {
794  m_theta += M_PI;
795  m_rho *= -1;
796  }
797  }
798 
799  else {
800  double jinter;
801  jinter = -m_c / m_b;
802  if (v2 < v1) {
803  m_theta += M_PI;
804  if (jinter > 0) {
805  m_rho *= -1;
806  }
807  }
808 
809  else {
810  if (jinter < 0) {
811  m_rho *= -1;
812  }
813  }
814  }
815 
816  if (vpDEBUG_ENABLE(2)) {
817  vpImagePoint ip, ip1, ip2;
818 
819  ip.set_i(i);
820  ip.set_j(j);
821  ip1.set_i(i1);
822  ip1.set_j(j1);
823  ip2.set_i(i2);
824  ip2.set_j(j2);
825 
828  }
829  }
830 }
831 
832 double vpMeLine::getRho() const { return m_rho; }
833 
834 double vpMeLine::getTheta() const { return m_theta; }
835 
837 {
838  /*Return the coordinates of the extremities of the line*/
839  ip1.set_i(m_PExt[0].m_ifloat);
840  ip1.set_j(m_PExt[0].m_jfloat);
841  ip2.set_i(m_PExt[1].m_ifloat);
842  ip2.set_j(m_PExt[1].m_jfloat);
843 }
844 
845 bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
846 {
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;
853 
854  try {
855  double i = 0, j = 0;
856  double denom = 0;
857 
858  if (a1 > 0.1) {
859  denom = (-(a2 / a1) * b1 + b2);
860 
861  // if (denom == 0)
862  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
863  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
864  return (false);
865  }
866 
867  // if (denom != 0 )
868  if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
869  j = ((a2 / a1) * c1 - c2) / denom;
870  i = (-b1 * j - c1) / a1;
871  }
872  }
873 
874  else {
875  denom = (-(b2 / b1) * a1 + a2);
876 
877  // if (denom == 0)
878  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
879  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
880  return (false);
881  }
882 
883  // if (denom != 0 )
884  if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
885  i = ((b2 / b1) * c1 - c2) / denom;
886  j = (-a1 * i - c1) / b1;
887  }
888  }
889  ip.set_i(i);
890  ip.set_j(j);
891 
892  return (true);
893  }
894  catch (...) {
895  return (false);
896  }
897 }
898 
899 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
921 void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
922  const double &B, const double &C, const vpColor &color, unsigned int thickness)
923 {
924  vpMeLine::displayLine(I, PExt1, PExt2, A, B, C, color, thickness);
925 }
926 
948 void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
949  const double &B, const double &C, const vpColor &color, unsigned int thickness)
950 {
951  vpMeLine::displayLine(I, PExt1, PExt2, A, B, C, color, thickness);
952 }
953 
977 void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
978  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
979  const vpColor &color, unsigned int thickness)
980 {
981  vpMeLine::displayLine(I, PExt1, PExt2, site_list, A, B, C, color, thickness);
982 }
983 
1007 void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1008  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1009  const vpColor &color, unsigned int thickness)
1010 {
1011 
1012  vpMeLine::displayLine(I, PExt1, PExt2, site_list, A, B, C, color, thickness);
1013 }
1014 #endif // Deprecated
1015 
1016 void vpMeLine::displayLine(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1017  const double &B, const double &C, const vpColor &color, unsigned int thickness)
1018 {
1019  vpImagePoint ip1, ip2;
1020 
1021  if (fabs(A) < fabs(B)) {
1022  double i1, j1, i2, j2;
1023  i1 = 0;
1024  j1 = (-A * i1 - C) / B;
1025  i2 = I.getHeight() - 1.0;
1026  j2 = (-A * i2 - C) / B;
1027 
1028  ip1.set_i(i1);
1029  ip1.set_j(j1);
1030  ip2.set_i(i2);
1031  ip2.set_j(j2);
1032  vpDisplay::displayLine(I, ip1, ip2, color);
1033  }
1034  else {
1035  double i1, j1, i2, j2;
1036  j1 = 0;
1037  i1 = -(B * j1 + C) / A;
1038  j2 = I.getWidth() - 1.0;
1039  i2 = -(B * j2 + C) / A;
1040 
1041  ip1.set_i(i1);
1042  ip1.set_j(j1);
1043  ip2.set_i(i2);
1044  ip2.set_j(j2);
1045  vpDisplay::displayLine(I, ip1, ip2, color);
1046  }
1047 
1048  ip1.set_i(PExt1.m_ifloat);
1049  ip1.set_j(PExt1.m_jfloat);
1050  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1051 
1052  ip1.set_i(PExt2.m_ifloat);
1053  ip1.set_j(PExt2.m_jfloat);
1054  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1055 }
1056 
1057 void vpMeLine::displayLine(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1058  const double &B, const double &C, const vpColor &color, unsigned int thickness)
1059 {
1060  vpImagePoint ip1, ip2;
1061 
1062  if (fabs(A) < fabs(B)) {
1063  double i1, j1, i2, j2;
1064  i1 = 0;
1065  j1 = (-A * i1 - C) / B;
1066  i2 = I.getHeight() - 1.0;
1067  j2 = (-A * i2 - C) / B;
1068 
1069  ip1.set_i(i1);
1070  ip1.set_j(j1);
1071  ip2.set_i(i2);
1072  ip2.set_j(j2);
1073  vpDisplay::displayLine(I, ip1, ip2, color);
1074  }
1075  else {
1076  double i1, j1, i2, j2;
1077  j1 = 0;
1078  i1 = -(B * j1 + C) / A;
1079  j2 = I.getWidth() - 1.0;
1080  i2 = -(B * j2 + C) / A;
1081 
1082  ip1.set_i(i1);
1083  ip1.set_j(j1);
1084  ip2.set_i(i2);
1085  ip2.set_j(j2);
1086  vpDisplay::displayLine(I, ip1, ip2, color);
1087  }
1088 
1089  ip1.set_i(PExt1.m_ifloat);
1090  ip1.set_j(PExt1.m_jfloat);
1091  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1092 
1093  ip1.set_i(PExt2.m_ifloat);
1094  ip1.set_j(PExt2.m_jfloat);
1095  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1096 }
1097 
1098 void vpMeLine::displayLine(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1099  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1100  const vpColor &color, unsigned int thickness)
1101 {
1102  vpImagePoint ip;
1103 
1104  for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1105  vpMeSite pix = *it;
1106  ip.set_i(pix.m_ifloat);
1107  ip.set_j(pix.m_jfloat);
1108 
1109  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1110  vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1111  else
1112  vpDisplay::displayCross(I, ip, 5, color, thickness);
1113  }
1114 
1115  vpImagePoint ip1, ip2;
1116 
1117  if (fabs(A) < fabs(B)) {
1118  double i1, j1, i2, j2;
1119  i1 = 0;
1120  j1 = (-A * i1 - C) / B;
1121  i2 = I.getHeight() - 1.0;
1122  j2 = (-A * i2 - C) / B;
1123 
1124  ip1.set_i(i1);
1125  ip1.set_j(j1);
1126  ip2.set_i(i2);
1127  ip2.set_j(j2);
1128  vpDisplay::displayLine(I, ip1, ip2, color);
1129  }
1130  else {
1131  double i1, j1, i2, j2;
1132  j1 = 0;
1133  i1 = -(B * j1 + C) / A;
1134  j2 = I.getWidth() - 1.0;
1135  i2 = -(B * j2 + C) / A;
1136 
1137  ip1.set_i(i1);
1138  ip1.set_j(j1);
1139  ip2.set_i(i2);
1140  ip2.set_j(j2);
1141  vpDisplay::displayLine(I, ip1, ip2, color);
1142  }
1143 
1144  ip1.set_i(PExt1.m_ifloat);
1145  ip1.set_j(PExt1.m_jfloat);
1146  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1147 
1148  ip1.set_i(PExt2.m_ifloat);
1149  ip1.set_j(PExt2.m_jfloat);
1150  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1151 }
1152 
1153 void vpMeLine::displayLine(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1154  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1155  const vpColor &color, unsigned int thickness)
1156 {
1157  vpImagePoint ip;
1158 
1159  for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1160  vpMeSite pix = *it;
1161  ip.set_i(pix.m_ifloat);
1162  ip.set_j(pix.m_jfloat);
1163 
1164  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1165  vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1166  else
1167  vpDisplay::displayCross(I, ip, 5, color, thickness);
1168  }
1169 
1170  vpImagePoint ip1, ip2;
1171 
1172  if (fabs(A) < fabs(B)) {
1173  double i1, j1, i2, j2;
1174  i1 = 0;
1175  j1 = (-A * i1 - C) / B;
1176  i2 = I.getHeight() - 1.0;
1177  j2 = (-A * i2 - C) / B;
1178 
1179  ip1.set_i(i1);
1180  ip1.set_j(j1);
1181  ip2.set_i(i2);
1182  ip2.set_j(j2);
1183  vpDisplay::displayLine(I, ip1, ip2, color);
1184  }
1185  else {
1186  double i1, j1, i2, j2;
1187  j1 = 0;
1188  i1 = -(B * j1 + C) / A;
1189  j2 = I.getWidth() - 1.0;
1190  i2 = -(B * j2 + C) / A;
1191 
1192  ip1.set_i(i1);
1193  ip1.set_j(j1);
1194  ip2.set_i(i2);
1195  ip2.set_j(j2);
1196  vpDisplay::displayLine(I, ip1, ip2, color);
1197  }
1198 
1199  ip1.set_i(PExt1.m_ifloat);
1200  ip1.set_j(PExt1.m_jfloat);
1201  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1202 
1203  ip1.set_i(PExt2.m_ifloat);
1204  ip1.set_j(PExt2.m_jfloat);
1205  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1206 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor blue
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:214
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.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:305
double get_j() const
Definition: vpImagePoint.h:125
void set_i(double ii)
Definition: vpImagePoint.h:294
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double sqr(double x)
Definition: vpMath.h:201
static int round(double x)
Definition: vpMath.h:405
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2343
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:147
double m_angle
Angle in deg between the extremities.
Definition: vpMeLine.h:158
double m_angle_1
Angle in deg between the extremities.
Definition: vpMeLine.h:159
double m_theta
theta parameter of the line
Definition: vpMeLine.h:155
int m_sign
Sign.
Definition: vpMeLine.h:160
void suppressPoints()
Definition: vpMeLine.cpp:424
double m_delta_1
Angle in rad between the extremities.
Definition: vpMeLine.h:157
virtual ~vpMeLine() vp_override
Definition: vpMeLine.cpp:115
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) vp_override
Definition: vpMeLine.cpp:120
double getRho() const
Definition: vpMeLine.cpp:832
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:729
void updateDelta()
Definition: vpMeLine.cpp:627
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
Definition: vpMeLine.cpp:192
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:594
double m_c
Parameter c of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:168
double getTheta() const
Definition: vpMeLine.cpp:834
bool m_useIntensityForRho
Definition: vpMeLine.h:164
double m_delta
Angle in rad between the extremities.
Definition: vpMeLine.h:156
double m_rho
rho parameter of the line
Definition: vpMeLine.h:154
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:483
vpMeLine()
Definition: vpMeLine.cpp:89
void track(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:662
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
Definition: vpMeLine.cpp:845
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
Definition: vpMeLine.cpp:836
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)
Definition: vpMeLine.cpp:1016
double m_a
Parameter a of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:166
double m_b
Parameter b of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:167
vpMeSite m_PExt[2]
Line extremities.
Definition: vpMeLine.h:152
void leastSquare()
Definition: vpMeLine.cpp:223
void setExtremities()
Definition: vpMeLine.cpp:437
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:197
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
int m_mask_sign
Mask sign.
Definition: vpMeSite.h:104
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
Definition: vpMeSite.h:89
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:247
double m_ifloat
Subpixel coordinates along i of a site.
Definition: vpMeSite.h:100
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:212
void setAlpha(const double &a)
Definition: vpMeSite.h:242
void init()
Definition: vpMeSite.cpp:55
vpMeSiteState getState() const
Definition: vpMeSite.h:266
int m_j
Integer coordinates along j of a site.
Definition: vpMeSite.h:98
int m_i
Integer coordinate along i of a site.
Definition: vpMeSite.h:96
double m_jfloat
Subpixel coordinates along j of a site.
Definition: vpMeSite.h:102
void setContrastThreshold(const double &thresh, const vpMe &me)
Definition: vpMeSite.h:286
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:267
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:256
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:60
void initTracking(const vpImage< unsigned char > &I)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMeTracker.h:74
unsigned int numberOfSignal()
Definition: vpMeTracker.cpp:95
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
Definition: vpMeTracker.h:78
void track(const vpImage< unsigned char > &I)
vpMe * m_me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:68
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.
Definition: vpMeTracker.h:76
std::list< vpMeSite > m_meList
Definition: vpMeTracker.h:66
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void setRange(const unsigned int &range)
Definition: vpMe.h:429
double getThresholdMarginRatio() const
Definition: vpMe.h:314
double getSampleStep() const
Definition: vpMe.h:289
unsigned int getRange() const
Definition: vpMe.h:282
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:136
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:156
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 vpCDEBUG(level)
Definition: vpDebug.h:499
#define vpDERROR_TRACE
Definition: vpDebug.h:452
#define vpERROR_TRACE
Definition: vpDebug.h:384
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:526