Visual Servoing Platform version 3.5.0
vpTemplateTrackerTriangle.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#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{
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;
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
127vpTemplateTrackerTriangle::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
153vpTemplateTrackerTriangle::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
189void 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
200void 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
265bool 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
288bool 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
308bool 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
327void 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
352vpColVector vpTemplateTrackerTriangle::getCorner1() const
353{
354 vpColVector c(2);
355 c[0] = C1.x;
356 c[1] = C1.y;
357
358 return c;
359}
365vpColVector vpTemplateTrackerTriangle::getCorner2() const
366{
367 vpColVector c(2);
368 c[0] = C2.x;
369 c[1] = C2.y;
370 return c;
371}
372
378vpColVector vpTemplateTrackerTriangle::getCorner3() const
379{
380 vpColVector c(2);
381 c[0] = C3.x;
382 c[1] = C3.y;
383 return c;
384}
385
391void vpTemplateTrackerTriangle::getSize(double &w, double &h) const
392{
393 w = l_t;
394 h = h_t;
395}
401void vpTemplateTrackerTriangle::getSize(int &w, int &h) const
402{
403 w = (int)l_t + 1;
404 h = (int)h_t + 1;
405}
406
411double vpTemplateTrackerTriangle::getMinx() const { return minx_temp - 1; }
416double vpTemplateTrackerTriangle::getMiny() const { return miny_temp - 1; }
421double vpTemplateTrackerTriangle::getMaxx() const { return minx_temp + l_t + 1; }
426double vpTemplateTrackerTriangle::getMaxy() const { return miny_temp + h_t + 1; }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_j() const
Definition: vpImagePoint.h:214
double get_u() const
Definition: vpImagePoint.h:262
void set_uv(double u, double v)
Definition: vpImagePoint.h:247
double get_i() const
Definition: vpImagePoint.h:203
double get_v() const
Definition: vpImagePoint.h:273
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix inverseByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
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