Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMeEllipse.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
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 https://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
31#include <cmath> // std::fabs
32#include <limits> // numeric_limits
33#include <vector>
34
35#include <visp3/core/vpDebug.h>
36#include <visp3/core/vpMatrixException.h>
37#include <visp3/core/vpTrackingException.h>
38#include <visp3/core/vpImagePoint.h>
39#include <visp3/core/vpRobust.h>
40#include <visp3/me/vpMe.h>
41#include <visp3/me/vpMeEllipse.h>
42
43
45 : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
46#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
47 mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
48#endif
49 thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.), m_expectedDensity(0),
50 m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
51{
52 // Resize internal parameters vector
53 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
54 K.resize(6);
55 iP1.set_ij(0, 0);
56 iP2.set_ij(0, 0);
57}
58
60 : vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
61 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce),
62 se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
63#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
64 mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02), m10(me_ellipse.m10), m01(me_ellipse.m01),
65 m11(me_ellipse.m11), m02(me_ellipse.m02), m20(me_ellipse.m20),
66#endif
67 thresholdWeight(me_ellipse.thresholdWeight),
68
69 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
70 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
71 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
72 m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc)
73{ }
74
76{
77 list.clear();
78 angle.clear();
79}
80
82{
83 double u = iP.get_u();
84 double v = iP.get_v();
85
86 return (computeTheta(u, v));
87}
88
89double vpMeEllipse::computeTheta(double u, double v) const
90{
91 double A = K[0] * u + K[2] * v + K[3];
92 double B = K[1] * v + K[2] * u + K[4];
93
94 double theta = atan2(B, A); // Angle between the tangent and the u axis.
95 if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
96 theta += M_PI;
97 }
98 return (theta);
99}
100
102{
103 vpMeSite p_me;
104 vpImagePoint iP;
105 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
106 p_me = *it;
107 // (i,j) frame used for vpMESite
108 iP.set_ij(p_me.ifloat, p_me.jfloat);
109 p_me.alpha = computeTheta(iP);
110 *it = p_me;
111 }
112}
113
115{
116 // Two versions are available. If you change from one version to the other
117 // one, do not forget to change also the reciprocal function
118 // computeAngleOnEllipse() just below and, for a correct display of an arc
119 // of ellipse, adapt vpMeEllipse::display() below and
120 // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
121 // so that the two extremities of the arc are correctly shown.
122
123 // Version that gives a regular angular sampling on the ellipse, so less
124 // points at its extremities
125 /*
126 double co = cos(angle);
127 double si = sin(angle);
128 double coef = a * b / sqrt(b * b * co * co + a * a * si * si);
129 double u = co * coef;
130 double v = si * coef;
131 iP.set_u(uc + ce * u - se * v);
132 iP.set_v(vc + se * u + ce * v);
133 */
134
135 // Version from "the two concentric circles" method that gives more points
136 // at the ellipse extremities for a regular angle sampling. It is better to
137 // display an ellipse, not necessarily to track it
138
139 // (u,v) are the coordinates on the canonical centered ellipse;
140 double u = a * cos(angle);
141 double v = b * sin(angle);
142 // a rotation of e and a translation by (uc,vc) are done
143 // to get the coordinates of the point on the shifted ellipse
144 iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v);
145}
146
148{
149 // Two versions are available. If you change from one version to the other
150 // one, do not forget to change also the reciprocal function
151 // computePointOnEllipse() just above. Adapt also the display; see comment
152 // at the beginning of computePointOnEllipse()
153
154 // Regular angle sampling method
155 /*
156 double du = pt.get_u() - uc;
157 double dv = pt.get_v() - vc;
158 double ang = atan2(dv,du) - e;
159 if (ang > M_PI) {
160 ang -= 2.0 * M_PI;
161 }
162 else if (ang < -M_PI) {
163 ang += 2.0 * M_PI;
164 }
165 */
166 // for the "two concentric circles method" starting from the previous one
167 // (just to remember the link between both methods:
168 // tan(theta_2cc) = a/b tan(theta_rs))
169 /*
170 double co = cos(ang);
171 double si = sin(ang);
172 double coeff = 1.0/sqrt(b*b*co*co+a*a*si*si);
173 si *= a*coeff;
174 co *= b*coeff;
175 ang = atan2(si,co);
176 */
177 // For the "two concentric circles" method starting from scratch
178 double du = pt.get_u() - m_uc;
179 double dv = pt.get_v() - m_vc;
180 double co = (du * ce + dv * se) / a;
181 double si = (-du * se + dv * ce) / b;
182 double angle = atan2(si, co);
183
184 return (angle);
185}
186
188{
189 double num = m_n20 - m_n02;
190 double d = num * num + 4.0 * m_n11 * m_n11; // always >= 0
191 if (d <= std::numeric_limits<double>::epsilon()) {
192 e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
193 ce = 1.0;
194 se = 0.0;
195 a = b = 2.0 * sqrt(m_n20); // = sqrt(2.0*(n20+n02))
196 }
197 else { // real ellipse
198 e = atan2(2.0 * m_n11, num) / 2.0; // e in [-Pi/2 ; Pi/2]
199 ce = cos(e);
200 se = sin(e);
201
202 d = sqrt(d); // d in sqrt always >= 0
203 num = m_n20 + m_n02;
204 a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
205 b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
206 }
207}
208
210{
211 K[0] = m_n02;
212 K[1] = m_n20;
213 K[2] = -m_n11;
214 K[3] = m_n11 * m_vc - m_n02 * m_uc;
215 K[4] = m_n11 * m_uc - m_n20 * m_vc;
216 K[5] = m_n02 * m_uc * m_uc + m_n20 * m_vc * m_vc - 2.0 * m_n11 * m_uc * m_vc + 4.0 * (m_n11 * m_n11 - m_n20 * m_n02);
217}
218
220{
221 m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se);
222 m_n11 = 0.25 * se * ce * (a * a - b * b);
223 m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce);
224}
225
227{
228 // Equations below from Chaumette PhD and TRO 2004 paper
229 double num = K[0] * K[1] - K[2] * K[2]; // > 0 for an ellipse
230 if (num <= 0) {
231 throw(vpException(vpException::fatalError, "The points do not belong to an ellipse! num: %f", num));
232 }
233
234 m_uc = (K[2] * K[4] - K[1] * K[3]) / num;
235 m_vc = (K[2] * K[3] - K[0] * K[4]) / num;
237
238 double d = (K[0] * m_uc * m_uc + K[1] * m_vc * m_vc + 2.0 * K[2] * m_uc * m_vc - K[5]) / (4.0 * num);
239 m_n20 = K[1] * d; // always > 0
240 m_n11 = -K[2] * d;
241 m_n02 = K[0] * d; // always > 0
242
244
245 // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
246 d = m_n02 / K[0]; // fabs(K[0]) > 0
247 for (unsigned int i = 0; i < 6; i++) {
248 K[i] *= d;
249 }
250 if (vpDEBUG_ENABLE(3)) {
252 }
253}
254
256{
257 std::cout << "K :" << K.t() << std::endl;
258 std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
259 std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 << std::endl;
260 std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) << std::endl;
261}
262
263void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
264{
265 // Warning: similar code in vpMbtMeEllipse::sample()
266 if (!me) {
267 throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
268 }
269 // Delete old lists
270 list.clear();
271 angle.clear();
272
273 int nbrows = static_cast<int>(I.getHeight());
274 int nbcols = static_cast<int>(I.getWidth());
275 // New version using distance for sampling
276 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
277 std::cout << "Warning: In vpMeEllipse::sample() ";
278 std::cout << "function called with sample step = 0. We set it rather to 10 pixels";
279 // std::cout << "function called with sample step = 0, set to 10 dg";
280 me->setSampleStep(10.0);
281 }
282 // Perimeter of the ellipse using Ramanujan formula
283 double perim = M_PI * (3.0 * (a + b) - sqrt((3.0 * a + b) * (a + 3.0 * b)));
284 // Number of points for a complete ellipse
285 unsigned int nb_pt = static_cast<unsigned int>(floor(perim / me->getSampleStep()));
286 double incr = 2.0 * M_PI / nb_pt;
287 // Compute of the expected density
288 if (!m_trackArc) { // number of points for a complete ellipse
289 m_expectedDensity = nb_pt;
290 }
291 else { // number of points for an arc of ellipse
292 m_expectedDensity *= static_cast<unsigned int>(floor(perim / me->getSampleStep() * (alpha2 - alpha1) / (2.0 * M_PI)));
293 }
294
295 // Starting angle for sampling: new version to not start at 0
296 double ang = alpha1 + incr / 2.0;
297
298 // sample positions
299 for (unsigned int i = 0; i < m_expectedDensity; i++) {
300 vpImagePoint iP;
301 computePointOnEllipse(ang, iP);
302 // If point is in the image, add to the sample list
303 // Check done in (i,j) frame)
304 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
306
307 double theta = computeTheta(iP);
308 vpMeSite pix;
309 // (i,j) frame used for vpMeSite
310 pix.init(iP.get_i(), iP.get_j(), theta);
313 list.push_back(pix);
314 angle.push_back(ang);
315 }
316 ang += incr;
317 }
318
319 if (!doNotTrack) {
321 }
322}
323
325{
326 if (!me) {
327 throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
328 }
329 unsigned int nb_pts_added = 0;
330 int nbrows = static_cast<int>(I.getHeight());
331 int nbcols = static_cast<int>(I.getWidth());
332
333 unsigned int memory_range = me->getRange();
334 me->setRange(2);
335
336 // Perimeter of the ellipse using Ramanujan formula
337 double perim = M_PI * (3.0 * (a + b) - sqrt((3.0 * a + b) * (a + 3.0 * b)));
338 // Number of points for a complete ellipse
339 unsigned int nb_pt = static_cast<unsigned int>(floor(perim / me->getSampleStep()));
340 double incr = 2.0 * M_PI / nb_pt;
341
342 // Detect holes and try to complete them
343 // In this option, the sample step is used to complete the holes as much as possible
344 std::list<double>::iterator angleList = angle.begin();
345 std::list<vpMeSite>::iterator meList = list.begin();
346 double ang = *angleList;
347 ++angleList;
348 ++meList;
349 while (meList != list.end()) {
350 double nextang = *angleList;
351 if ((nextang - ang) > 2.0 * incr) { // A hole exists
352 ang += incr; // next point to be checked
353 // adding only 1 point if hole of 1 point
354 while (ang < (nextang - incr)) {
355 vpImagePoint iP;
356 computePointOnEllipse(ang, iP);
357 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
358 double theta = computeTheta(iP);
359 vpMeSite pix;
360 pix.init(iP.get_i(), iP.get_j(), theta);
363 pix.track(I, me, false);
364 if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
365 nb_pts_added++;
366 iP.set_ij(pix.ifloat, pix.jfloat);
367 double new_ang = computeAngleOnEllipse(iP);
368 if ((new_ang - ang) > M_PI) {
369 new_ang -= 2.0 * M_PI;
370 }
371 else if ((ang - new_ang) > M_PI) {
372 new_ang += 2.0 * M_PI;
373 }
374 list.insert(meList, pix);
375 angle.insert(angleList, new_ang);
376 if (vpDEBUG_ENABLE(3)) {
378 }
379 }
380 }
381 ang += incr;
382 }
383 }
384 ang = nextang;
385 ++angleList;
386 ++meList;
387 }
388
389 if (vpDEBUG_ENABLE(3)) {
390 if (nb_pts_added > 0) {
391 std::cout << "Number of added points from holes with angles: " << nb_pts_added << std::endl;
392 }
393 }
394
395 // Add points in case two neighboring points are too far away
396 angleList = angle.begin();
397 ang = *angleList;
398 meList = list.begin();
399 vpMeSite pix1 = *meList;
400 ++angleList;
401 ++meList;
402 while (meList != list.end()) {
403 double nextang = *angleList;
404 vpMeSite pix2 = *meList;
405 double dist = sqrt((pix1.ifloat - pix2.ifloat) * (pix1.ifloat - pix2.ifloat)
406 + (pix1.jfloat - pix2.jfloat) * (pix1.jfloat - pix2.jfloat));
407 // Only one point is added if two neighboring points are too far away
408 if (dist > 2.0 * me->getSampleStep()) {
409 ang = (nextang + ang) / 2.0; // point added at mid angle
410 vpImagePoint iP;
411 computePointOnEllipse(ang, iP);
412 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
413 double theta = computeTheta(iP);
414 vpMeSite pix;
415 pix.init(iP.get_i(), iP.get_j(), theta);
418 pix.track(I, me, false);
419 if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
420 nb_pts_added++;
421 iP.set_ij(pix.ifloat, pix.jfloat);
422 double new_ang = computeAngleOnEllipse(iP);
423 if ((new_ang - ang) > M_PI) {
424 new_ang -= 2.0 * M_PI;
425 }
426 else if ((ang - new_ang) > M_PI) {
427 new_ang += 2.0 * M_PI;
428 }
429 list.insert(meList, pix);
430 angle.insert(angleList, new_ang);
431 if (vpDEBUG_ENABLE(3)) {
433 }
434 }
435 }
436 }
437 ang = nextang;
438 pix1 = pix2;
439 ++angleList;
440 ++meList;
441 }
442
443 if (vpDEBUG_ENABLE(3)) {
444 if (nb_pts_added > 0) {
445 std::cout << "Number of added points from holes : " << nb_pts_added << std::endl;
446 angleList = angle.begin();
447 while (angleList != angle.end()) {
448 ang = *angleList;
449 std::cout << "ang = " << vpMath::deg(ang) << std::endl;
450 ++angleList;
451 }
452 }
453 }
454
455 // Try to fill the first extremity: from alpha_min - incr to alpha1 + incr/2
456 unsigned int nbpts = 0;
457 // Add - incr/2.0 to avoid being too close to 0
458 if ((m_alphamin - alpha1 - incr / 2.0) > 0.0) {
459 nbpts = static_cast<unsigned int>(floor((m_alphamin - alpha1 - incr / 2.0) / incr));
460 }
461 ang = m_alphamin - incr;
462 for (unsigned int i = 0; i < nbpts; i++) {
463 vpImagePoint iP;
464 computePointOnEllipse(ang, iP);
465 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
466 double theta = computeTheta(iP);
467 vpMeSite pix;
468 pix.init(iP.get_i(), iP.get_j(), theta);
471 pix.track(I, me, false);
472 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
473 nb_pts_added++;
474 iP.set_ij(pix.ifloat, pix.jfloat);
475 double new_ang = computeAngleOnEllipse(iP);
476 if ((new_ang - ang) > M_PI) {
477 new_ang -= 2.0 * M_PI;
478 }
479 else if ((ang - new_ang) > M_PI) {
480 new_ang += 2.0 * M_PI;
481 }
482 list.push_front(pix);
483 angle.push_front(new_ang);
484 if (vpDEBUG_ENABLE(3)) {
486 std::cout << "Add extremity 1, ang = " << vpMath::deg(new_ang) << std::endl;
487 }
488 }
489 }
490 ang -= incr;
491 }
492
493 if (vpDEBUG_ENABLE(3)) {
494 if (nb_pts_added > 0) std::cout << "Number of added points from holes and first extremity : " << nb_pts_added << std::endl;
495 }
496
497 // Try to fill the second extremity: from alphamax + incr to alpha2 - incr/2
498 nbpts = 0;
499 if ((alpha2 - incr / 2.0 - m_alphamax) > 0.0) {
500 nbpts = static_cast<unsigned int>(floor((alpha2 - incr / 2.0 - m_alphamax) / incr));
501 }
502 ang = m_alphamax + incr;
503 for (unsigned int i = 0; i < nbpts; i++) {
504 vpImagePoint iP;
505 computePointOnEllipse(ang, iP);
506 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
507 double theta = computeTheta(iP);
508 vpMeSite pix;
509 pix.init(iP.get_i(), iP.get_j(), theta);
512 pix.track(I, me, false);
513 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
514 nb_pts_added++;
515 iP.set_ij(pix.ifloat, pix.jfloat);
516 double new_ang = computeAngleOnEllipse(iP);
517 if ((new_ang - ang) > M_PI) {
518 new_ang -= 2.0 * M_PI;
519 }
520 else if ((ang - new_ang) > M_PI) {
521 new_ang += 2.0 * M_PI;
522 }
523 list.push_back(pix);
524 angle.push_back(new_ang);
525 if (vpDEBUG_ENABLE(3)) {
527 std::cout << "Add extremity 2, ang = " << vpMath::deg(new_ang) << std::endl;
528 }
529 }
530 }
531 ang += incr;
532 }
533 me->setRange(memory_range);
534
535 if (vpDEBUG_ENABLE(3)) {
536 if (nb_pts_added > 0) std::cout << "In plugHoles(): nb of added points : " << nb_pts_added << std::endl;
537 }
538 return nb_pts_added;
539}
540
541void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
542{
543 double um = I.getWidth() / 2.;
544 double vm = I.getHeight() / 2.;
545 unsigned int n = static_cast<unsigned int>(iP.size());
546
547 if (m_trackCircle) { // we track a circle
548 if (n < 3) {
549 throw(vpException(vpException::dimensionError, "Not enough points to compute the circle"));
550 }
551 // System A x = b to be solved by least squares
552 // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2)
553
554 vpMatrix A(n, 3);
555 vpColVector b(n);
556
557 for (unsigned int k = 0; k < n; k++) {
558 // normalization so that (u,v) in [-1;1]
559 double u = (iP[k].get_u() - um) / um;
560 double v = (iP[k].get_v() - vm) / um; // um here to not deform the circle
561 A[k][0] = u;
562 A[k][1] = v;
563 A[k][2] = 1.0;
564 b[k] = u * u + v * v;
565 }
566 vpColVector x(3);
567 x = A.solveBySVD(b);
568 // A circle is a particular ellipse. Going from x for circle to K for ellipse
569 // using inverse normalization to go back to pixel values
570 double ratio = vm / um;
571 K[0] = K[1] = 1.0 / (um * um);
572 K[2] = 0.0;
573 K[3] = -(1.0 + x[0] / 2.0) / um;
574 K[4] = -(ratio + x[1] / 2.0) / um;
575 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
576 }
577 else { // we track an ellipse
578 if (n < 5) {
579 throw(vpException(vpException::dimensionError, "Not enough points to compute the ellipse"));
580 }
581 // Homogeneous system A x = 0 ; x is the nullspace of A
582 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
583 // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
584
585 // It would be a bad idea to solve the same system using A x = b where
586 // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
587 // cannot consider the case where the origin belongs to the ellipse.
588 // Another possibility would be to consider K0+K1=1 which is always valid,
589 // leading to the system A x = b where
590 // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
591
592 vpMatrix A(n, 6);
593
594 for (unsigned int k = 0; k < n; k++) {
595 // Normalization so that (u,v) in [-1;1]
596 double u = (iP[k].get_u() - um) / um;
597 double v = (iP[k].get_v() - vm) / vm;
598 A[k][0] = u * u;
599 A[k][1] = v * v;
600 A[k][2] = 2.0 * u * v;
601 A[k][3] = 2.0 * u;
602 A[k][4] = 2.0 * v;
603 A[k][5] = 1.0;
604 }
605 vpMatrix KerA;
606 unsigned int dim = A.nullSpace(KerA, 1);
607 if (dim > 1) { // case with less than 5 independent points
608 throw(vpException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned"));
609 }
610 for (unsigned int i = 0; i < 6; i++)
611 K[i] = KerA[i][0];
612
613 // inverse normalization
614 K[0] *= vm / um;
615 K[1] *= um / vm;
616 K[3] = K[3] * vm - K[0] * um - K[2] * vm;
617 K[4] = K[4] * um - K[1] * vm - K[2] * um;
618 K[5] = K[5] * um * vm - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
619 }
621}
622
624{
625 double um = I.getWidth() / 2.;
626 double vm = I.getHeight() / 2.;
627
628 const unsigned int nos = numberOfSignal();
629 unsigned int k = 0; // count the number of tracked MEs
630
631 vpColVector w(nos);
632 w = 1.0;
633 // Note that the (nos-k) last rows of w are not used. Hopefully, this is not an issue.
634
635 if (m_trackCircle) { // we track a circle
636 // System A x = b to be solved by least squares
637 // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2)
638
639 // Note that the (nos-k) last rows of A, b, xp and yp are not used.
640 // Hopefully, this is not an issue.
641 vpMatrix A(nos, 3);
642 vpColVector b(nos);
643
644 // Useful to compute the weights in the robust estimation
645 vpColVector xp(nos), yp(nos);
646
647 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
648 vpMeSite p_me = *it;
649 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
650 // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
651 double u = (p_me.jfloat - um) / um;
652 double v = (p_me.ifloat - vm) / um; // um to not deform the circle
653 A[k][0] = u;
654 A[k][1] = v;
655 A[k][2] = 1.0;
656 b[k] = u * u + v * v;
657 // Useful to compute the weights in the robust estimation
658 xp[k] = p_me.jfloat;
659 yp[k] = p_me.ifloat;
660
661 k++;
662 }
663 }
664 if (k < 3) {
665 throw(vpException(vpException::dimensionError, "Not enough moving edges %d / %d to track the circle ", k, list.size()));
666 }
667
668 vpRobust r;
669 r.setMinMedianAbsoluteDeviation(1.0); // Image noise in pixels for the algebraic distance
670
671 unsigned int iter = 0;
672 double var = 1.0;
673 vpColVector x(3);
674 vpMatrix DA(k, 3);
675 vpColVector Db(k);
676 vpColVector xg_prev(2);
677 xg_prev = -10.0;
678
679 // stop after 4 it or if cog variation between 2 it is more than 1 pixel
680 while ((iter < 4) && (var > 0.1)) {
681 for (unsigned int i = 0; i < k; i++) {
682 for (unsigned int j = 0; j < 3; j++) {
683 DA[i][j] = w[i] * A[i][j];
684 }
685 Db[i] = w[i] * b[i];
686 }
687 x = DA.solveBySVD(Db);
688
689 // A circle is a particular ellipse. Going from x for circle to K for ellipse
690 // using inverse normalization to go back to pixel values
691 double ratio = vm / um;
692 K[0] = K[1] = 1.0 / (um * um);
693 K[2] = 0.0;
694 K[3] = -(1.0 + x[0] / 2.0) / um;
695 K[4] = -(ratio + x[1] / 2.0) / um;
696 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
697
699 vpColVector xg(2);
700 xg[0] = m_uc;
701 xg[1] = m_vc;
702 var = (xg - xg_prev).frobeniusNorm();
703 xg_prev = xg;
704
705 vpColVector residu(k); // near to geometric distance in pixel
706 for (unsigned int i = 0; i < k; i++) {
707 double x = xp[i];
708 double y = yp[i];
709 double sign = K[0] * x * x + K[1] * y * y + 2. * K[2] * x * y + 2. * K[3] * x + 2. * K[4] * y + K[5];
710 vpImagePoint ip1, ip2;
711 ip1.set_uv(x, y);
712 double ang = computeAngleOnEllipse(ip1);
713 computePointOnEllipse(ang, ip2);
714 // residu = 0 if point is exactly on the ellipse, not otherwise
715 if (sign > 0) residu[i] = vpImagePoint::distance(ip1, ip2);
716 else residu[i] = -vpImagePoint::distance(ip1, ip2);
717 }
718 r.MEstimator(vpRobust::TUKEY, residu, w);
719
720 iter++;
721 }
722 }
723 else { // we track an ellipse
724
725 // Homogeneous system A x = 0 ; x is the nullspace of A
726 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
727 // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
728
729 // It would be a bad idea to solve the same system using A x = b where
730 // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
731 // cannot consider the case where the origin belongs to the ellipse.
732 // Another possibility would be to consider K0+K1=1 which is always valid,
733 // leading to the system A x = b where
734 // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
735
736 vpMatrix A(nos, 6);
737 // Useful to compute the weights in the robust estimation
738 vpColVector xp(nos), yp(nos);
739
740 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
741 vpMeSite p_me = *it;
742 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
743 // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
744 double u = (p_me.jfloat - um) / um;
745 double v = (p_me.ifloat - vm) / vm;
746 A[k][0] = u * u;
747 A[k][1] = v * v;
748 A[k][2] = 2.0 * u * v;
749 A[k][3] = 2.0 * u;
750 A[k][4] = 2.0 * v;
751 A[k][5] = 1.0;
752 // Useful to compute the weights in the robust estimation
753 xp[k] = p_me.jfloat;
754 yp[k] = p_me.ifloat;
755
756 k++;
757 }
758 }
759 if (k < 5) {
760 throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
761 }
762
763 vpRobust r;
764
765 r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance
766 unsigned int iter = 0;
767 double var = 1.0;
768 vpMatrix DA(k, 6);
769 vpMatrix KerDA;
770 vpColVector xg_prev(2);
771 xg_prev = -10.0;
772
773 // Stop after 4 iterations or if cog variation between 2 iterations is more than 0.1 pixel
774 while ((iter < 4) && (var > 0.1)) {
775 for (unsigned int i = 0; i < k; i++) {
776 for (unsigned int j = 0; j < 6; j++) {
777 DA[i][j] = w[i] * A[i][j];
778 }
779 }
780 unsigned int dim = DA.nullSpace(KerDA, 1);
781 if (dim > 1) { // case with less than 5 independent points
782 throw(vpException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned"));
783 }
784
785 for (unsigned int i = 0; i < 6; i++)
786 K[i] = KerDA[i][0]; // norm(K) = 1
787
788 // inverse normalization
789 K[0] *= vm / um;
790 K[1] *= um / vm;
791 K[3] = K[3] * vm - K[0] * um - K[2] * vm;
792 K[4] = K[4] * um - K[1] * vm - K[2] * um;
793 K[5] = K[5] * um * vm - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
794
795 getParameters(); // since a, b, and e are used just after
796 vpColVector xg(2);
797 xg[0] = m_uc;
798 xg[1] = m_vc;
799 var = (xg - xg_prev).frobeniusNorm();
800 xg_prev = xg;
801
802 vpColVector residu(k);
803 for (unsigned int i = 0; i < k; i++) {
804 double x = xp[i];
805 double y = yp[i];
806 double sign = K[0] * x * x + K[1] * y * y + 2. * K[2] * x * y + 2. * K[3] * x + 2. * K[4] * y + K[5];
807 vpImagePoint ip1, ip2;
808 ip1.set_uv(x, y);
809 double ang = computeAngleOnEllipse(ip1);
810 computePointOnEllipse(ang, ip2);
811 // residu = 0 if point is exactly on the ellipse, not otherwise
812 if (sign > 0) residu[i] = vpImagePoint::distance(ip1, ip2);
813 else residu[i] = -vpImagePoint::distance(ip1, ip2);
814 }
815 r.MEstimator(vpRobust::TUKEY, residu, w);
816
817 iter++;
818 }
819 } // end of case ellipse
820
821 // Remove bad points and outliers from the lists
822 // Modify the angle to order the list
823 double previous_ang = -4.0 * M_PI;
824 k = 0;
825 std::list<double>::iterator angleList = angle.begin();
826 for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
827 vpMeSite p_me = *meList;
828 if (p_me.getState() != vpMeSite::NO_SUPPRESSION) {
829 // points not selected as me
830 double ang = *angleList;
831 meList = list.erase(meList);
832 angleList = angle.erase(angleList);
833 if (vpDEBUG_ENABLE(3)) {
834 vpImagePoint iP;
835 iP.set_ij(p_me.ifloat, p_me.jfloat);
836 printf("point %d not me i : %.0f , j : %0.f, ang = %lf\n", k, p_me.ifloat, p_me.jfloat, vpMath::deg(ang));
838 }
839 }
840 else {
841 if (w[k] < thresholdWeight) { // outlier
842 double ang = *angleList;
843 meList = list.erase(meList);
844 angleList = angle.erase(angleList);
845 if (vpDEBUG_ENABLE(3)) {
846 vpImagePoint iP;
847 iP.set_ij(p_me.ifloat, p_me.jfloat);
848 printf("point %d outlier i : %.0f , j : %0.f, ang = %lf, poids : %lf\n",
849 k, p_me.ifloat, p_me.jfloat, vpMath::deg(ang), w[k]);
851 }
852 }
853 else { // good point
854 double ang = *angleList;
855 vpImagePoint iP;
856 iP.set_ij(p_me.ifloat, p_me.jfloat);
857 double new_ang = computeAngleOnEllipse(iP);
858 if ((new_ang - ang) > M_PI) {
859 new_ang -= 2.0 * M_PI;
860 }
861 else if ((ang - new_ang) > M_PI) {
862 new_ang += 2.0 * M_PI;
863 }
864 *angleList = previous_ang = new_ang;
865 ++meList;
866 ++angleList;
867 if (vpDEBUG_ENABLE(3)) {
868 printf("point %d inlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.ifloat, p_me.jfloat, w[k]);
870 }
871 }
872 k++;
873 }
874 }
875
876 // Manage the list so that all new angles belong to [0;2Pi]
877 bool nbdeb = false;
878 std::list<double> finAngle;
879 finAngle.clear();
880 std::list<vpMeSite> finMe;
881 finMe.clear();
882 std::list<double>::iterator debutAngleList;
883 std::list<vpMeSite>::iterator debutMeList;
884 angleList = angle.begin();
885 std::list<vpMeSite>::iterator meList;
886 for (meList = list.begin(); meList != list.end();) {
887 vpMeSite p_me = *meList;
888 double ang = *angleList;
889
890 // Move these ones to another list to be added at the end
891 if (ang < alpha1) {
892 ang += 2.0 * M_PI;
893 angleList = angle.erase(angleList);
894 finAngle.push_back(ang);
895 meList = list.erase(meList);
896 finMe.push_back(p_me);
897 }
898 // Moved at the beginning of the list
899 else if (ang > alpha2) {
900 ang -= 2.0 * M_PI;
901 angleList = angle.erase(angleList);
902 meList = list.erase(meList);
903 if (!nbdeb) {
904 angle.push_front(ang);
905 debutAngleList = angle.begin();
906 ++debutAngleList;
907
908 list.push_front(p_me);
909 debutMeList = list.begin();
910 ++debutMeList;
911
912 nbdeb = true;
913 }
914 else {
915 debutAngleList = angle.insert(debutAngleList, ang);
916 ++debutAngleList;
917 debutMeList = list.insert(debutMeList, p_me);
918 ++debutMeList;
919 }
920 }
921 else {
922 ++angleList;
923 ++meList;
924 }
925 }
926 // Fuse the lists
927 angleList = angle.end();
928 angle.splice(angleList, finAngle);
929 meList = list.end();
930 list.splice(meList, finMe);
931
932 unsigned int numberOfGoodPoints = 0;
933 previous_ang = -4.0 * M_PI;
934
935 // Perimeter of the ellipse using Ramanujan formula
936 double perim = M_PI * (3.0 * (a + b) - sqrt((3.0 * a + b) * (a + 3.0 * b)));
937 unsigned int nb_pt = static_cast<unsigned int>(floor(perim / me->getSampleStep()));
938 double incr = 2.0 * M_PI / nb_pt;
939 // Update of the expected density
940 if (!m_trackArc) { // number of points for a complete ellipse
941 m_expectedDensity = nb_pt;
942 }
943 else { // number of points for an arc of ellipse
944 m_expectedDensity *= static_cast<unsigned int>(floor(perim / me->getSampleStep() * (alpha2 - alpha1) / (2.0 * M_PI)));
945 }
946
947 // Keep only the points in the interval [alpha1 ; alpha2]
948 // and those that are not too close
949 angleList = angle.begin();
950 for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
951 vpMeSite p_me = *meList;
952 double new_ang = *angleList;
953 if ((new_ang >= alpha1) && (new_ang <= alpha2)) {
954 if ((new_ang - previous_ang) >= (0.6 * incr)) {
955 previous_ang = new_ang;
956 numberOfGoodPoints++;
957 ++meList;
958 ++angleList;
959 if (vpDEBUG_ENABLE(3)) {
960 vpImagePoint iP;
961 iP.set_ij(p_me.ifloat, p_me.jfloat);
963 printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n", vpMath::deg(new_ang), iP.get_i(), iP.get_j());
964 }
965 }
966 else {
967 meList = list.erase(meList);
968 angleList = angle.erase(angleList);
969 if (vpDEBUG_ENABLE(3)) {
970 vpImagePoint iP;
971 iP.set_ij(p_me.ifloat, p_me.jfloat);
973 printf("too near : angle %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
974 }
975 }
976 }
977 else { // point not in the interval [alpha1 ; alpha2]
978 meList = list.erase(meList);
979 angleList = angle.erase(angleList);
980 if (vpDEBUG_ENABLE(3)) {
981 vpImagePoint iP;
982 iP.set_ij(p_me.ifloat, p_me.jfloat);
984 printf("not in interval: angle : %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
985 }
986 }
987 }
988 // set extremities of the angle list
989 m_alphamin = angle.front();
990 m_alphamax = angle.back();
991
992 if (vpDEBUG_ENABLE(3)) {
993 printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
994 printf("Fin leastSquareRobust : nb pts ok = %d \n", numberOfGoodPoints);
995 }
996
997 return(numberOfGoodPoints);
998}
999
1000void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
1001{
1002 vpMeEllipse::displayEllipse(I, iPc, a, b, e, alpha1, alpha2, col, thickness);
1003}
1004
1005void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, bool trackCircle, bool trackArc)
1006{
1007 unsigned int n = 5; // by default, 5 points for an ellipse
1008 m_trackCircle = trackCircle;
1009 if (trackCircle)
1010 n = 3;
1011 std::vector<vpImagePoint> iP(n);
1012 m_trackArc = trackArc;
1013
1015
1016 if (m_trackCircle) {
1017 if (m_trackArc) {
1018 std::cout << "First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
1019 }
1020 for (unsigned int k = 0; k < n; k++) {
1021 std::cout << "Click point " << k + 1 << "/" << n << " on the circle " << std::endl;
1022 vpDisplay::getClick(I, iP[k], true);
1025 std::cout << iP[k] << std::endl;
1026 }
1027 }
1028 else {
1029 if (m_trackArc) {
1030 std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
1031 }
1032 for (unsigned int k = 0; k < n; k++) {
1033 std::cout << "Click point " << k + 1 << "/" << n << " on the ellipse " << std::endl;
1034 vpDisplay::getClick(I, iP[k], true);
1037 std::cout << iP[k] << std::endl;
1038 }
1039 }
1040 initTracking(I, iP, trackCircle, trackArc);
1041}
1042
1043void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP,
1044 bool trackCircle, bool trackArc)
1045{
1046 m_trackArc = trackArc;
1047 m_trackCircle = trackCircle;
1048 // useful for sample(I):
1049 leastSquare(I, iP);
1050 if (trackArc) {
1051 // useful for track(I):
1052 iP1 = iP.front();
1053 iP2 = iP.back();
1054 // useful for sample(I):
1057 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
1058 alpha2 += 2.0 * M_PI;
1059 }
1060 }
1061 else {
1062 alpha1 = 0.0;
1063 alpha2 = 2 * M_PI;
1064 // useful for track(I):
1065 vpImagePoint ip;
1067 iP1 = iP2 = ip;
1068 }
1069
1070 sample(I);
1071 track(I);
1074}
1075
1077 const vpImagePoint *pt2, bool trackCircle)
1078{
1079 m_trackCircle = trackCircle;
1080 if (pt1 != NULL && pt2 != NULL) {
1081 m_trackArc = true;
1082 }
1083 // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
1084 m_uc = param[0];
1085 m_vc = param[1];
1086 m_n20 = param[2];
1087 m_n11 = param[3];
1088 m_n02 = param[4];
1091
1092 if (m_trackArc) {
1095 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
1096 alpha2 += 2.0 * M_PI;
1097 }
1098 // useful for track(I)
1099 iP1 = *pt1;
1100 iP2 = *pt2;
1101 }
1102 else {
1103 alpha1 = 0.0;
1104 alpha2 = 2.0 * M_PI;
1105 // useful for track(I)
1106 vpImagePoint ip;
1108 iP1 = iP2 = ip;
1109 }
1110 // useful for display(I) so useless if no display before track(I)
1111 iPc.set_uv(m_uc, m_vc);
1112
1113 sample(I);
1114 track(I);
1117}
1118
1125{
1127
1128 // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
1129 if (m_trackArc) {
1132 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
1133 alpha2 += 2.0 * M_PI;
1134 }
1135 }
1136 // Compute the ellipse parameters from the tracked points, manage the lists,
1137 // and update the expected density (
1139 if (vpDEBUG_ENABLE(3)) {
1140 printf("1st step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1143 }
1144
1145 if (plugHoles(I) > 0) {
1146 m_numberOfGoodPoints = leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists
1147 if (vpDEBUG_ENABLE(3)) {
1148 printf("2nd step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", m_numberOfGoodPoints, m_expectedDensity,
1150 }
1151 }
1152
1153 if (m_numberOfGoodPoints <= 5) {
1154 if (vpDEBUG_ENABLE(3)) {
1155 printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
1156 printf("A click to continue \n");
1160 }
1161 sample(I);
1164 if (vpDEBUG_ENABLE(3)) {
1165 printf("nb of Good points %u, density %d %lf, alphamin %lf, alphamax\n",
1168 }
1169
1170 // Stop in case of failure after resample
1171 if (m_numberOfGoodPoints <= 5) {
1172 throw(vpException(vpTrackingException::notEnoughPointError, "Impossible to track the ellipse, not enough features"));
1173 }
1174 }
1175
1176 if (vpDEBUG_ENABLE(3)) {
1178 }
1179
1180 // remet a jour l'angle delta pour chaque vpMeSite de la liste
1181 updateTheta();
1182 // not in getParameters since computed only once for each image
1183 m00 = M_PI * a * b;
1184
1185 // Useful only for tracking an arc of ellipse, but done to give them a value
1188
1189#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1190 computeMoments();
1191#endif
1192
1193 if (vpDEBUG_ENABLE(3)) {
1197 }
1198}
1199
1200#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1204void vpMeEllipse::computeMoments()
1205{
1206 // m00 = M_PI * a * b; // moved in track(I)
1207
1208 m10 = m_uc * m00;
1209 m01 = m_vc * m00;
1210
1211 mu20 = m_n20 * m00;
1212 mu11 = m_n11 * m00;
1213 mu02 = m_n02 * m00;
1214
1215 m20 = mu20 + m10 * m_uc;
1216 m11 = mu11 + m10 * m_vc;
1217 m02 = mu02 + m01 * m_vc;
1218}
1219
1233void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &center_p, double a_p,
1234 double b_p, double e_p, double alpha1_p, double alpha2_p)
1235{
1236 m_trackArc = true;
1237 // useful for sample(I): uc, vc, a, b, e, Ki, alpha1, alpha2
1238 m_uc = center_p.get_u();
1239 m_vc = center_p.get_v();
1240 a = a_p;
1241 b = b_p;
1242 e = e_p;
1243 ce = cos(e);
1244 se = sin(e);
1247
1248 alpha1 = alpha1_p;
1249 alpha2 = alpha2_p;
1250 if (alpha2 < alpha1) {
1251 alpha2 += 2 * M_PI;
1252 }
1253 // useful for track(I)
1254 vpImagePoint ip;
1256 iP1 = ip;
1258 iP2 = ip;
1259 // currently not used after, but done to be complete
1260 // would be needed for displaying the ellipse here
1261 iPc = center_p;
1262
1263 sample(I);
1264 track(I);
1267}
1268
1282{
1283 std::vector<vpImagePoint> v_iP(n);
1284
1285 for (unsigned int i = 0; i < n; i++) {
1286 v_iP[i] = iP[i];
1287 }
1288 initTracking(I, v_iP);
1289}
1290
1294void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, unsigned *i, unsigned *j)
1295{
1296 std::vector<vpImagePoint> v_iP(n);
1297
1298 for (unsigned int k = 0; k < n; k++) {
1299 v_iP[k].set_ij(i[k], j[k]);
1300 }
1301 initTracking(I, v_iP);
1302}
1303
1328void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
1329 const double &B, const double &E, const double &smallalpha, const double &highalpha,
1330 const vpColor &color, unsigned int thickness)
1331{
1332 vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
1333}
1334
1362void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1363 const double &E, const double &smallalpha, const double &highalpha,
1364 const vpColor &color, unsigned int thickness)
1365{
1366 vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
1367}
1368#endif // Deprecated
1369
1370
1371void vpMeEllipse::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
1372 const double &B, const double &E, const double &smallalpha, const double &highalpha,
1373 const vpColor &color, unsigned int thickness)
1374{
1375 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1376}
1377
1378void vpMeEllipse::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1379 const double &E, const double &smallalpha, const double &highalpha,
1380 const vpColor &color, unsigned int thickness)
1381{
1382 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1383}
Implementation of column vector and the associated operations.
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor cyan
Definition vpColor.h:220
static const vpColor orange
Definition vpColor.h:221
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_j() const
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
double get_u() const
void set_uv(double u, double v)
double get_i() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static int round(double x)
Definition vpMath.h:323
static double deg(double rad)
Definition vpMath.h:106
@ rankDeficient
Rank deficient.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void solveBySVD(const vpColVector &B, vpColVector &x) const
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Class that tracks an ellipse using moving edges.
Definition vpMeEllipse.h:90
double m_n20
Second order centered and normalized moments .
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
double a
is the semi major axis of the ellipse.
void computePointOnEllipse(const double angle, vpImagePoint &iP)
double se
Value of sin(e).
double mu11
Second order centered moments.
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
double m10
First order raw moments.
void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness=1)
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
vpImagePoint iP2
vpColVector K
double m_vc
Value of v coordinate of iPc.
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
vpImagePoint iPc
The coordinates of the ellipse center.
void computeKiFromNij()
unsigned int plugHoles(const vpImage< unsigned char > &I)
void computeNijFromAbe()
std::list< double > angle
Stores the value in increasing order of the angle on the ellipse for each vpMeSite.
double b
is the semi minor axis of the ellipse.
void getParameters()
void printParameters() const
double m_uc
Value of u coordinate of iPc.
bool m_trackCircle
Track a circle (true) or an ellipse (false).
void computeAbeFromNij()
double ce
Value of cos(e).
double m00
Ellipse area.
double m_alphamin
double m11
Second order raw moments.
bool m_trackArc
Track an arc of ellipse/circle (true) or a complete one (false).
void initTracking(const vpImage< unsigned char > &I, bool trackCircle=false, bool trackArc=false)
unsigned int leastSquareRobust(const vpImage< unsigned char > &I)
double m_alphamax
double computeTheta(const vpImagePoint &iP) const
double m_n02
Second order centered and normalized moments .
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
void updateTheta()
virtual ~vpMeEllipse()
double alpha2
vpImagePoint iP1
double m_n11
Second order centered and normalized moments .
void track(const vpImage< unsigned char > &I)
double alpha1
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &A, const double &B, const double &E, const double &smallalpha, const double &highalpha, const vpColor &color=vpColor::green, unsigned int thickness=1)
double thresholdWeight
Threshold on the weights for the robust least square.
double computeAngleOnEllipse(const vpImagePoint &pt) const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition vpMeSite.h:65
@ NO_SUPPRESSION
Point used by the tracker.
Definition vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition vpMeSite.h:210
double ifloat
Floating coordinates along i of a site.
Definition vpMeSite.h:100
double alpha
Angle of tangent at site.
Definition vpMeSite.h:106
void init()
Definition vpMeSite.cpp:59
double jfloat
Floating coordinates along j of a site.
Definition vpMeSite.h:102
vpMeSiteState getState() const
Definition vpMeSite.h:261
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
Definition vpMeSite.cpp:305
void setState(const vpMeSiteState &flag)
Definition vpMeSite.h:247
Contains abstract elements for a Distance to Feature type feature.
Definition vpMeTracker.h:60
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition vpMeTracker.h:86
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
std::list< vpMeSite > list
Definition vpMeTracker.h:72
void display(const vpImage< unsigned char > &I)
vpMe * me
Moving edges initialisation parameters.
Definition vpMeTracker.h:74
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
double getSampleStep() const
Definition vpMe.h:397
unsigned int getRange() const
Definition vpMe.h:273
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
@ notEnoughPointError
Not enough point to track.
#define vpDEBUG_ENABLE(level)
Definition vpDebug.h:533