Visual Servoing Platform  version 3.0.0
vpTemplateTrackerZone.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Template tracker.
32  *
33  * Authors:
34  * Amaury Dame
35  * Aurelien Yol
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <limits> // numeric_limits
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
45 #include <opencv2/imgproc/imgproc.hpp>
46 #endif
47 
48 #include <visp3/tt/vpTemplateTrackerZone.h>
49 
50 
55  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
56 {
57 }
58 
63  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
64 {
65  *this = z;
66 }
67 
72 {
73  min_x=-1;
74  min_y=-1;
75  max_x=-1;
76  max_y=-1;
77 
78  Zone.clear();
79 }
80 
85 {
86  clear();
87 
88  this->copy(z);
89  return (*this);
90 }
91 
113 {
114  Zone.clear();
115 
116  std::vector<vpImagePoint> vip;
117 
118  bool end = false;
119 
120  do {
121  vpImagePoint p;
123  if (vpDisplay::getClick(I, p, button, false) ) {
124  vip.push_back(p);
125 
127 
128  if (vip.size() > 1) {
129  if (delaunay) {
130  // Draw a line between the 2 last points
131  vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
132  }
133  else {
134  if(vip.size() % 3 ==2)
135  // draw line between point 2-1
136  vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
137  else if(vip.size() % 3 ==0) {
138  // draw line between point 3-2
139  vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
140  // draw line between point 3-1
141  vpDisplay::displayLine(I, p, vip[vip.size()-3], vpColor::blue, 3);
142  }
143 
144  }
145  }
146 
147  if (button == vpMouseButton::button3)
148  end = true;
149  }
150 
151  vpTime::wait(20);
152  vpDisplay::flush(I);
153  } while(!end);
154 
155  initFromPoints(I, vip, delaunay);
156 }
157 
169 void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &vip, bool delaunay)
170 {
171  if (delaunay) {
172  if(vip.size() == 3) {
173  initFromPoints(I, vip, false);
174  }
175  else if(vip.size() == 4) {
176  std::vector<vpImagePoint> vip_delaunay;
177  vip_delaunay.push_back(vip[0]);
178  vip_delaunay.push_back(vip[1]);
179  vip_delaunay.push_back(vip[2]);
180  vip_delaunay.push_back(vip[2]);
181  vip_delaunay.push_back(vip[3]);
182  vip_delaunay.push_back(vip[0]);
183  initFromPoints(I, vip_delaunay, false);
184  }
185  else {
186 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
187  // Init Delaunay
188  cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
189  for(size_t i=0; i< vip.size(); i++) {
190  cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
191  //std::cout << "Click point: " << vip[i] << std::endl;
192  subdiv.insert(fp);
193  }
194 
195  // Compute Delaunay triangulation
196  std::vector<cv::Vec6f> triangleList;
197  subdiv.getTriangleList(triangleList);
198 
199  // Keep only the Delaunay points that are inside the area
200  vpRect rect(0, 0, I.getWidth(), I.getHeight());
201 
202  std::vector<vpImagePoint> vip_delaunay;
203  for( size_t i = 0; i < triangleList.size(); i++ ) {
204  cv::Vec6f t = triangleList[i];
205  std::vector<vpImagePoint> p(3);
206 
207  p[0].set_uv(t[0], t[1]);
208  p[1].set_uv(t[2], t[3]);
209  p[2].set_uv(t[4], t[5]);
210 
211  if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
212  vip_delaunay.push_back(p[0]);
213  vip_delaunay.push_back(p[1]);
214  vip_delaunay.push_back(p[2]);
215  }
216  }
217 
218  initFromPoints(I, vip_delaunay, false);
219 #else
220  throw vpException(vpException::functionNotImplementedError,"Delaunay triangulation is not available!");
221 #endif
222  }
223  }
224  else {
225  Zone.clear();
226  for(unsigned int i=0; i<vip.size(); i+=3) {
227  vpTemplateTrackerTriangle triangle(vip[i], vip[i+1], vip[i+2]);
228  add(triangle);
229 
230 // vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green, 1);
231 // vpDisplay::displayLine(I, vip[i+1], vip[i+2], vpColor::green, 1);
232 // vpDisplay::displayLine(I, vip[i+2], vip[i], vpColor::green,1);
233 // vpDisplay::flush(I) ;
234 
235  // Update the bounding box
236  if((triangle.getMinx()<min_x)||(min_x==-1))
237  min_x=(int)triangle.getMinx();
238  if((triangle.getMaxx()>max_x)||(max_x==-1))
239  max_x=(int)triangle.getMaxx();
240  if((triangle.getMiny()<min_y)||(min_y==-1))
241  min_y=(int)triangle.getMiny();
242  if((triangle.getMaxy()>max_y)||(max_y==-1))
243  max_y=(int)triangle.getMaxy();
244  }
245  }
246 }
247 
253 {
254  Zone.push_back(t);
255 
256  // Update the bounding box
257  if((t.getMinx()<min_x)||(min_x==-1))
258  min_x=(int)t.getMinx();
259  if((t.getMaxx()>max_x)||(max_x==-1))
260  max_x=(int)t.getMaxx();
261  if((t.getMiny()<min_y)||(min_y==-1))
262  min_y=(int)t.getMiny();
263  if((t.getMaxy()>max_y)||(max_y==-1))
264  max_y=(int)t.getMaxy();
265 }
266 
272 bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
273 {
274  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
275  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
276  {
277  if(Iterateurvecteur->inTriangle(i,j))
278  return true;
279  }
280  return false;
281 }
282 
288 bool vpTemplateTrackerZone::inZone(const double &i,const double &j) const
289 {
290  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
291  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
292  {
293  if(Iterateurvecteur->inTriangle(i,j))
294  return true;
295  }
296  return false;
297 }
298 
306 bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_triangle) const
307 {
308  unsigned int id=0;
309  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
310  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
311  {
312  if(Iterateurvecteur->inTriangle(i,j))
313  {
314  id_triangle=id;
315  return true;
316  }
317  id++;
318  }
319  return false;
320 }
321 
329 bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int &id_triangle) const
330 {
331  unsigned int id=0;
332  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
333  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
334  {
335  if(Iterateurvecteur->inTriangle(i,j))
336  {
337  id_triangle=id;
338  return true;
339  }
340  id++;
341  }
342  return false;
343 }
344 
362 {
363  if (i > getNbTriangle()-1)
364  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
365 
366  T = Zone[i];
367 }
383 {
384  if (i > getNbTriangle()-1)
385  throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
386 
387  return Zone[i];
388 }
394 {
395  double xc=0;
396  double yc=0;
397  int cpt=0;
398  for(int i=min_y;i<max_y;i++)
399  for(int j=min_x;j<max_x;j++)
400  if(inZone(i,j))
401  {
402  xc+=j;
403  yc+=i;
404  cpt ++;
405  }
406  if(! cpt) {
408  "Cannot compute the zone center: size = 0")) ;
409  }
410  xc=xc/cpt;
411  yc=yc/cpt;
412  vpImagePoint ip;
413  ip.set_uv(xc, yc);
414  return ip;
415 }
416 
422 {
423  return max_x;
424 }
430 {
431  return max_y;
432 }
438 {
439  return min_x;
440 }
446 {
447  return min_y;
448 }
449 
455 {
456  vpRect bbox;
459  return bbox;
460 }
461 
468 void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
469 {
470  std::vector<vpImagePoint> ip;
471  for (unsigned int i=0; i < Zone.size(); i++) {
472  vpTemplateTrackerTriangle triangle;
473  Zone[i].getCorners(ip);
474  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
475  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
476  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
477  }
478 }
479 
486 void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
487 {
488  std::vector<vpImagePoint> ip;
489  for (unsigned int i=0; i < Zone.size(); i++) {
490  vpTemplateTrackerTriangle triangle;
491  Zone[i].getCorners(ip);
492  vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
493  vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
494  vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
495  }
496 }
497 
502 {
503  clear();
504 }
505 
513 void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char>& I, unsigned int id,unsigned char gray_level)
514 {
515  assert(id < getNbTriangle());
516  vpTemplateTrackerTriangle triangle;
517  getTriangle(id, triangle);
518  for (int i=0 ; i < (int) I.getHeight() ; i++)
519  {
520  for (int j=0 ; j < (int) I.getWidth() ; j++)
521  {
522  if(triangle.inTriangle(i,j))
523  {
524  I[i][j]=gray_level;
525  }
526  }
527  }
528 }
529 
534 {
535  vpTemplateTrackerZone tempZone;
537  vpTemplateTrackerTriangle TtempDown;
538  for(unsigned int i=0;i<getNbTriangle();i++)
539  {
540  getTriangle(i,Ttemp);
541  TtempDown=Ttemp.getPyramidDown();
542  tempZone.add(TtempDown);
543  }
544  return tempZone;
545 }
546 
553 {
554  vpTemplateTrackerTriangle triangle;
555  for(unsigned int i=0;i<z.getNbTriangle();i++)
556  {
557  z.getTriangle(i, triangle);
558  add(triangle);
559  // Update the bounding box
560  if((triangle.getMinx()<min_x)||(min_x==-1))
561  min_x=(int)triangle.getMinx();
562  if((triangle.getMaxx()>max_x)||(max_x==-1))
563  max_x=(int)triangle.getMaxx();
564  if((triangle.getMiny()<min_y)||(min_y==-1))
565  min_y=(int)triangle.getMiny();
566  if((triangle.getMaxy()>max_y)||(max_y==-1))
567  max_y=(int)triangle.getMaxy();
568  }
569 }
570 
578 vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
579 {
580  int cpt_pt=0;
581  double x_center=0,y_center=0;
582  for(int j=0;j<borne_x;j++)
583  for(int i=0;i<borne_y;i++)
584  if(inZone(i,j))
585  {
586  x_center+=j;
587  y_center+=i;
588  cpt_pt++;
589  }
590 
591  if(! cpt_pt) {
593  "Cannot compute the zone center: size = 0")) ;
594  }
595 
596  x_center=x_center/cpt_pt;
597  y_center=y_center/cpt_pt;
598  vpImagePoint center;
599  center.set_uv(x_center, y_center);
600  return center;
601 }
602 
607 {
608  double area = 0;
609  vpTemplateTrackerTriangle triangle;
610  for (unsigned int i=0; i < getNbTriangle(); i++) {
611  getTriangle(i, triangle);
612  area += triangle.getArea();
613  }
614  return area;
615 }
616 
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
std::vector< vpTemplateTrackerTriangle > Zone
Vector of triangles that defines the zone.
vpTemplateTrackerTriangle getPyramidDown() const
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getWidth() const
Definition: vpImage.h:161
int max_y
Bounding box parameter.
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
int min_x
Bounding box parameter.
error that can be emited by ViSP classes.
Definition: vpException.h:73
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)
vpImagePoint getCenter() const
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
void copy(const vpTemplateTrackerZone &z)
static const vpColor red
Definition: vpColor.h:163
void add(const vpTemplateTrackerTriangle &t)
unsigned int getNbTriangle() const
vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z)
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
bool inTriangle(const vpImagePoint &ip) const
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:285
bool inZone(const int &i, const int &j) const
int max_x
Bounding box parameter.
vpTemplateTrackerZone getPyramidDown() const
unsigned int getHeight() const
Definition: vpImage.h:152
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:234
Defines a rectangle in the plane.
Definition: vpRect.h:81
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void setBottomRight(const vpImagePoint &bottomRight)
Definition: vpRect.h:221
void fillTriangle(vpImage< unsigned char > &I, unsigned int id, unsigned char gray_level)
static const vpColor blue
Definition: vpColor.h:169