Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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 
355 {
356  if (i > getNbTriangle() - 1)
357  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
358 
359  T = Zone[i];
360 }
376 {
377  if (i > getNbTriangle() - 1)
378  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
379 
380  return Zone[i];
381 }
387 {
388  double xc = 0;
389  double yc = 0;
390  int cpt = 0;
391  for (int i = min_y; i < max_y; i++)
392  for (int j = min_x; j < max_x; j++)
393  if (inZone(i, j)) {
394  xc += j;
395  yc += i;
396  cpt++;
397  }
398  if (!cpt) {
399  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
400  }
401  xc = xc / cpt;
402  yc = yc / cpt;
403  vpImagePoint ip;
404  ip.set_uv(xc, yc);
405  return ip;
406 }
407 
412 int vpTemplateTrackerZone::getMaxx() const { return max_x; }
417 int vpTemplateTrackerZone::getMaxy() const { return max_y; }
422 int vpTemplateTrackerZone::getMinx() const { return min_x; }
427 int vpTemplateTrackerZone::getMiny() const { return min_y; }
428 
434 {
435  vpRect bbox;
438  return bbox;
439 }
440 
446 void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
447 {
448  std::vector<vpImagePoint> ip;
449  for (unsigned int i = 0; i < Zone.size(); i++) {
450  vpTemplateTrackerTriangle triangle;
451  Zone[i].getCorners(ip);
452  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
453  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
454  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
455  }
456 }
457 
463 void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, unsigned int thickness)
464 {
465  std::vector<vpImagePoint> ip;
466  for (unsigned int i = 0; i < Zone.size(); i++) {
467  vpTemplateTrackerTriangle triangle;
468  Zone[i].getCorners(ip);
469  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
470  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
471  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
472  }
473 }
474 
479 
487 void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
488 {
489  assert(id < getNbTriangle());
490  vpTemplateTrackerTriangle triangle;
491  getTriangle(id, triangle);
492  for (int i = 0; i < (int)I.getHeight(); i++) {
493  for (int j = 0; j < (int)I.getWidth(); j++) {
494  if (triangle.inTriangle(i, j)) {
495  I[i][j] = gray_level;
496  }
497  }
498  }
499 }
500 
505 {
506  vpTemplateTrackerZone tempZone;
508  vpTemplateTrackerTriangle TtempDown;
509  for (unsigned int i = 0; i < getNbTriangle(); i++) {
510  getTriangle(i, Ttemp);
511  TtempDown = Ttemp.getPyramidDown();
512  tempZone.add(TtempDown);
513  }
514  return tempZone;
515 }
516 
523 {
524  vpTemplateTrackerTriangle triangle;
525  for (unsigned int i = 0; i < z.getNbTriangle(); i++) {
526  z.getTriangle(i, triangle);
527  add(triangle);
528  // Update the bounding box
529  if ((triangle.getMinx() < min_x) || (min_x == -1))
530  min_x = (int)triangle.getMinx();
531  if ((triangle.getMaxx() > max_x) || (max_x == -1))
532  max_x = (int)triangle.getMaxx();
533  if ((triangle.getMiny() < min_y) || (min_y == -1))
534  min_y = (int)triangle.getMiny();
535  if ((triangle.getMaxy() > max_y) || (max_y == -1))
536  max_y = (int)triangle.getMaxy();
537  }
538 }
539 
547 vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
548 {
549  int cpt_pt = 0;
550  double x_center = 0, y_center = 0;
551  for (int j = 0; j < borne_x; j++)
552  for (int i = 0; i < borne_y; i++)
553  if (inZone(i, j)) {
554  x_center += j;
555  y_center += i;
556  cpt_pt++;
557  }
558 
559  if (!cpt_pt) {
560  throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
561  }
562 
563  x_center = x_center / cpt_pt;
564  y_center = y_center / cpt_pt;
565  vpImagePoint center;
566  center.set_uv(x_center, y_center);
567  return center;
568 }
569 
574 {
575  double area = 0;
576  vpTemplateTrackerTriangle triangle;
577  for (unsigned int i = 0; i < getNbTriangle(); i++) {
578  getTriangle(i, triangle);
579  area += triangle.getArea();
580  }
581  return area;
582 }
vpTemplateTrackerZone getPyramidDown() const
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, unsigned int thickness=3)
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
std::vector< vpTemplateTrackerTriangle > Zone
Vector of triangles that defines the zone.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
int max_y
Bounding box parameter.
void set_uv(double u, double v)
Definition: vpImagePoint.h:248
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
int min_x
Bounding box parameter.
error that can be emited by ViSP classes.
Definition: vpException.h:71
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
static void flush(const vpImage< unsigned char > &I)
void copy(const vpTemplateTrackerZone &z)
static const vpColor red
Definition: vpColor.h:179
void add(const vpTemplateTrackerTriangle &t)
bool inZone(const int &i, const int &j) const
vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z)
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &ip, bool delaunay=false)
int min_y
Bounding box parameter.
void setTopLeft(const vpImagePoint &topLeft)
Definition: vpRect.h:334
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int max_x
Bounding box parameter.
unsigned int getHeight() const
Definition: vpImage.h:186
unsigned int getNbTriangle() const
Defines a rectangle in the plane.
Definition: vpRect.h:78
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
bool inTriangle(const vpImagePoint &ip) const
unsigned int getWidth() const
Definition: vpImage.h:244
Function not implemented.
Definition: vpException.h:90
vpImagePoint getCenter() const
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:264
void fillTriangle(vpImage< unsigned char > &I, unsigned int id, unsigned char gray_level)
vpTemplateTrackerTriangle getPyramidDown() const
static const vpColor blue
Definition: vpColor.h:185