Visual Servoing Platform version 3.5.0
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
54vpTemplateTrackerZone::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);
150 } while (!end);
151
152 initFromPoints(I, vip, delaunay);
153}
154
168void 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
270bool 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
286bool 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
303bool 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
324bool 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
428
434{
435 vpRect bbox;
438 return bbox;
439}
440
446void 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++) {
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
463void 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++) {
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
487void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
488{
489 assert(id < getNbTriangle());
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;
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{
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
547vpImagePoint 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;
577 for (unsigned int i = 0; i < getNbTriangle(); i++) {
578 getTriangle(i, triangle);
579 area += triangle.getArea();
580 }
581 return area;
582}
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:88
void set_uv(double u, double v)
Definition: vpImagePoint.h:247
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)