Visual Servoing Platform  version 3.6.1 under development (2024-12-06)
vpDot2.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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  * Track a white dot.
32  */
33 
39 #include <visp3/core/vpDisplay.h>
40 
41 // exception handling
42 #include <visp3/core/vpIoTools.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpTrackingException.h>
45 
46 #include <cmath> // std::fabs
47 #include <iostream>
48 #include <limits> // numeric_limits
49 #include <math.h>
50 #include <visp3/blob/vpDot2.h>
51 
52 BEGIN_VISP_NAMESPACE
53 
54 /******************************************************************************
55  *
56  * CONSTRUCTORS AND DESTRUCTORS
57  *
58 *****************************************************************************/
64 void vpDot2::init()
65 {
66  const unsigned int val_max = 255;
67  const unsigned int val_median = 128;
68  m_cog.set_u(0);
69  m_cog.set_v(0);
70 
71  m_width = 0;
72  m_height = 0;
73  m_surface = 0;
74  m_mean_gray_level = 0;
75  m_gray_level_min = val_median;
76  m_gray_level_max = val_max;
77  m_grayLevelPrecision = 0.80;
78  m_gamma = 1.5;
79 
80  m_sizePrecision = 0.65;
81  m_ellipsoidShapePrecision = 0.65;
82  m_maxSizeSearchDistPrecision = 0.65;
84  m00 = 0.;
85  m11 = 0.;
86  m02 = 0.;
87  m20 = 0.;
88  m10 = 0.;
89  m01 = 0.;
90  mu11 = 0.;
91  mu02 = 0.;
92  mu20 = 0.;
93 
94  m_bbox_u_min = 0;
95  m_bbox_u_max = 0;
96  m_bbox_v_min = 0;
97  m_bbox_v_max = 0;
98 
99  m_firstBorder_u = 0;
100  m_firstBorder_v = 0;
101 
102  m_compute_moment = false;
103  m_graphics = false;
104  m_thickness = 1;
105 }
106 
111  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(), m_width(0), m_height(0),
112  m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5),
113  m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65),
114  m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false),
115  m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v()
116 {
117  const unsigned int val_max = 255;
118  const unsigned int val_median = 128;
119  m_gray_level_min = val_median;
120  m_gray_level_max = val_max;
121 }
122 
132  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(ip), m_width(0), m_height(0),
133  m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5),
134  m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65),
135  m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false),
136  m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v()
137 {
138  const unsigned int val_max = 255;
139  const unsigned int val_median = 128;
140  m_gray_level_min = val_median;
141  m_gray_level_max = val_max;
142 }
143 
147 vpDot2::vpDot2(const vpDot2 &twinDot)
148  : vpTracker(twinDot), m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(),
149  m_width(0), m_height(0), m_surface(0), m_mean_gray_level(0),
150  m_grayLevelPrecision(0.8), m_gamma(1.5), m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65),
151  m_maxSizeSearchDistPrecision(0.65), m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(),
152  m_compute_moment(false), m_graphics(false), m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0),
153  m_firstBorder_u(0), m_firstBorder_v()
154 {
155  const unsigned int val_max = 255;
156  const unsigned int val_median = 128;
157  m_gray_level_min = val_median;
158  m_gray_level_max = val_max;
159  *this = twinDot;
160 }
161 
166 {
167  m_cog = twinDot.m_cog;
168 
169  m_width = twinDot.m_width;
170  m_height = twinDot.m_height;
171  m_surface = twinDot.m_surface;
172  m_gray_level_min = twinDot.m_gray_level_min;
173  m_gray_level_max = twinDot.m_gray_level_max;
174  m_mean_gray_level = twinDot.m_mean_gray_level;
175  m_grayLevelPrecision = twinDot.m_grayLevelPrecision;
176  m_gamma = twinDot.m_gamma;
177 
178  m_sizePrecision = twinDot.m_sizePrecision;
179  m_ellipsoidShapePrecision = twinDot.m_ellipsoidShapePrecision;
180  m_maxSizeSearchDistPrecision = twinDot.m_maxSizeSearchDistPrecision;
181  m_allowedBadPointsPercentage = twinDot.m_allowedBadPointsPercentage;
182  m_area = twinDot.m_area;
183 
184  m_direction_list = twinDot.m_direction_list;
185  m_ip_edges_list = twinDot.m_ip_edges_list;
186 
187  m_compute_moment = twinDot.m_compute_moment;
188  m_graphics = twinDot.m_graphics;
189  m_thickness = twinDot.m_thickness;
190 
191  m_bbox_u_min = twinDot.m_bbox_u_min;
192  m_bbox_u_max = twinDot.m_bbox_u_max;
193  m_bbox_v_min = twinDot.m_bbox_v_min;
194  m_bbox_v_max = twinDot.m_bbox_v_max;
195 
196  m_firstBorder_u = twinDot.m_firstBorder_u;
197  m_firstBorder_v = twinDot.m_firstBorder_v;
198 
199  m00 = twinDot.m00;
200  m01 = twinDot.m01;
201  m11 = twinDot.m11;
202  m10 = twinDot.m10;
203  m02 = twinDot.m02;
204  m20 = twinDot.m20;
205 
206  mu11 = twinDot.mu11;
207  mu20 = twinDot.mu20;
208  mu02 = twinDot.mu02;
209 
210  return (*this);
211 }
212 
213 /******************************************************************************
214  *
215  * PUBLIC METHODS
216  *****************************************************************************/
217 
225 void vpDot2::display(const vpImage<unsigned char> &I, vpColor color, unsigned int t) const
226 {
227  const unsigned int val_3 = 3;
228  const unsigned int val_8 = 8;
229  vpDisplay::displayCross(I, m_cog, (val_3 * t) + val_8, color, t);
230  std::list<vpImagePoint>::const_iterator it;
231 
232  std::list<vpImagePoint>::const_iterator ip_edges_list_end = m_ip_edges_list.end();
233  for (it = m_ip_edges_list.begin(); it != ip_edges_list_end; ++it) {
234  vpDisplay::displayPoint(I, *it, color);
235  }
236 }
237 
269 void vpDot2::initTracking(const vpImage<unsigned char> &I, unsigned int size)
270 {
271  while (vpDisplay::getClick(I, m_cog) != true) {
272  // block empty waiting user interaction
273  }
274 
275  unsigned int i = static_cast<unsigned int>(m_cog.get_i());
276  unsigned int j = static_cast<unsigned int>(m_cog.get_j());
277  const unsigned int val_max = 255;
278 
279  double Ip = pow(static_cast<double>(I[i][j]) / val_max, 1 / m_gamma);
280 
281  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
282  m_gray_level_min = 0;
283  }
284  else {
285  m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
286  if (m_gray_level_min > val_max) {
287  m_gray_level_min = val_max;
288  }
289  }
290  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
291  if (m_gray_level_max > val_max) {
292  m_gray_level_max = val_max;
293  }
294 
295  setWidth(size);
296  setHeight(size);
297 
298  track(I);
299 }
300 
328 void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size)
329 {
330  m_cog = ip;
331 
332  unsigned int i = static_cast<unsigned int>(m_cog.get_i());
333  unsigned int j = static_cast<unsigned int>(m_cog.get_j());
334  const unsigned int val_max = 255;
335 
336  double Ip = pow(static_cast<double>(I[i][j]) / val_max, 1 / m_gamma);
337 
338  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
339  m_gray_level_min = 0;
340  }
341  else {
342  m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
343  if (m_gray_level_min > val_max) {
344  m_gray_level_min = val_max;
345  }
346  }
347  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
348  if (m_gray_level_max > val_max) {
349  m_gray_level_max = val_max;
350  }
351 
352  setWidth(size);
353  setHeight(size);
354 
355  track(I);
356 }
357 
397 void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int gray_lvl_min,
398  unsigned int gray_lvl_max, unsigned int size)
399 {
400  m_cog = ip;
401 
402  m_gray_level_min = gray_lvl_min;
403  m_gray_level_max = gray_lvl_max;
404 
405  setWidth(size);
406  setHeight(size);
407 
408  track(I);
409 }
410 
452 void vpDot2::track(const vpImage<unsigned char> &I, bool canMakeTheWindowGrow)
453 {
454  m00 = 0;
455  m11 = 0;
456  m02 = 0;
457  m20 = 0;
458  m10 = 0;
459  m01 = 0;
460 
461  // First, we will estimate the position of the tracked point
462 
463  // Set the search area to the entire image
464  setArea(I);
465 
466  // create a copy of the dot to search
467  // This copy can be saw as the previous dot used to check if the current one
468  // found with computeParameters() is similar to the previous one (see
469  // isValid() function). If the found dot is not similar (or valid), we use
470  // this copy to set the current found dot to the previous one (see below).
471  vpDot2 wantedDot(*this);
472 
473  bool found = computeParameters(I, m_cog.get_u(), m_cog.get_v());
474 
475  if (found) {
476  // test if the found dot is valid (ie similar to the previous one)
477  found = isValid(I, wantedDot);
478  if (!found) {
479  *this = wantedDot;
480  // std::cout << "The found dot is not valid" << std::endl;
481  }
482  }
483 
484  if (!found) {
485  // if estimation was wrong (get an error tracking), look for the dot
486  // closest from the estimation,
487  // i.e. search for dots in an a region of interest around the this dot and
488  // get the first element in the area.
489 
490  // first get the size of the search window from the dot size
491  double searchWindowWidth = 0.0;
492  double searchWindowHeight = 0.0;
493 
494  if ((std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon()) ||
495  (std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon())) {
496  searchWindowWidth = 80.;
497  searchWindowHeight = 80.;
498  }
499  else if (canMakeTheWindowGrow) {
500  const unsigned int val_5 = 5;
501  searchWindowWidth = getWidth() * val_5;
502  searchWindowHeight = getHeight() * val_5;
503  }
504  else {
505  searchWindowWidth = getWidth();
506  searchWindowHeight = getHeight();
507  }
508 
509  std::list<vpDot2> candidates;
510  searchDotsInArea(I, static_cast<int>(m_cog.get_u() - (searchWindowWidth / 2.0)),
511  static_cast<int>(m_cog.get_v() - (searchWindowHeight / 2.0)),
512  static_cast<unsigned int>(searchWindowWidth),
513  static_cast<unsigned int>(searchWindowHeight), candidates);
514 
515  // if the vector is empty, that mean we didn't find any candidate
516  // in the area, return an error tracking.
517  if (candidates.empty()) {
519  }
520 
521  // otherwise we've got our dot, update this dot's parameters
522  vpDot2 movingDot = candidates.front();
523 
524  setCog(movingDot.getCog());
525  setArea(movingDot.getArea());
526  setWidth(movingDot.getWidth());
527  setHeight(movingDot.getHeight());
528 
529  // Update the moments
530  m00 = movingDot.m00;
531  m01 = movingDot.m01;
532  m10 = movingDot.m10;
533  m11 = movingDot.m11;
534  m20 = movingDot.m20;
535  m02 = movingDot.m02;
536 
537  // Update the bounding box
538  m_bbox_u_min = movingDot.m_bbox_u_min;
539  m_bbox_u_max = movingDot.m_bbox_u_max;
540  m_bbox_v_min = movingDot.m_bbox_v_min;
541  m_bbox_v_max = movingDot.m_bbox_v_max;
542  }
543 
544  // if this dot is partially out of the image, return an error tracking.
545  if (!isInImage(I)) {
547  "The center of gravity of the dot is not in the image"));
548  }
549 
550  const unsigned int val_max = 255;
551  double Ip = pow(getMeanGrayLevel() / val_max, 1 / m_gamma);
552  // printf("current value of gray level center : %i\n", I[v][u]);
553 
554  // get Mean Gray Level of I
555  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
556  m_gray_level_min = 0;
557  }
558  else {
559  m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
560  if (m_gray_level_min > val_max) {
561  m_gray_level_min = val_max;
562  }
563  }
564  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
565  if (m_gray_level_max > val_max) {
566  m_gray_level_max = val_max;
567  }
568 
569  if (m_graphics) {
570  // display a red cross at the center of gravity's location in the image.
571  const unsigned int val_3 = 3;
572  const unsigned int val_8 = 8;
573  vpDisplay::displayCross(I, m_cog, (val_3 * m_thickness) + val_8, vpColor::red, m_thickness);
574  }
575 }
576 
599 void vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip, bool canMakeTheWindowGrow)
600 {
601  track(I, canMakeTheWindowGrow);
602 
603  ip = m_cog;
604 }
605 
608 
614 double vpDot2::getWidth() const { return m_width; }
615 
621 double vpDot2::getHeight() const { return m_height; }
622 
628 double vpDot2::getArea() const { return fabs(m_surface); }
629 
635 double vpDot2::getGrayLevelPrecision() const { return m_grayLevelPrecision; }
636 
642 double vpDot2::getSizePrecision() const { return m_sizePrecision; }
643 
651 double vpDot2::getEllipsoidShapePrecision() const { return m_ellipsoidShapePrecision; }
652 
659 double vpDot2::getMaxSizeSearchDistPrecision() const { return m_maxSizeSearchDistPrecision; }
660 
664 double vpDot2::getDistance(const vpDot2 &distantDot) const
665 {
666  vpImagePoint cogDistantDot = distantDot.getCog();
667  double diff_u = m_cog.get_u() - cogDistantDot.get_u();
668  double diff_v = m_cog.get_v() - cogDistantDot.get_v();
669  return sqrt((diff_u * diff_u) + (diff_v * diff_v));
670 }
671 
673 
683 void vpDot2::setWidth(const double &w) { m_width = w; }
684 
695 void vpDot2::setHeight(const double &h) { m_height = h; }
696 
707 void vpDot2::setArea(const double &a) { m_surface = a; }
708 
726 void vpDot2::setGrayLevelPrecision(const double &precision)
727 {
728  double epsilon = 0.05;
729  if (m_grayLevelPrecision < epsilon) {
730  m_grayLevelPrecision = epsilon;
731  }
732  else if (m_grayLevelPrecision > 1) {
733  m_grayLevelPrecision = 1.0;
734  }
735  else {
736  m_grayLevelPrecision = precision;
737  }
738 }
756 void vpDot2::setSizePrecision(const double &precision)
757 {
758  if (m_sizePrecision < 0) {
759  m_sizePrecision = 0;
760  }
761  else if (m_sizePrecision > 1) {
762  m_sizePrecision = 1.0;
763  }
764  else {
765  m_sizePrecision = precision;
766  }
767 }
768 
801 void vpDot2::setEllipsoidShapePrecision(const double &precision)
802 {
803 
804  if (m_ellipsoidShapePrecision < 0) {
805  m_ellipsoidShapePrecision = 0;
806  }
807  else if (m_ellipsoidShapePrecision > 1) {
808  m_ellipsoidShapePrecision = 1.0;
809  }
810  else {
811  m_ellipsoidShapePrecision = precision;
812  }
813 }
814 
830 void vpDot2::setMaxSizeSearchDistPrecision(const double &precision)
831 {
832  double epsilon = 0.05;
833  if (m_maxSizeSearchDistPrecision < epsilon) {
834  m_maxSizeSearchDistPrecision = epsilon;
835  }
836  else if (m_maxSizeSearchDistPrecision > 1) {
837  m_maxSizeSearchDistPrecision = 1.0;
838  }
839  else {
840  m_maxSizeSearchDistPrecision = precision;
841  }
842 }
843 
852 void vpDot2::setArea(const vpImage<unsigned char> &I) { setArea(I, 0, 0, I.getWidth(), I.getHeight()); }
853 
866 void vpDot2::setArea(const vpImage<unsigned char> &I, int u, int v, unsigned int w, unsigned int h)
867 {
868  unsigned int image_w = I.getWidth();
869  unsigned int image_h = I.getHeight();
870 
871  // Bounds the area to the image
872  if (u < 0) {
873  u = 0;
874  }
875  else if (u >= static_cast<int>(image_w)) {
876  u = static_cast<int>(image_w) - 1;
877  }
878  if (v < 0) {
879  v = 0;
880  }
881  else if (v >= static_cast<int>(image_h)) {
882  v = static_cast<int>(image_h) - 1;
883  }
884 
885  if ((static_cast<unsigned int>(u) + w) > image_w) {
886  w = image_w - static_cast<unsigned int>(u) - 1;
887  }
888  if ((static_cast<unsigned int>(v) + h) > image_h) {
889  h = image_h - static_cast<unsigned int>(v) - 1;
890  }
891 
892  m_area.setRect(u, v, w, h);
893 }
894 
902 void vpDot2::setArea(const vpRect &area) { m_area = area; }
903 
905 
957 void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, std::list<vpDot2> &niceDots)
958 {
959  searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
960 }
961 
982 bool vpDot2::isValid(const vpImage<unsigned char> &I, const vpDot2 &wantedDot)
983 {
984  double size_precision = wantedDot.getSizePrecision();
985  double ellipsoidShape_precision = wantedDot.getEllipsoidShapePrecision();
986 
987  //
988  // First, check the width, height and surface of the dot. Those parameters
989  // must be the same.
990  //
991  // if ( (wantedDot.getWidth() != 0)
992  // && (wantedDot.getHeight() != 0)
993  // && (wantedDot.getArea() != 0) )
994  if ((std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon()) &&
995  (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon()) &&
996  (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon())) {
997  if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()) {
998  double epsilon = 0.001;
999 #ifdef DEBUG
1000  std::cout << "test size precision......................\n";
1001  std::cout << "wanted dot: "
1002  << "w=" << wantedDot.getWidth() << " h=" << wantedDot.getHeight() << " s=" << wantedDot.getArea()
1003  << " precision=" << size_precision << " epsilon=" << epsilon << std::endl;
1004  std::cout << "dot found: "
1005  << "w=" << getWidth() << " h=" << getHeight() << " s=" << getArea() << std::endl;
1006 #endif
1007 
1008  if ((((wantedDot.getWidth() * size_precision) - epsilon) < getWidth()) == false) {
1009 #ifdef DEBUG
1010  printf("Bad width > for dot (%g, %g)\n", m_cog.get_u(), m_cog.get_v());
1011 #endif
1012  return false;
1013  }
1014 
1015  if ((getWidth() < (wantedDot.getWidth() / (size_precision + epsilon))) == false) {
1016 #ifdef DEBUG
1017  printf("Bad width %g > %g for dot (%g, %g)\n", getWidth(), wantedDot.getWidth() / (size_precision + epsilon),
1018  m_cog.get_u(), m_cog.get_v());
1019 #endif
1020  return false;
1021  }
1022 
1023  if ((((wantedDot.getHeight() * size_precision) - epsilon) < getHeight()) == false) {
1024 #ifdef DEBUG
1025  printf("Bad height %g > %g for dot (%g, %g)\n", wantedDot.getHeight() * size_precision - epsilon, getHeight(),
1026  m_cog.get_u(), m_cog.get_v());
1027 #endif
1028  return false;
1029  }
1030 
1031  if ((getHeight() < (wantedDot.getHeight() / (size_precision + epsilon))) == false) {
1032 #ifdef DEBUG
1033  printf("Bad height %g > %g for dot (%g, %g)\n", getHeight(), wantedDot.getHeight() / (size_precision + epsilon),
1034  m_cog.get_u(), m_cog.get_v());
1035 #endif
1036  return false;
1037  }
1038 
1039  if ((((wantedDot.getArea() * (size_precision * size_precision)) - epsilon) < getArea()) == false) {
1040 #ifdef DEBUG
1041  printf("Bad surface %g > %g for dot (%g, %g)\n",
1042  wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), m_cog.get_u(), m_cog.get_v());
1043 #endif
1044  return false;
1045  }
1046 
1047  if ((getArea() < (wantedDot.getArea() / ((size_precision * size_precision) + epsilon))) == false) {
1048 #ifdef DEBUG
1049  printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(),
1050  wantedDot.getArea() / (size_precision * size_precision + epsilon), m_cog.get_u(), m_cog.get_v());
1051 #endif
1052  return false;
1053  }
1054  }
1055  }
1056  //
1057  // Now we can proceed to more advanced (and costy) checks.
1058  // First check there is a white (>level) elipse within dot
1059  // Then check the dot is surrounded by a black ellipse.
1060  //
1061  int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid
1062  int nb_bad_points = 0;
1063  int nb_max_bad_points = static_cast<int>(nb_point_to_test * m_allowedBadPointsPercentage);
1064  double step_angle = (2 * M_PI) / nb_point_to_test;
1065 
1066  // --comment: if ellipsoidShape_precision diff 0 and compute_moment is true
1067  if ((std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon()) && m_compute_moment) {
1068  // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 15
1069 
1070  /*
1071  // -comment: mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
1072  // -comment: = m11 - m10 * m01 / m00
1073  // -comment: mu20 = m20 - m00 * xg^2 = m20 - m00 * m10/m00 * m10/m00
1074  // -comment: = m20 - m10^2 / m00
1075  // -comment: mu02 = m02 - m01^2 / m00
1076  // -comment: alpha = 1/2 arctan( 2 * mu11 / (mu20 - mu02) )
1077  //
1078  // -comment: a1^2 = 2 / m00 * (mu02 + mu20 + sqrt( (mu20 - mu02)^2 + 4mu11^2) )
1079  //
1080  // -comment: a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) )
1081  */
1082  // we compute parameters of the estimated ellipse
1083  double tmp1 = (((m01 * m01) - (m10 * m10)) / m00) + (m20 - m02);
1084  double tmp2 = m11 - ((m10 * m01) / m00);
1085  double Sqrt = sqrt((tmp1 * tmp1) + (4 * tmp2 * tmp2));
1086  double a1 = sqrt((2 / m00) * (((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00)) + Sqrt));
1087  double a2 = sqrt((2 / m00) * (((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00)) - Sqrt));
1088  double alpha = 0.5 * atan2(2 * ((m11 * m00) - (m10 * m01)), ((((m20 - m02) * m00) - (m10 * m10)) + (m01 * m01)));
1089 
1090  // to be able to track small dots, minorize the ellipsoid radius for the
1091  // inner test
1092  a1 -= 1.0;
1093  a2 -= 1.0;
1094 
1095  double innerCoef = ellipsoidShape_precision;
1096  unsigned int u, v;
1097  double cog_u = m_cog.get_u();
1098  double cog_v = m_cog.get_v();
1099  double val_2 = 2;
1100 
1101  vpImagePoint ip;
1102  nb_bad_points = 0;
1103  for (double theta = 0.; theta < (val_2 * M_PI); theta += step_angle) {
1104  u = static_cast<unsigned int>(cog_u + (innerCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta)))));
1105  v = static_cast<unsigned int>(cog_v + (innerCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta)))));
1106  if (!this->hasGoodLevel(I, u, v)) {
1107 #ifdef DEBUG
1108  printf("Inner circle pixel (%u, %u) has bad level for dot (%g, %g): "
1109  "%d not in [%u, %u]\n",
1110  u, v, cog_u, cog_v, I[v][u], m_gray_level_min, m_gray_level_max);
1111 #endif
1112  ++nb_bad_points;
1113  }
1114  if (m_graphics) {
1115  for (unsigned int t = 0; t < m_thickness; ++t) {
1116  ip.set_u(u + t);
1117  ip.set_v(v);
1119  }
1120  }
1121 #ifdef DEBUG
1123  vpDisplay::flush(I);
1124 #endif
1125  }
1126  if (nb_bad_points > nb_max_bad_points) {
1127 #ifdef DEBUG
1128  printf("Inner ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points);
1129 #endif
1130  return false;
1131  }
1132  // to be able to track small dots, maximize the ellipsoid radius for the
1133  // inner test
1134  a1 += 2.0;
1135  a2 += 2.0;
1136 
1137  double outCoef = 2 - ellipsoidShape_precision; // --comment: 1.6
1138  nb_bad_points = 0;
1139  for (double theta = 0.; theta < (val_2 * M_PI); theta += step_angle) {
1140  u = static_cast<unsigned int>(cog_u + (outCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta)))));
1141  v = static_cast<unsigned int>(cog_v + (outCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta)))));
1142 #ifdef DEBUG
1143  // vpDisplay::displayRectangle(I, area, vpColor::yellow);
1144  vpDisplay::displayCross(I, (int)v, (int)u, 7, vpColor::purple);
1145  vpDisplay::flush(I);
1146 #endif
1147  // If outside the area, continue
1148  if ((static_cast<double>(u) < m_area.getLeft()) ||
1149  (static_cast<double>(u) > m_area.getRight()) ||
1150  (static_cast<double>(v) < m_area.getTop()) ||
1151  (static_cast<double>(v) > m_area.getBottom())) {
1152  // continue
1153  }
1154  else {
1155  if (!this->hasReverseLevel(I, u, v)) {
1156 #ifdef DEBUG
1157  printf("Outside circle pixel (%u, %u) has bad level for dot (%g, "
1158  "%g): %d not in [%u, %u]\n",
1159  u, v, cog_u, cog_v, I[v][u], m_gray_level_min, m_gray_level_max);
1160 #endif
1161  ++nb_bad_points;
1162  }
1163  if (m_graphics) {
1164  for (unsigned int t = 0; t < m_thickness; ++t) {
1165  ip.set_u(u + t);
1166  ip.set_v(v);
1167 
1169  }
1170  }
1171  }
1172  }
1173  }
1174  if (nb_bad_points > nb_max_bad_points) {
1175 #ifdef DEBUG
1176  printf("Outside ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points);
1177 #endif
1178  return false;
1179  }
1180 
1181  return true;
1182 }
1183 
1202 bool vpDot2::hasGoodLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
1203 {
1204  if (!isInArea(u, v)) {
1205  return false;
1206  }
1207 
1208  if ((I[v][u] >= m_gray_level_min) && (I[v][u] <= m_gray_level_max)) {
1209  return true;
1210  }
1211  else {
1212  return false;
1213  }
1214 }
1215 
1228 bool vpDot2::hasReverseLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
1229 {
1230 
1231  if (!isInArea(u, v)) {
1232  return false;
1233  }
1234 
1235  if ((I[v][u] < m_gray_level_min) || (I[v][u] > m_gray_level_max)) {
1236  return true;
1237  }
1238  else {
1239  return false;
1240  }
1241 }
1242 
1251 vpDot2 *vpDot2::getInstance() { return new vpDot2(); }
1252 
1253 
1254 /******************************************************************************
1255  *
1256  * PRIVATE METHODS
1257  *
1258  ******************************************************************************/
1259 
1291 bool vpDot2::computeParameters(const vpImage<unsigned char> &I, const double &v_u, const double &v_v)
1292 {
1293  m_direction_list.clear();
1294  m_ip_edges_list.clear();
1295 
1296  double est_u = v_u; // estimated
1297  double est_v = v_v;
1298 
1299  // if u has default value, set it to the actual center value
1300  // if( est_u == -1.0 )
1301  if (std::fabs(est_u + 1.0) <= (vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits<double>::epsilon())) {
1302  est_u = m_cog.get_u();
1303  }
1304 
1305  // if v has default value, set it to the actual center value
1306  // if( est_v == -1.0 )
1307  if (std::fabs(est_v + 1.0) <= (vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits<double>::epsilon())) {
1308  est_v = m_cog.get_v();
1309  }
1310 
1311  // if the estimated position of the dot is out of the image, not need to
1312  // continue, return an error tracking
1313  if (!isInArea(static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v))) {
1314  return false;
1315  }
1316 
1317  m_bbox_u_min = static_cast<int>(I.getWidth());
1318  m_bbox_u_max = 0;
1319  m_bbox_v_min = static_cast<int>(I.getHeight());
1320  m_bbox_v_max = 0;
1321 
1322  // if the first point doesn't have the right level then there's no point to
1323  // continue.
1324  if (!hasGoodLevel(I, static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v))) {
1325  return false;
1326  }
1327 
1328  // find the border
1329 
1330  if (!findFirstBorder(I, static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v), m_firstBorder_u, m_firstBorder_v)) {
1331  return false;
1332  }
1333 
1334  unsigned int dir = 6;
1335 
1336  // Determine the first element of the Freeman chain
1337  computeFreemanChainElement(I, m_firstBorder_u, m_firstBorder_v, dir);
1338  unsigned int firstDir = dir;
1339 
1340  // if we are now out of the image, return an error tracking
1341  if (!isInArea(m_firstBorder_u, m_firstBorder_v)) {
1342  return false;
1343  }
1344 
1345  // store the new direction and dot border coordinates.
1346  m_direction_list.push_back(dir);
1347  vpImagePoint ip;
1348  ip.set_u(m_firstBorder_u);
1349  ip.set_v(m_firstBorder_v);
1350 
1351  m_ip_edges_list.push_back(ip);
1352 
1353  int border_u = static_cast<int>(m_firstBorder_u);
1354  int border_v = static_cast<int>(m_firstBorder_v);
1355  int du, dv;
1356  float dS, dMu, dMv, dMuv, dMu2, dMv2;
1357  m00 = 0.0;
1358  m10 = 0.0;
1359  m01 = 0.0;
1360  m11 = 0.0;
1361  m20 = 0.0;
1362  m02 = 0.0;
1363  // while we didn't come back to the first point, follow the border
1364  do {
1365  // if it was asked, show the border
1366  if (m_graphics) {
1367  for (int t = 0; t < static_cast<int>(m_thickness); ++t) {
1368  ip.set_u(border_u + t);
1369  ip.set_v(border_v);
1370 
1372  }
1373  }
1374 #ifdef DEBUG
1375  vpDisplay::displayPoint(I, border_v, border_u, vpColor::red);
1376  vpDisplay::flush(I);
1377 #endif
1378  // Determine the increments for the parameters
1379  computeFreemanParameters(border_u, border_v, dir, du, dv,
1380  dS, // surface
1381  dMu, dMv, // first order moments
1382  dMuv, dMu2, dMv2); // second order moment
1383 
1384  // Update the parameters
1385  border_u += du; // Next position on the border
1386  border_v += dv;
1387  m00 += dS; // enclosed area
1388  m10 += dMu; // First order moment along v axis
1389  m01 += dMv; // First order moment along u axis
1390  if (m_compute_moment) {
1391  m11 += dMuv; // Second order moment
1392  m20 += dMu2; // Second order moment along v axis
1393  m02 += dMv2; // Second order moment along u axis
1394  }
1395  // if we are now out of the image, return an error tracking
1396  if (!isInArea(static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v))) {
1397  // Can Occur on a single pixel dot located on the top border
1398  return false;
1399  }
1400 
1401  // store the new direction and dot border coordinates.
1402 
1403  m_direction_list.push_back(dir);
1404 
1405  ip.set_u(border_u);
1406  ip.set_v(border_v);
1407  m_ip_edges_list.push_back(ip);
1408 
1409  // update the extreme point of the dot.
1410  if (border_v < m_bbox_v_min) {
1411  m_bbox_v_min = border_v;
1412  }
1413  if (border_v > m_bbox_v_max) {
1414  m_bbox_v_max = border_v;
1415  }
1416  if (border_u < m_bbox_u_min) {
1417  m_bbox_u_min = border_u;
1418  }
1419  if (border_u > m_bbox_u_max) {
1420  m_bbox_u_max = border_u;
1421  }
1422 
1423  // move around the tracked entity by following the border.
1424  if (computeFreemanChainElement(I, static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v), dir) == false) {
1425  return false;
1426  }
1427  } while (((getFirstBorder_u() != static_cast<unsigned int>(border_u)) || (getFirstBorder_v() != static_cast<unsigned int>(border_v)) ||
1428  (firstDir != dir)) &&
1429  isInArea(static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v)));
1430 
1431 #ifdef VP_DEBUG
1432 #if VP_DEBUG_MODE == 3
1433  vpDisplay::flush(I);
1434 #endif
1435 #endif
1436 
1437  // if the surface is one or zero , the center of gravity wasn't properly
1438  // detected. Return an error tracking.
1439  // if( m00 == 0 || m00 == 1 )
1440  if ((std::fabs(m00) <= std::numeric_limits<double>::epsilon()) ||
1441  (std::fabs(m00 - 1.) <= (vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits<double>::epsilon()))) {
1442  return false;
1443  }
1444  else // compute the center
1445  {
1446  // this magic formula gives the coordinates of the center of gravity
1447  double tmpCenter_u = m10 / m00;
1448  double tmpCenter_v = m01 / m00;
1449 
1450  // Updates the second order centered moments
1451  if (m_compute_moment) {
1452  mu11 = m11 - (tmpCenter_u * m01);
1453  mu02 = m02 - (tmpCenter_v * m01);
1454  mu20 = m20 - (tmpCenter_u * m10);
1455  }
1456 
1457  m_cog.set_u(tmpCenter_u);
1458  m_cog.set_v(tmpCenter_v);
1459  }
1460 
1461  m_width = (m_bbox_u_max - m_bbox_u_min) + 1;
1462  m_height = (m_bbox_v_max - m_bbox_v_min) + 1;
1463  m_surface = m00;
1464 
1465  computeMeanGrayLevel(I);
1466  return true;
1467 }
1468 
1484 bool vpDot2::findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
1485  unsigned int &border_u, unsigned int &border_v)
1486 {
1487  // find the border
1488 
1489  // NOTE:
1490  // from here we use int and not double. This is because we don't have
1491  // rounding problems and it's actually more a trouble than something else to
1492  // work with double when navigating around the dot.
1493  border_u = u;
1494  border_v = v;
1495  double epsilon = 0.001;
1496 
1497 #ifdef DEBUG
1498  std::cout << "gray level: " << m_gray_level_min << " " << m_gray_level_max << std::endl;
1499 #endif
1500  while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < m_area.getRight()) /*I.getWidth()*/) {
1501  // if the width of this dot was initialised and we already crossed the dot
1502  // on more than the max possible width, no need to continue, return an
1503  // error tracking
1504  if ((getWidth() > 0) && ((border_u - u) > ((getWidth() / getMaxSizeSearchDistPrecision()) + epsilon))) {
1505  return false;
1506  }
1507 #ifdef DEBUG
1508  vpDisplay::displayPoint(I, static_cast<int>(border_v), static_cast<int>(border_u) + 1, vpColor::green);
1509  vpDisplay::flush(I);
1510 #endif
1511 
1512  ++border_u;
1513  }
1514  return true;
1515 }
1516 
1528 bool vpDot2::isInImage(const vpImage<unsigned char> &I) const { return isInImage(I, m_cog); }
1529 
1541 bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const
1542 {
1543  unsigned int h = I.getHeight();
1544  unsigned int w = I.getWidth();
1545  double u = ip.get_u();
1546  double v = ip.get_v();
1547 
1548  if ((u < 0) || (u >= w)) {
1549  return false;
1550  }
1551  if ((v < 0) || (v >= h)) {
1552  return false;
1553  }
1554  return true;
1555 }
1556 
1568 bool vpDot2::isInArea(const unsigned int &u, const unsigned int &v) const
1569 {
1570  unsigned int area_u_min = static_cast<unsigned int>(m_area.getLeft());
1571  unsigned int area_u_max = static_cast<unsigned int>(m_area.getRight());
1572  unsigned int area_v_min = static_cast<unsigned int>(m_area.getTop());
1573  unsigned int area_v_max = static_cast<unsigned int>(m_area.getBottom());
1574 
1575  if ((u < area_u_min) || (u > area_u_max)) {
1576  return false;
1577  }
1578  if ((v < area_v_min) || (v > area_v_max)) {
1579  return false;
1580  }
1581  return true;
1582 }
1583 
1592 void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight)
1593 {
1594  // first get the research grid width and height Note that
1595  // 1/sqrt(2)=cos(pi/4). The grid squares should be small enough to be
1596  // contained in the dot. We gent this here if the dot is a perfect disc.
1597  // More accurate criterium to define the grid should be implemented if
1598  // necessary
1599  gridWidth = static_cast<unsigned int>((getWidth() * getMaxSizeSearchDistPrecision()) / sqrt(2.));
1600  gridHeight = static_cast<unsigned int>((getHeight() * getMaxSizeSearchDistPrecision()) / sqrt(2.0));
1601 
1602  if (gridWidth == 0) {
1603  gridWidth = 1;
1604  }
1605  if (gridHeight == 0) {
1606  gridHeight = 1;
1607  }
1608 }
1609 
1619 void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char> &I)
1620 {
1621  int cog_u = static_cast<int>(m_cog.get_u());
1622  int cog_v = static_cast<int>(m_cog.get_v());
1623 
1624  unsigned int sum_value = 0;
1625  unsigned int nb_pixels = 0;
1626 
1627  for (unsigned int i = static_cast<unsigned int>(m_bbox_u_min); i <= static_cast<unsigned int>(m_bbox_u_max); ++i) {
1628  unsigned int pixel_gray = static_cast<unsigned int>(I[static_cast<unsigned int>(cog_v)][i]);
1629  if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
1630  sum_value += pixel_gray;
1631  ++nb_pixels;
1632  }
1633  }
1634  for (unsigned int i = static_cast<unsigned int>(m_bbox_v_min); i <= static_cast<unsigned int>(m_bbox_v_max); ++i) {
1635  unsigned char pixel_gray = I[i][static_cast<unsigned int>(cog_u)];
1636  if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
1637  sum_value += pixel_gray;
1638  ++nb_pixels;
1639  }
1640  }
1641  const unsigned int nb_min_pixels = 10;
1642  if (nb_pixels < nb_min_pixels) { // could be good to choose the min nb points from area of dot
1643  // add diagonals points to have enough point
1644  int imin, imax;
1645  if ((cog_u - m_bbox_u_min) >(cog_v - m_bbox_v_min)) {
1646  imin = cog_v - m_bbox_v_min;
1647  }
1648  else {
1649  imin = cog_u - m_bbox_u_min;
1650  }
1651  if ((m_bbox_u_max - cog_u) > (m_bbox_v_max - cog_v)) {
1652  imax = m_bbox_v_max - cog_v;
1653  }
1654  else {
1655  imax = m_bbox_u_max - cog_u;
1656  }
1657  for (int i = -imin; i <= imax; ++i) {
1658  unsigned int pixel_gray = static_cast<unsigned int>(I[static_cast<unsigned int>(cog_v + i)][static_cast<unsigned int>(cog_u + i)]);
1659  if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
1660  sum_value += pixel_gray;
1661  ++nb_pixels;
1662  }
1663  }
1664 
1665  if ((cog_u - m_bbox_u_min) > (m_bbox_v_max - cog_v)) {
1666  imin = m_bbox_v_max - cog_v;
1667  }
1668  else {
1669  imin = cog_u - m_bbox_u_min;
1670  }
1671  if ((m_bbox_u_max - cog_u) > (cog_v - m_bbox_v_min)) {
1672  imax = cog_v - m_bbox_v_min;
1673  }
1674  else {
1675  imax = m_bbox_u_max - cog_u;
1676  }
1677 
1678  for (int i = -imin; i <= imax; ++i) {
1679  unsigned char pixel_gray = I[static_cast<unsigned int>(cog_v - i)][static_cast<unsigned int>(cog_u + i)];
1680  if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
1681  sum_value += pixel_gray;
1682  ++nb_pixels;
1683  }
1684  }
1685  }
1686 
1687  if (nb_pixels == 0) {
1688  // should never happen
1689  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No point was found"));
1690  }
1691  else {
1692  m_mean_gray_level = sum_value / nb_pixels;
1693  }
1694 }
1695 
1714 vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I,
1715  vpColor col, bool trackDot)
1716 {
1717  vpMatrix Cogs(n, 2);
1718  vpImagePoint cog;
1719  unsigned int i;
1720  bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
1721  if (fromFile) {
1722  vpMatrix::loadMatrix(dotFile, Cogs);
1723  std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
1724  }
1725 
1726  // test number of cogs in file
1727  if (Cogs.getRows() < n) {
1728  std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
1729  fromFile = false;
1730  }
1731 
1732  // read from file and tracks the dots
1733  if (fromFile) {
1734  try {
1735  const unsigned int cross_size = 10;
1736  for (i = 0; i < n; ++i) {
1737  cog.set_uv(Cogs[i][0], Cogs[i][1]);
1738  dot[i].setGraphics(true);
1739  dot[i].setCog(cog);
1740  if (trackDot) {
1741  dot[i].initTracking(I, cog);
1742  dot[i].track(I);
1743  vpDisplay::displayCross(I, cog, cross_size, col);
1744  }
1745  }
1746  }
1747  catch (...) {
1748  std::cout << "Cannot track dots from file" << std::endl;
1749  fromFile = false;
1750  }
1751  vpDisplay::flush(I);
1752 
1753  // check that dots are far away ones from the other
1754  i = 0;
1755  while ((i < n) && fromFile) {
1756  double d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth()));
1757  unsigned int j = 0;
1758  while ((j < n) && fromFile) {
1759  if (j != i) {
1760  if (dot[i].getDistance(dot[j]) < d) {
1761  fromFile = false;
1762  std::cout << "Dots from file seem incoherent" << std::endl;
1763  }
1764  }
1765  ++j;
1766  }
1767  ++i;
1768  }
1769  }
1770 
1771  if (!fromFile) {
1772  vpDisplay::display(I);
1773  vpDisplay::flush(I);
1774 
1775  std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
1776  const unsigned int cross_size = 10;
1777  for (i = 0; i < n; ++i) {
1778  if (trackDot) {
1779  dot[i].setGraphics(true);
1780  dot[i].initTracking(I);
1781  cog = dot[i].getCog();
1782  }
1783  else {
1784  vpDisplay::getClick(I, cog);
1785  dot[i].setCog(cog);
1786  }
1787  Cogs[i][0] = cog.get_u();
1788  Cogs[i][1] = cog.get_v();
1789  vpDisplay::displayCross(I, cog, cross_size, col);
1790  vpDisplay::flush(I);
1791  }
1792  }
1793 
1794  if ((!fromFile) && (dotFile != "")) {
1795  vpMatrix::saveMatrix(dotFile, Cogs);
1796  std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
1797  }
1798 
1799  // back to non graphic mode
1800  for (i = 0; i < n; ++i) {
1801  dot[i].setGraphics(false);
1802  }
1803 
1804  return Cogs;
1805 }
1806 
1823 void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
1824  std::vector<vpImagePoint> &cogs, vpImagePoint *cogStar)
1825 {
1826  // tracking
1827  for (unsigned int i = 0; i < n; ++i) {
1828  dot[i].track(I);
1829  cogs.push_back(dot[i].getCog());
1830  }
1831  // trajectories
1832  unsigned int cogs_size = static_cast<unsigned int>(cogs.size());
1833  for (unsigned int i = n; i < cogs_size; ++i) {
1834  const unsigned int circle_size = 4;
1835  vpDisplay::displayCircle(I, cogs[i], circle_size, vpColor::green, true);
1836  }
1837  // initial position
1838  for (unsigned int i = 0; i < n; ++i) {
1839  const unsigned int circle_size = 4;
1840  vpDisplay::displayCircle(I, cogs[i], circle_size, vpColor::blue, true);
1841  }
1842  // if exists, desired position
1843  if (cogStar != nullptr) {
1844  const unsigned int circle_size = 10;
1845  for (unsigned int i = 0; i < n; ++i) {
1846  vpDisplay::displayDotLine(I, cogStar[i], dot[i].getCog(), vpColor::red);
1847  vpDisplay::displayCircle(I, cogStar[i], circle_size, vpColor::red, true);
1848  }
1849  }
1850  vpDisplay::flush(I);
1851 }
1852 
1868  const std::list<vpImagePoint> &edges_list, vpColor color, unsigned int thickness)
1869 {
1870  const unsigned int val_3 = 3;
1871  const unsigned int val_8 = 8;
1872 
1873  vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness);
1874  std::list<vpImagePoint>::const_iterator it;
1875 
1876  std::list<vpImagePoint>::const_iterator edges_list_end = edges_list.end();
1877  for (it = edges_list.begin(); it != edges_list_end; ++it) {
1878  vpDisplay::displayPoint(I, *it, color);
1879  }
1880 }
1881 
1896 void vpDot2::display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
1897  vpColor color, unsigned int thickness)
1898 {
1899  const unsigned int val_3 = 3;
1900  const unsigned int val_8 = 8;
1901  vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness);
1902  std::list<vpImagePoint>::const_iterator it;
1903  std::list<vpImagePoint>::const_iterator edges_list_end = edges_list.end();
1904  for (it = edges_list.begin(); it != edges_list_end; ++it) {
1905  vpDisplay::displayPoint(I, *it, color);
1906  }
1907 }
1908 
1914 VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDot2 &d) { return (os << "(" << d.getCog() << ")"); }
1915 
1916 END_VISP_NAMESPACE
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:614
unsigned int getRows() const
Definition: vpArray2D.h:347
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 purple
Definition: vpColor.h:228
static const vpColor green
Definition: vpColor.h:220
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void display(const vpImage< unsigned char > &I)
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 displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayDotLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:125
unsigned int getGrayLevelMin() const
Definition: vpDot2.h:219
vpDot2()
Definition: vpDot2.cpp:110
unsigned int getGrayLevelMax() const
Definition: vpDot2.h:225
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition: vpDot2.cpp:452
void setGraphics(bool activate)
Definition: vpDot2.h:318
vpDot2 & operator=(const vpDot2 &twinDot)
Definition: vpDot2.cpp:165
static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage< unsigned char > &I, std::vector< vpImagePoint > &cogs, vpImagePoint *cogStar=nullptr)
Definition: vpDot2.cpp:1823
double getEllipsoidShapePrecision() const
Definition: vpDot2.cpp:651
void searchDotsInArea(const vpImage< unsigned char > &I, int area_u, int area_v, unsigned int area_w, unsigned int area_h, std::list< vpDot2 > &niceDots)
void setMaxSizeSearchDistPrecision(const double &maxSizeSearchDistancePrecision)
Definition: vpDot2.cpp:830
void display(const vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1) const
Definition: vpDot2.cpp:225
double getArea() const
Definition: vpDot2.cpp:628
void setSizePrecision(const double &sizePrecision)
Definition: vpDot2.cpp:756
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition: vpDot2.cpp:726
void setHeight(const double &height)
Definition: vpDot2.cpp:695
double getMaxSizeSearchDistPrecision() const
Definition: vpDot2.cpp:659
void setCog(const vpImagePoint &ip)
Definition: vpDot2.h:261
vpImagePoint getCog() const
Definition: vpDot2.h:181
double getSizePrecision() const
Definition: vpDot2.cpp:642
double getGrayLevelPrecision() const
Definition: vpDot2.cpp:635
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition: vpDot2.h:290
double getDistance(const vpDot2 &distantDot) const
Definition: vpDot2.cpp:664
void setWidth(const double &width)
Definition: vpDot2.cpp:683
double getWidth() const
Definition: vpDot2.cpp:614
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition: vpDot2.cpp:801
double getMeanGrayLevel() const
Definition: vpDot2.h:234
void setArea(const double &area)
Definition: vpDot2.cpp:707
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:269
static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage< unsigned char > &I, vpColor col=vpColor::blue, bool trackDot=true)
Definition: vpDot2.cpp:1714
double getHeight() const
Definition: vpDot2.cpp:621
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_u() const
Definition: vpImagePoint.h:136
void set_u(double u)
Definition: vpImagePoint.h:335
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
void set_v(double v)
Definition: vpImagePoint.h:346
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:786
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static double sqr(double x)
Definition: vpMath.h:203
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=nullptr)
Definition: vpMatrix.h:829
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
Definition: vpMatrix.h:985
Defines a rectangle in the plane.
Definition: vpRect.h:79
double getLeft() const
Definition: vpRect.h:173
void setRect(double l, double t, double w, double h)
Definition: vpRect.h:333
double getRight() const
Definition: vpRect.h:179
double getBottom() const
Definition: vpRect.h:97
double getTop() const
Definition: vpRect.h:192
Class that defines what is a feature generic tracker.
Definition: vpTracker.h:61
Error that can be emitted by the vpTracker class and its derivatives.
@ featureLostError
Tracker lost feature.
@ notEnoughPointError
Not enough point to track.