Visual Servoing Platform  version 3.1.0
vpTemplateTrackerTriangle.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 #include <visp3/tt/vpTemplateTrackerTriangle.h>
41 
46  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
47  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
48 {
49 }
50 
55  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
56  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
57 {
58  *this = T;
59 }
60 
65 {
66  minx_temp = T.minx_temp;
67  miny_temp = T.miny_temp;
68 
69  l_t = T.l_t;
70  h_t = T.h_t;
71  C1.x = T.C1.x;
72  C1.y = T.C1.y;
73  C2.x = T.C2.x;
74  C2.y = T.C2.y;
75  C3.x = T.C3.x;
76  C3.y = T.C3.y;
77  // uvinv.resize(2,2);
78  // uvinv=T.uvinv;
79  // p_ds_uv.resize(2);
80  // p_ds_uv=T.p_ds_uv;
81  // ptempo.resize(2);
82  // ptempo=T.ptempo;
83  not_good = T.not_good;
84 
85  uvinv00 = T.uvinv00;
86  uvinv01 = T.uvinv01;
87  uvinv10 = T.uvinv10;
88  uvinv11 = T.uvinv11;
89 
91  area = T.area;
92 
93  return (*this);
94 }
95 
106  const vpColVector &c3)
107  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
108  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
109 {
110  init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
111 }
116 {
118  Ttemp.init(C1.x / 2., C1.y / 2., C2.x / 2., C2.y / 2., C3.x / 2., C3.y / 2.);
119  return Ttemp;
120 }
121 
127 vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3)
128  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
129  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
130 {
131  init(x1, y1, x2, y2, x3, y3);
132 }
133 
141  const vpImagePoint &c3)
142  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
143  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
144 {
145  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
146 }
147 
153 vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3)
154  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
155  uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
156 {
157  init(x1, y1, x2, y2, x3, y3);
158 }
169 {
170  init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
171 }
179 {
180  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
181 }
182 
189 void vpTemplateTrackerTriangle::init(int x1, int y1, int x2, int y2, int x3, int y3)
190 {
191  init((double)x1, (double)y1, (double)x2, (double)y2, (double)x3, (double)y3);
192 }
193 
200 void vpTemplateTrackerTriangle::init(double x1, double y1, double x2, double y2, double x3, double y3)
201 {
202  C1.x = x1;
203  C1.y = y1;
204  C2.x = x2;
205  C2.y = y2;
206  C3.x = x3;
207  C3.y = y3;
208 
209  double minx, miny, maxx, maxy;
210  // calcul du rectangle minimal contenant le triangle seletionne
211  minx = (x1 < x2) ? x1 : x2;
212  miny = (y1 < y2) ? y1 : y2;
213  minx = (minx < x3) ? minx : x3;
214  miny = (miny < y3) ? miny : y3;
215  maxx = (x1 > x2) ? x1 : x2;
216  maxy = (y1 > y2) ? y1 : y2;
217  maxx = (maxx > x3) ? maxx : x3;
218  maxy = (maxy > y3) ? maxy : y3;
219 
220  vpColVector u;
221  vpColVector v;
222  u.resize(2);
223  v.resize(2);
224  vpMatrix uv(2, 2);
225  vpMatrix uvinv(2, 2);
226 
227  u[0] = C2.x - C1.x;
228  u[1] = C2.y - C1.y;
229 
230  v[0] = C3.x - C1.x;
231  v[1] = C3.y - C1.y;
232 
233  uv[0][0] = u[0];
234  uv[1][0] = v[0];
235  uv[0][1] = u[1];
236  uv[1][1] = v[1];
237  try {
238  uvinv = uv.inverseByLU();
239  not_good = false;
240  } catch (...) {
241  not_good = true;
242  std::cout << "Triangle vide" << std::endl;
243  }
244  uvinv00 = uvinv[0][0];
245  uvinv01 = uvinv[0][1];
246  uvinv10 = uvinv[1][0];
247  uvinv11 = uvinv[1][1];
248 
249  l_t = maxx - minx;
250  h_t = maxy - miny;
251  minx_temp = minx;
252  miny_temp = miny;
253 
254  marge_triangle = 0.00001;
255  area = 0.5 * fabs(uv.det());
256 }
257 
258 // marge ajoutee a zone pour que sommet soit pris en compte
259 
265 bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
266 {
267  if (not_good)
268  return false;
269 
270  /*ptempo[0]=j-C1.x;
271  ptempo[1]=i-C1.y;
272 
273  p_ds_uv=ptempo*uvinv;
274  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
275 
276  double ptempo0 = j - C1.x;
277  double ptempo1 = i - C1.y;
278  double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
279  double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
280  return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
281 }
282 
288 bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
289 {
290  if (not_good)
291  return false;
292  /*ptempo[0]=j-C1.x;
293  ptempo[1]=i-C1.y;
294 
295  p_ds_uv=ptempo*uvinv;
296  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
297  double ptempo0 = j - C1.x;
298  double ptempo1 = i - C1.y;
299  double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
300  double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
301  return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
302 }
303 
308 bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const { return inTriangle(ip.get_i(), ip.get_j()); }
316 {
317  c1.set_uv(C1.x, C1.y);
318  c2.set_uv(C2.x, C2.y);
319  c3.set_uv(C3.x, C3.y);
320 }
321 
327 void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
328 {
329  c.resize(3);
330  c[0].set_uv(C1.x, C1.y);
331  c[1].set_uv(C2.x, C2.y);
332  c[2].set_uv(C3.x, C3.y);
333 }
334 
341 {
342  c1 = getCorner1();
343  c2 = getCorner2();
344  c3 = getCorner3();
345 }
346 
352 vpColVector vpTemplateTrackerTriangle::getCorner1() const
353 {
354  vpColVector c(2);
355  c[0] = C1.x;
356  c[1] = C1.y;
357 
358  return c;
359 }
365 vpColVector vpTemplateTrackerTriangle::getCorner2() const
366 {
367  vpColVector c(2);
368  c[0] = C2.x;
369  c[1] = C2.y;
370  return c;
371 }
372 
378 vpColVector vpTemplateTrackerTriangle::getCorner3() const
379 {
380  vpColVector c(2);
381  c[0] = C3.x;
382  c[1] = C3.y;
383  return c;
384 }
385 
391 void vpTemplateTrackerTriangle::getSize(double &w, double &h) const
392 {
393  w = l_t;
394  h = h_t;
395 }
401 void vpTemplateTrackerTriangle::getSize(int &w, int &h) const
402 {
403  w = (int)l_t + 1;
404  h = (int)h_t + 1;
405 }
406 
411 double vpTemplateTrackerTriangle::getMinx() const { return minx_temp - 1; }
416 double vpTemplateTrackerTriangle::getMiny() const { return miny_temp - 1; }
421 double vpTemplateTrackerTriangle::getMaxx() const { return minx_temp + l_t + 1; }
426 double vpTemplateTrackerTriangle::getMaxy() const { return miny_temp + h_t + 1; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
double get_i() const
Definition: vpImagePoint.h:204
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:4848
vpMatrix inverseByLU() const
vpTemplateTrackerDPoint C2
Corner 1.
double get_u() const
Definition: vpImagePoint.h:263
void getSize(double &w, double &h) const
double get_j() const
Definition: vpImagePoint.h:215
vpTemplateTrackerDPoint C3
Corner 2.
vpTemplateTrackerTriangle & operator=(const vpTemplateTrackerTriangle &T)
void init(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:248
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
double get_v() const
Definition: vpImagePoint.h:274
vpTemplateTrackerTriangle getPyramidDown() const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:241