Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
vpTemplateTrackerZone.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 #include <limits> // numeric_limits
42 
43 #include <visp3/core/vpConfig.h>
44 
45 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
46 #include <opencv2/imgproc/imgproc.hpp>
47 #endif
48 
49 #include <visp3/tt/vpTemplateTrackerZone.h>
50 
54 vpTemplateTrackerZone::vpTemplateTrackerZone() : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1) {}
55 
60  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
61 {
62  *this = z;
63 }
64 
69 {
70  min_x = -1;
71  min_y = -1;
72  max_x = -1;
73  max_y = -1;
74 
75  Zone.clear();
76 }
77 
82 {
83  clear();
84 
85  this->copy(z);
86  return (*this);
87 }
88 
112 {
113  Zone.clear();
114 
115  std::vector<vpImagePoint> vip;
116 
117  bool end = false;
118 
119  do {
120  vpImagePoint p;
122  if (vpDisplay::getClick(I, p, button, false)) {
123  vip.push_back(p);
124 
126 
127  if (vip.size() > 1) {
128  if (delaunay) {
129  // Draw a line between the 2 last points
130  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
131  } else {
132  if (vip.size() % 3 == 2)
133  // draw line between point 2-1
134  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
135  else if (vip.size() % 3 == 0) {
136  // draw line between point 3-2
137  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
138  // draw line between point 3-1
139  vpDisplay::displayLine(I, p, vip[vip.size() - 3], vpColor::blue, 3);
140  }
141  }
142  }
143 
144  if (button == vpMouseButton::button3)
145  end = true;
146  }
147 
148  vpTime::wait(20);
149  vpDisplay::flush(I);
150  } while (!end);
151 
152  initFromPoints(I, vip, delaunay);
153 }
154 
168 void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
169  bool delaunay)
170 {
171  if (delaunay) {
172  if (vip.size() == 3) {
173  initFromPoints(I, vip, false);
174  } else if (vip.size() == 4) {
175  std::vector<vpImagePoint> vip_delaunay;
176  vip_delaunay.push_back(vip[0]);
177  vip_delaunay.push_back(vip[1]);
178  vip_delaunay.push_back(vip[2]);
179  vip_delaunay.push_back(vip[2]);
180  vip_delaunay.push_back(vip[3]);
181  vip_delaunay.push_back(vip[0]);
182  initFromPoints(I, vip_delaunay, false);
183  } else {
184 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
185  // Init Delaunay
186  cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
187  for (size_t i = 0; i < vip.size(); i++) {
188  cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
189  // std::cout << "Click point: " << vip[i] << std::endl;
190  subdiv.insert(fp);
191  }
192 
193  // Compute Delaunay triangulation
194  std::vector<cv::Vec6f> triangleList;
195  subdiv.getTriangleList(triangleList);
196 
197  // Keep only the Delaunay points that are inside the area
198  vpRect rect(0, 0, I.getWidth(), I.getHeight());
199 
200  std::vector<vpImagePoint> vip_delaunay;
201  for (size_t i = 0; i < triangleList.size(); i++) {
202  cv::Vec6f t = triangleList[i];
203  std::vector<vpImagePoint> p(3);
204 
205  p[0].set_uv(t[0], t[1]);
206  p[1].set_uv(t[2], t[3]);
207  p[2].set_uv(t[4], t[5]);
208 
209  if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
210  vip_delaunay.push_back(p[0]);
211  vip_delaunay.push_back(p[1]);
212  vip_delaunay.push_back(p[2]);
213  }
214  }
215 
216  initFromPoints(I, vip_delaunay, false);
217 #else
218  throw vpException(vpException::functionNotImplementedError, "Delaunay triangulation is not available!");
219 #endif
220  }
221  } else {
222  Zone.clear();
223  for (unsigned int i = 0; i < vip.size(); i += 3) {
224  vpTemplateTrackerTriangle triangle(vip[i], vip[i + 1], vip[i + 2]);
225  add(triangle);
226 
227  // vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green,
228  // 1); vpDisplay::displayLine(I, vip[i+1], vip[i+2],
229  // vpColor::green, 1); vpDisplay::displayLine(I, vip[i+2], vip[i],
230  // vpColor::green,1); vpDisplay::flush(I) ;
231 
232  // Update the bounding box
233  if ((triangle.getMinx() < min_x) || (min_x == -1))
234  min_x = (int)triangle.getMinx();
235  if ((triangle.getMaxx() > max_x) || (max_x == -1))
236  max_x = (int)triangle.getMaxx();
237  if ((triangle.getMiny() < min_y) || (min_y == -1))
238  min_y = (int)triangle.getMiny();
239  if ((triangle.getMaxy() > max_y) || (max_y == -1))
240  max_y = (int)triangle.getMaxy();
241  }
242  }
243 }
244 
250 {
251  Zone.push_back(t);
252 
253  // Update the bounding box
254  if ((t.getMinx() < min_x) || (min_x == -1))
255  min_x = (int)t.getMinx();
256  if ((t.getMaxx() > max_x) || (max_x == -1))
257  max_x = (int)t.getMaxx();
258  if ((t.getMiny() < min_y) || (min_y == -1))
259  min_y = (int)t.getMiny();
260  if ((t.getMaxy() > max_y) || (max_y == -1))
261  max_y = (int)t.getMaxy();
262 }
263 
270 bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
271 {
272  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
273  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
274  if (Iterateurvecteur->inTriangle(i, j))
275  return true;
276  }
277  return false;
278 }
279 
286 bool vpTemplateTrackerZone::inZone(const double &i, const double &j) const
287 {
288  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
289  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
290  if (Iterateurvecteur->inTriangle(i, j))
291  return true;
292  }
293  return false;
294 }
295 
303 bool vpTemplateTrackerZone::inZone(const int &i, const int &j, unsigned int &id_triangle) const
304 {
305  unsigned int id = 0;
306  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
307  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
308  if (Iterateurvecteur->inTriangle(i, j)) {
309  id_triangle = id;
310  return true;
311  }
312  id++;
313  }
314  return false;
315 }
316 
324 bool vpTemplateTrackerZone::inZone(const double &i, const double &j, unsigned int &id_triangle) const
325 {
326  unsigned int id = 0;
327  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
328  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
329  if (Iterateurvecteur->inTriangle(i, j)) {
330  id_triangle = id;
331  return true;
332  }
333  id++;
334  }
335  return false;
336 }
337 
357 {
358  if (i > getNbTriangle() - 1)
359  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
360 
361  T = Zone[i];
362 }
378 {
379  if (i > getNbTriangle() - 1)
380  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
381 
382  return Zone[i];
383 }
389 {
390  double xc = 0;
391  double yc = 0;
392  int cpt = 0;
393  for (int i = min_y; i < max_y; i++)
394  for (int j = min_x; j < max_x; j++)
395  if (inZone(i, j)) {
396  xc += j;
397  yc += i;
398  cpt++;
399  }
400  if (!cpt) {
401  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
402  }
403  xc = xc / cpt;
404  yc = yc / cpt;
405  vpImagePoint ip;
406  ip.set_uv(xc, yc);
407  return ip;
408 }
409 
414 int vpTemplateTrackerZone::getMaxx() const { return max_x; }
419 int vpTemplateTrackerZone::getMaxy() const { return max_y; }
424 int vpTemplateTrackerZone::getMinx() const { return min_x; }
429 int vpTemplateTrackerZone::getMiny() const { return min_y; }
430 
436 {
437  vpRect bbox;
440  return bbox;
441 }
442 
448 void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
449 {
450  std::vector<vpImagePoint> ip;
451  for (unsigned int i = 0; i < Zone.size(); i++) {
452  vpTemplateTrackerTriangle triangle;
453  Zone[i].getCorners(ip);
454  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
455  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
456  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
457  }
458 }
459 
465 void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, unsigned int thickness)
466 {
467  std::vector<vpImagePoint> ip;
468  for (unsigned int i = 0; i < Zone.size(); i++) {
469  vpTemplateTrackerTriangle triangle;
470  Zone[i].getCorners(ip);
471  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
472  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
473  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
474  }
475 }
476 
481 
489 void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
490 {
491  assert(id < getNbTriangle());
492  vpTemplateTrackerTriangle triangle;
493  getTriangle(id, triangle);
494  for (int i = 0; i < (int)I.getHeight(); i++) {
495  for (int j = 0; j < (int)I.getWidth(); j++) {
496  if (triangle.inTriangle(i, j)) {
497  I[i][j] = gray_level;
498  }
499  }
500  }
501 }
502 
507 {
508  vpTemplateTrackerZone tempZone;
510  vpTemplateTrackerTriangle TtempDown;
511  for (unsigned int i = 0; i < getNbTriangle(); i++) {
512  getTriangle(i, Ttemp);
513  TtempDown = Ttemp.getPyramidDown();
514  tempZone.add(TtempDown);
515  }
516  return tempZone;
517 }
518 
525 {
526  vpTemplateTrackerTriangle triangle;
527  for (unsigned int i = 0; i < z.getNbTriangle(); i++) {
528  z.getTriangle(i, triangle);
529  add(triangle);
530  // Update the bounding box
531  if ((triangle.getMinx() < min_x) || (min_x == -1))
532  min_x = (int)triangle.getMinx();
533  if ((triangle.getMaxx() > max_x) || (max_x == -1))
534  max_x = (int)triangle.getMaxx();
535  if ((triangle.getMiny() < min_y) || (min_y == -1))
536  min_y = (int)triangle.getMiny();
537  if ((triangle.getMaxy() > max_y) || (max_y == -1))
538  max_y = (int)triangle.getMaxy();
539  }
540 }
541 
549 vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
550 {
551  int cpt_pt = 0;
552  double x_center = 0, y_center = 0;
553  for (int j = 0; j < borne_x; j++)
554  for (int i = 0; i < borne_y; i++)
555  if (inZone(i, j)) {
556  x_center += j;
557  y_center += i;
558  cpt_pt++;
559  }
560 
561  if (!cpt_pt) {
562  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
563  }
564 
565  x_center = x_center / cpt_pt;
566  y_center = y_center / cpt_pt;
567  vpImagePoint center;
568  center.set_uv(x_center, y_center);
569  return center;
570 }
571 
576 {
577  double area = 0;
578  vpTemplateTrackerTriangle triangle;
579  for (unsigned int i = 0; i < getNbTriangle(); i++) {
580  getTriangle(i, triangle);
581  area += triangle.getArea();
582  }
583  return area;
584 }
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
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)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Defines a rectangle in the plane.
Definition: vpRect.h:80
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:297
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:367
bool inTriangle(const vpImagePoint &ip) const
vpTemplateTrackerTriangle getPyramidDown() const
vpTemplateTrackerZone getPyramidDown() const
std::vector< vpTemplateTrackerTriangle > Zone
Vector of triangles that defines the zone.
int max_y
Bounding box parameter.
vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z)
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &ip, bool delaunay=false)
bool inZone(const int &i, const int &j) const
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getNbTriangle() const
void copy(const vpTemplateTrackerZone &z)
int max_x
Bounding box parameter.
void fillTriangle(vpImage< unsigned char > &I, unsigned int id, unsigned char gray_level)
void add(const vpTemplateTrackerTriangle &t)
vpImagePoint getCenter() const
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, unsigned int thickness=3)
int min_y
Bounding box parameter.
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
int min_x
Bounding box parameter.
VISP_EXPORT int wait(double t0, double t)