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