Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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{
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
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
126vpTemplateTrackerTriangle::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
152vpTemplateTrackerTriangle::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
188void 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
199void 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
264bool 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
287bool 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
307bool 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
326void 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
351vpColVector vpTemplateTrackerTriangle::getCorner1() const
352{
353 vpColVector c(2);
354 c[0] = C1.x;
355 c[1] = C1.y;
356
357 return c;
358}
364vpColVector vpTemplateTrackerTriangle::getCorner2() const
365{
366 vpColVector c(2);
367 c[0] = C2.x;
368 c[1] = C2.y;
369 return c;
370}
371
377vpColVector vpTemplateTrackerTriangle::getCorner3() const
378{
379 vpColVector c(2);
380 c[0] = C3.x;
381 c[1] = C3.y;
382 return c;
383}
384
390void vpTemplateTrackerTriangle::getSize(double &w, double &h) const
391{
392 w = l_t;
393 h = h_t;
394}
400void vpTemplateTrackerTriangle::getSize(int &w, int &h) const
401{
402 w = (int)l_t + 1;
403 h = (int)h_t + 1;
404}
405
410double vpTemplateTrackerTriangle::getMinx() const { return minx_temp - 1; }
415double vpTemplateTrackerTriangle::getMiny() const { return miny_temp - 1; }
420double vpTemplateTrackerTriangle::getMaxx() const { return minx_temp + l_t + 1; }
425double vpTemplateTrackerTriangle::getMaxy() const { return miny_temp + h_t + 1; }
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_j() const
double get_u() const
void set_uv(double u, double v)
double get_i() const
double get_v() const
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix inverseByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
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