Visual Servoing Platform  version 3.6.1 under development (2024-05-20)
vpTemplateTrackerZone.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <limits> // numeric_limits
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
41 #include <opencv2/imgproc/imgproc.hpp>
42 #endif
43 
44 #include <visp3/tt/vpTemplateTrackerZone.h>
45 
49 vpTemplateTrackerZone::vpTemplateTrackerZone() : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1) {}
50 
55  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
56 {
57  *this = z;
58 }
59 
64 {
65  min_x = -1;
66  min_y = -1;
67  max_x = -1;
68  max_y = -1;
69 
70  Zone.clear();
71 }
72 
77 {
78  clear();
79 
80  this->copy(z);
81  return (*this);
82 }
83 
107 {
108  Zone.clear();
109 
110  std::vector<vpImagePoint> vip;
111 
112  bool end = false;
113 
114  do {
115  vpImagePoint p;
117  if (vpDisplay::getClick(I, p, button, false)) {
118  vip.push_back(p);
119 
121 
122  if (vip.size() > 1) {
123  if (delaunay) {
124  // Draw a line between the 2 last points
125  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
126  } else {
127  if (vip.size() % 3 == 2)
128  // draw line between point 2-1
129  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
130  else if (vip.size() % 3 == 0) {
131  // draw line between point 3-2
132  vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
133  // draw line between point 3-1
134  vpDisplay::displayLine(I, p, vip[vip.size() - 3], vpColor::blue, 3);
135  }
136  }
137  }
138 
139  if (button == vpMouseButton::button3)
140  end = true;
141  }
142 
143  vpTime::wait(20);
144  vpDisplay::flush(I);
145  } while (!end);
146 
147  initFromPoints(I, vip, delaunay);
148 }
149 
163 void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
164  bool delaunay)
165 {
166  if (delaunay) {
167  if (vip.size() == 3) {
168  initFromPoints(I, vip, false);
169  } else if (vip.size() == 4) {
170  std::vector<vpImagePoint> vip_delaunay;
171  vip_delaunay.push_back(vip[0]);
172  vip_delaunay.push_back(vip[1]);
173  vip_delaunay.push_back(vip[2]);
174  vip_delaunay.push_back(vip[2]);
175  vip_delaunay.push_back(vip[3]);
176  vip_delaunay.push_back(vip[0]);
177  initFromPoints(I, vip_delaunay, false);
178  } else {
179 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
180  // Init Delaunay
181  cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
182  for (size_t i = 0; i < vip.size(); i++) {
183  cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
184  // std::cout << "Click point: " << vip[i] << std::endl;
185  subdiv.insert(fp);
186  }
187 
188  // Compute Delaunay triangulation
189  std::vector<cv::Vec6f> triangleList;
190  subdiv.getTriangleList(triangleList);
191 
192  // Keep only the Delaunay points that are inside the area
193  vpRect rect(0, 0, I.getWidth(), I.getHeight());
194 
195  std::vector<vpImagePoint> vip_delaunay;
196  for (size_t i = 0; i < triangleList.size(); i++) {
197  cv::Vec6f t = triangleList[i];
198  std::vector<vpImagePoint> p(3);
199 
200  p[0].set_uv(t[0], t[1]);
201  p[1].set_uv(t[2], t[3]);
202  p[2].set_uv(t[4], t[5]);
203 
204  if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
205  vip_delaunay.push_back(p[0]);
206  vip_delaunay.push_back(p[1]);
207  vip_delaunay.push_back(p[2]);
208  }
209  }
210 
211  initFromPoints(I, vip_delaunay, false);
212 #else
213  throw vpException(vpException::functionNotImplementedError, "Delaunay triangulation is not available!");
214 #endif
215  }
216  } else {
217  Zone.clear();
218  for (unsigned int i = 0; i < vip.size(); i += 3) {
219  vpTemplateTrackerTriangle triangle(vip[i], vip[i + 1], vip[i + 2]);
220  add(triangle);
221 
222  // vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green,
223  // 1); vpDisplay::displayLine(I, vip[i+1], vip[i+2],
224  // vpColor::green, 1); vpDisplay::displayLine(I, vip[i+2], vip[i],
225  // vpColor::green,1); vpDisplay::flush(I) ;
226 
227  // Update the bounding box
228  if ((triangle.getMinx() < min_x) || (min_x == -1))
229  min_x = (int)triangle.getMinx();
230  if ((triangle.getMaxx() > max_x) || (max_x == -1))
231  max_x = (int)triangle.getMaxx();
232  if ((triangle.getMiny() < min_y) || (min_y == -1))
233  min_y = (int)triangle.getMiny();
234  if ((triangle.getMaxy() > max_y) || (max_y == -1))
235  max_y = (int)triangle.getMaxy();
236  }
237  }
238 }
239 
245 {
246  Zone.push_back(t);
247 
248  // Update the bounding box
249  if ((t.getMinx() < min_x) || (min_x == -1))
250  min_x = (int)t.getMinx();
251  if ((t.getMaxx() > max_x) || (max_x == -1))
252  max_x = (int)t.getMaxx();
253  if ((t.getMiny() < min_y) || (min_y == -1))
254  min_y = (int)t.getMiny();
255  if ((t.getMaxy() > max_y) || (max_y == -1))
256  max_y = (int)t.getMaxy();
257 }
258 
265 bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
266 {
267  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
268  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
269  if (Iterateurvecteur->inTriangle(i, j))
270  return true;
271  }
272  return false;
273 }
274 
281 bool vpTemplateTrackerZone::inZone(const double &i, const double &j) const
282 {
283  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
284  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
285  if (Iterateurvecteur->inTriangle(i, j))
286  return true;
287  }
288  return false;
289 }
290 
298 bool vpTemplateTrackerZone::inZone(const int &i, const int &j, unsigned int &id_triangle) const
299 {
300  unsigned int id = 0;
301  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
302  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
303  if (Iterateurvecteur->inTriangle(i, j)) {
304  id_triangle = id;
305  return true;
306  }
307  id++;
308  }
309  return false;
310 }
311 
319 bool vpTemplateTrackerZone::inZone(const double &i, const double &j, unsigned int &id_triangle) const
320 {
321  unsigned int id = 0;
322  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
323  for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
324  if (Iterateurvecteur->inTriangle(i, j)) {
325  id_triangle = id;
326  return true;
327  }
328  id++;
329  }
330  return false;
331 }
332 
352 {
353  if (i > getNbTriangle() - 1)
354  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
355 
356  T = Zone[i];
357 }
373 {
374  if (i > getNbTriangle() - 1)
375  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
376 
377  return Zone[i];
378 }
384 {
385  double xc = 0;
386  double yc = 0;
387  int cpt = 0;
388  for (int i = min_y; i < max_y; i++)
389  for (int j = min_x; j < max_x; j++)
390  if (inZone(i, j)) {
391  xc += j;
392  yc += i;
393  cpt++;
394  }
395  if (!cpt) {
396  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
397  }
398  xc = xc / cpt;
399  yc = yc / cpt;
400  vpImagePoint ip;
401  ip.set_uv(xc, yc);
402  return ip;
403 }
404 
409 int vpTemplateTrackerZone::getMaxx() const { return max_x; }
414 int vpTemplateTrackerZone::getMaxy() const { return max_y; }
419 int vpTemplateTrackerZone::getMinx() const { return min_x; }
424 int vpTemplateTrackerZone::getMiny() const { return min_y; }
425 
431 {
432  vpRect bbox;
435  return bbox;
436 }
437 
443 void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
444 {
445  std::vector<vpImagePoint> ip;
446  for (unsigned int i = 0; i < Zone.size(); i++) {
447  vpTemplateTrackerTriangle triangle;
448  Zone[i].getCorners(ip);
449  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
450  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
451  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
452  }
453 }
454 
460 void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, unsigned int thickness)
461 {
462  std::vector<vpImagePoint> ip;
463  for (unsigned int i = 0; i < Zone.size(); i++) {
464  vpTemplateTrackerTriangle triangle;
465  Zone[i].getCorners(ip);
466  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
467  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
468  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
469  }
470 }
471 
476 
484 void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
485 {
486  assert(id < getNbTriangle());
487  vpTemplateTrackerTriangle triangle;
488  getTriangle(id, triangle);
489  for (int i = 0; i < (int)I.getHeight(); i++) {
490  for (int j = 0; j < (int)I.getWidth(); j++) {
491  if (triangle.inTriangle(i, j)) {
492  I[i][j] = gray_level;
493  }
494  }
495  }
496 }
497 
502 {
503  vpTemplateTrackerZone tempZone;
505  vpTemplateTrackerTriangle TtempDown;
506  for (unsigned int i = 0; i < getNbTriangle(); i++) {
507  getTriangle(i, Ttemp);
508  TtempDown = Ttemp.getPyramidDown();
509  tempZone.add(TtempDown);
510  }
511  return tempZone;
512 }
513 
520 {
521  vpTemplateTrackerTriangle triangle;
522  for (unsigned int i = 0; i < z.getNbTriangle(); i++) {
523  z.getTriangle(i, triangle);
524  add(triangle);
525  // Update the bounding box
526  if ((triangle.getMinx() < min_x) || (min_x == -1))
527  min_x = (int)triangle.getMinx();
528  if ((triangle.getMaxx() > max_x) || (max_x == -1))
529  max_x = (int)triangle.getMaxx();
530  if ((triangle.getMiny() < min_y) || (min_y == -1))
531  min_y = (int)triangle.getMiny();
532  if ((triangle.getMaxy() > max_y) || (max_y == -1))
533  max_y = (int)triangle.getMaxy();
534  }
535 }
536 
544 vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
545 {
546  int cpt_pt = 0;
547  double x_center = 0, y_center = 0;
548  for (int j = 0; j < borne_x; j++)
549  for (int i = 0; i < borne_y; i++)
550  if (inZone(i, j)) {
551  x_center += j;
552  y_center += i;
553  cpt_pt++;
554  }
555 
556  if (!cpt_pt) {
557  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
558  }
559 
560  x_center = x_center / cpt_pt;
561  y_center = y_center / cpt_pt;
562  vpImagePoint center;
563  center.set_uv(x_center, y_center);
564  return center;
565 }
566 
571 {
572  double area = 0;
573  vpTemplateTrackerTriangle triangle;
574  for (unsigned int i = 0; i < getNbTriangle(); i++) {
575  getTriangle(i, triangle);
576  area += triangle.getArea();
577  }
578  return area;
579 }
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor blue
Definition: vpColor.h:217
static 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 emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
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_uv(double u, double v)
Definition: vpImagePoint.h:353
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
Defines a rectangle in the plane.
Definition: vpRect.h:76
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:293
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:363
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)