Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpTemplateTrackerTriangle.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  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  *
38 *****************************************************************************/
39 #include <visp3/tt/vpTemplateTrackerTriangle.h>
40 
45  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
46  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
47 {
48 }
49 
54  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
55  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
56 {
57  *this = T;
58 }
59 
64 {
65  minx_temp = T.minx_temp;
66  miny_temp = T.miny_temp;
67 
68  l_t = T.l_t;
69  h_t = T.h_t;
70  C1.x = T.C1.x;
71  C1.y = T.C1.y;
72  C2.x = T.C2.x;
73  C2.y = T.C2.y;
74  C3.x = T.C3.x;
75  C3.y = T.C3.y;
76  // uvinv.resize(2,2);
77  // uvinv=T.uvinv;
78  // p_ds_uv.resize(2);
79  // p_ds_uv=T.p_ds_uv;
80  // ptempo.resize(2);
81  // ptempo=T.ptempo;
82  not_good = T.not_good;
83 
84  uvinv00 = T.uvinv00;
85  uvinv01 = T.uvinv01;
86  uvinv10 = T.uvinv10;
87  uvinv11 = T.uvinv11;
88 
90  area = T.area;
91 
92  return (*this);
93 }
94 
105  const vpColVector &c3)
106  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
107  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
108 {
109  init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
110 }
115 {
117  Ttemp.init(C1.x / 2., C1.y / 2., C2.x / 2., C2.y / 2., C3.x / 2., C3.y / 2.);
118  return Ttemp;
119 }
120 
126 vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
127  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
128  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
129 {
130  init(x1, y1, x2, y2, x3, y3);
131 }
132 
140  const vpImagePoint &c3)
141  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
142  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
143 {
144  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
145 }
146 
152 vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3)
153  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
154  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
155 {
156  init(x1, y1, x2, y2, x3, y3);
157 }
168 {
169  init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
170 }
178 {
179  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
180 }
181 
188 void vpTemplateTrackerTriangle::init(int x1, int y1, int x2, int y2, int x3, int y3)
189 {
190  init((double)x1, (double)y1, (double)x2, (double)y2, (double)x3, (double)y3);
191 }
192 
199 void vpTemplateTrackerTriangle::init(double x1, double y1, double x2, double y2, double x3, double y3)
200 {
201  C1.x = x1;
202  C1.y = y1;
203  C2.x = x2;
204  C2.y = y2;
205  C3.x = x3;
206  C3.y = y3;
207 
208  double minx, miny, maxx, maxy;
209  // calcul du rectangle minimal contenant le triangle seletionne
210  minx = (x1 < x2) ? x1 : x2;
211  miny = (y1 < y2) ? y1 : y2;
212  minx = (minx < x3) ? minx : x3;
213  miny = (miny < y3) ? miny : y3;
214  maxx = (x1 > x2) ? x1 : x2;
215  maxy = (y1 > y2) ? y1 : y2;
216  maxx = (maxx > x3) ? maxx : x3;
217  maxy = (maxy > y3) ? maxy : y3;
218 
219  vpColVector u;
220  vpColVector v;
221  u.resize(2);
222  v.resize(2);
223  vpMatrix uv(2, 2);
224  vpMatrix uvinv(2, 2);
225 
226  u[0] = C2.x - C1.x;
227  u[1] = C2.y - C1.y;
228 
229  v[0] = C3.x - C1.x;
230  v[1] = C3.y - C1.y;
231 
232  uv[0][0] = u[0];
233  uv[1][0] = v[0];
234  uv[0][1] = u[1];
235  uv[1][1] = v[1];
236  try {
237  uvinv = uv.inverseByLU();
238  not_good = false;
239  } catch (...) {
240  not_good = true;
241  std::cout << "Triangle vide" << std::endl;
242  }
243  uvinv00 = uvinv[0][0];
244  uvinv01 = uvinv[0][1];
245  uvinv10 = uvinv[1][0];
246  uvinv11 = uvinv[1][1];
247 
248  l_t = maxx - minx;
249  h_t = maxy - miny;
250  minx_temp = minx;
251  miny_temp = miny;
252 
253  marge_triangle = 0.00001;
254  area = 0.5 * fabs(uv.det());
255 }
256 
257 // marge ajoutee a zone pour que sommet soit pris en compte
258 
264 bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
265 {
266  if (not_good)
267  return false;
268 
269  /*ptempo[0]=j-C1.x;
270  ptempo[1]=i-C1.y;
271 
272  p_ds_uv=ptempo*uvinv;
273  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
274 
275  double ptempo0 = j - C1.x;
276  double ptempo1 = i - C1.y;
277  double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
278  double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
279  return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
280 }
281 
287 bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
288 {
289  if (not_good)
290  return false;
291  /*ptempo[0]=j-C1.x;
292  ptempo[1]=i-C1.y;
293 
294  p_ds_uv=ptempo*uvinv;
295  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
296  double ptempo0 = j - C1.x;
297  double ptempo1 = i - C1.y;
298  double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
299  double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
300  return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
301 }
302 
307 bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const { return inTriangle(ip.get_i(), ip.get_j()); }
315 {
316  c1.set_uv(C1.x, C1.y);
317  c2.set_uv(C2.x, C2.y);
318  c3.set_uv(C3.x, C3.y);
319 }
320 
326 void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
327 {
328  c.resize(3);
329  c[0].set_uv(C1.x, C1.y);
330  c[1].set_uv(C2.x, C2.y);
331  c[2].set_uv(C3.x, C3.y);
332 }
333 
340 {
341  c1 = getCorner1();
342  c2 = getCorner2();
343  c3 = getCorner3();
344 }
345 
351 vpColVector vpTemplateTrackerTriangle::getCorner1() const
352 {
353  vpColVector c(2);
354  c[0] = C1.x;
355  c[1] = C1.y;
356 
357  return c;
358 }
364 vpColVector vpTemplateTrackerTriangle::getCorner2() const
365 {
366  vpColVector c(2);
367  c[0] = C2.x;
368  c[1] = C2.y;
369  return c;
370 }
371 
377 vpColVector vpTemplateTrackerTriangle::getCorner3() const
378 {
379  vpColVector c(2);
380  c[0] = C3.x;
381  c[1] = C3.y;
382  return c;
383 }
384 
390 void vpTemplateTrackerTriangle::getSize(double &w, double &h) const
391 {
392  w = l_t;
393  h = h_t;
394 }
400 void vpTemplateTrackerTriangle::getSize(int &w, int &h) const
401 {
402  w = (int)l_t + 1;
403  h = (int)h_t + 1;
404 }
405 
410 double vpTemplateTrackerTriangle::getMinx() const { return minx_temp - 1; }
415 double vpTemplateTrackerTriangle::getMiny() const { return miny_temp - 1; }
420 double vpTemplateTrackerTriangle::getMaxx() const { return minx_temp + l_t + 1; }
425 double vpTemplateTrackerTriangle::getMaxy() const { return miny_temp + h_t + 1; }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_u() const
Definition: vpImagePoint.h:136
void set_uv(double u, double v)
Definition: vpImagePoint.h:352
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix inverseByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6199
vpTemplateTrackerDPoint C3
Corner 2.
bool inTriangle(const vpImagePoint &ip) const
void getSize(double &w, double &h) const
vpTemplateTrackerDPoint C2
Corner 1.
vpTemplateTrackerTriangle getPyramidDown() const
void init(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3)
vpTemplateTrackerTriangle & operator=(const vpTemplateTrackerTriangle &T)
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const