Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
49vpTemplateTrackerZone::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);
145 } while (!end);
146
147 initFromPoints(I, vip, delaunay);
148}
149
163void 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
265bool 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
281bool 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
298bool 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
319bool 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
425
431{
432 vpRect bbox;
435 return bbox;
436}
437
443void 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++) {
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
460void 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++) {
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
484void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
485{
486 assert(id < getNbTriangle());
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;
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{
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
544vpImagePoint 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;
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 ...
void set_uv(double u, double v)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
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
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) 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)