Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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
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 #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),
46  uvinv00(0.), uvinv01(0.), 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),
55  uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
56 {
57  *this = T;
58 }
59 
64 {
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;
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 
104  const vpColVector &c2,
105  const vpColVector &c3)
106  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
107  uvinv00(0.), uvinv01(0.), 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),
128  uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
129 {
130  init(x1,y1,x2,y2,x3,y3);
131 }
132 
140  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
141  uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
142 {
143  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
144 }
145 
151 vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1,double y1, double x2,double y2, double x3,double y3)
152  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
153  uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
154 {
155  init(x1,y1,x2,y2,x3,y3);
156 }
166 {
167  init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
168 }
176 {
177  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
178 }
179 
185 void vpTemplateTrackerTriangle::init(int x1, int y1, int x2,int y2, int x3, int y3)
186 {
187  init((double)x1,(double)y1,(double)x2,(double)y2,(double)x3,(double)y3);
188 }
189 
195 void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2, double x3, double y3)
196 {
197  C1.x=x1;C1.y=y1;
198  C2.x=x2;C2.y=y2;
199  C3.x=x3;C3.y=y3;
200 
201  double minx,miny,maxx,maxy;
202  //calcul du rectangle minimal contenant le triangle seletionne
203  minx=(x1<x2)?x1:x2;
204  miny=(y1<y2)?y1:y2;
205  minx=(minx<x3)?minx:x3;
206  miny=(miny<y3)?miny:y3;
207  maxx=(x1>x2)?x1:x2;
208  maxy=(y1>y2)?y1:y2;
209  maxx=(maxx>x3)?maxx:x3;
210  maxy=(maxy>y3)?maxy:y3;
211 
212  vpColVector u;
213  vpColVector v;
214  u.resize(2);
215  v.resize(2);
216  vpMatrix uv(2,2);
217  vpMatrix uvinv(2,2);
218 
219  u[0]=C2.x-C1.x;
220  u[1]=C2.y-C1.y;
221 
222  v[0]=C3.x-C1.x;
223  v[1]=C3.y-C1.y;
224 
225  uv[0][0]=u[0];uv[1][0]=v[0];
226  uv[0][1]=u[1];uv[1][1]=v[1];
227  try
228  {
229  uvinv=uv.inverseByLU();
230  not_good=false;
231  }
232  catch(...)
233  {
234  not_good=true;
235  std::cout<<"Triangle vide"<<std::endl;
236 
237  }
238  uvinv00=uvinv[0][0];
239  uvinv01=uvinv[0][1];
240  uvinv10=uvinv[1][0];
241  uvinv11=uvinv[1][1];
242 
243  l_t=maxx-minx;
244  h_t=maxy-miny;
245  minx_temp=minx;
246  miny_temp=miny;
247 
248  marge_triangle = 0.00001;
249  area = 0.5 * fabs(uv.det());
250 }
251 
252 //marge ajoutee a zone pour que sommet soit pris en compte
253 
259 bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
260 {
261  if(not_good)
262  return false;
263 
264  /*ptempo[0]=j-C1.x;
265  ptempo[1]=i-C1.y;
266 
267  p_ds_uv=ptempo*uvinv;
268  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
269 
270  double ptempo0=j-C1.x;
271  double ptempo1=i-C1.y;
272  double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
273  double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
274  return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
275 }
276 
282 bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
283 {
284  if(not_good)
285  return false;
286  /*ptempo[0]=j-C1.x;
287  ptempo[1]=i-C1.y;
288 
289  p_ds_uv=ptempo*uvinv;
290  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
291  double ptempo0=j-C1.x;
292  double ptempo1=i-C1.y;
293  double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
294  double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
295  return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
296 }
297 
303 {
304  return inTriangle(ip.get_i(), ip.get_j());
305 }
313 {
314  c1.set_uv(C1.x, C1.y);
315  c2.set_uv(C2.x, C2.y);
316  c3.set_uv(C3.x, C3.y);
317 }
318 
323 void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
324 {
325  c.resize(3);
326  c[0].set_uv(C1.x, C1.y);
327  c[1].set_uv(C2.x, C2.y);
328  c[2].set_uv(C3.x, C3.y);
329 }
330 
338 {
339  c1=getCorner1();
340  c2=getCorner2();
341  c3=getCorner3();
342 }
343 
348 vpColVector vpTemplateTrackerTriangle::getCorner1() const
349 {
350  vpColVector c(2);
351  c[0]=C1.x;
352  c[1]=C1.y;
353 
354  return c;
355 }
360 vpColVector vpTemplateTrackerTriangle::getCorner2() const
361 {
362  vpColVector c(2);
363  c[0]=C2.x;
364  c[1]=C2.y;
365  return c;
366 }
367 
372 vpColVector vpTemplateTrackerTriangle::getCorner3() const
373 {
374  vpColVector c(2);
375  c[0]=C3.x;
376  c[1]=C3.y;
377  return c;
378 }
379 
385 void vpTemplateTrackerTriangle::getSize(double &w,double &h) const
386 {
387  w=l_t;
388  h=h_t;
389 }
395 void vpTemplateTrackerTriangle::getSize(int &w,int &h) const
396 {
397  w=(int)l_t+1;
398  h=(int)h_t+1;
399 }
400 
406 {
407  return minx_temp-1;
408 }
414 {
415  return miny_temp-1;
416 }
422 {
423  return minx_temp+l_t+1;
424 }
430 {
431  return miny_temp+h_t+1;
432 }
433 
434 
435 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
double get_v() const
Definition: vpImagePoint.h:268
vpTemplateTrackerTriangle getPyramidDown() const
double get_i() const
Definition: vpImagePoint.h:199
double get_u() const
Definition: vpImagePoint.h:257
vpTemplateTrackerDPoint C2
Corner 1.
double get_j() const
Definition: vpImagePoint.h:210
void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const
void getSize(double &w, double &h) const
bool inTriangle(const vpImagePoint &ip) const
vpTemplateTrackerDPoint C3
Corner 2.
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:3352
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
vpMatrix inverseByLU() const
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:243
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225