Visual Servoing Platform version 3.5.0
vpMeEllipse.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 * Moving edges.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39#include <visp3/me/vpMeEllipse.h>
40
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpImagePoint.h>
44#include <visp3/core/vpRobust.h>
45#include <visp3/me/vpMe.h>
46
47#include <cmath> // std::fabs
48#include <limits> // numeric_limits
49#include <vector>
50
55 : K(), iPc(), a(0.), b(0.), e(0.),
56 iP1(), iP2(), alpha1(0), alpha2(2 * M_PI),
57 ce(0.), se(0.), angle(), m00(0.),
58 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
59 mu11(0.), mu20(0.), mu02(0.),
60 m10(0.), m01(0.),
61 m11(0.), m02(0.), m20(0.),
62 #endif
63 thresholdWeight(0.2),
64 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
65 expecteddensity(0.),
66 #endif
67 m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
68 m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
69{
70 // Resize internal parameters vector
71 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
72 K.resize(6);
73 iP1.set_ij(0,0);
74 iP2.set_ij(0,0);
75}
76
81 : vpMeTracker(me_ellipse),
82 K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
83 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2),
84 ce(me_ellipse.ce), se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
85 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86 mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02),
87 m10(me_ellipse.m10), m01(me_ellipse.m01), m11(me_ellipse.m11),
88 m02(me_ellipse.m02), m20(me_ellipse.m20),
89 #endif
90 thresholdWeight(me_ellipse.thresholdWeight),
91 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
92 expecteddensity(me_ellipse.expecteddensity),
93 #endif
94 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax),
95 m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
96 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
97 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints), m_trackArc(me_ellipse.m_trackArc)
98{
99}
100
105{
106 list.clear();
107 angle.clear();
108}
109
118{
119 double u = iP.get_u();
120 double v = iP.get_v();
121
122 return (computeTheta(u, v));
123}
124
132double vpMeEllipse::computeTheta(double u, double v) const
133{
134 double A = K[0] * u + K[2] * v + K[3];
135 double B = K[1] * v + K[2] * u + K[4];
136
137 double theta = atan2(B, A); // Angle between the tangent and the u axis.
138 if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
139 theta += M_PI;
140 }
141 return (theta);
142}
143
150{
151 vpMeSite p_me;
152 vpImagePoint iP;
153 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
154 p_me = *it;
155 // (i,j) frame used for vpMESite
156 iP.set_ij(p_me.ifloat, p_me.jfloat);
157 p_me.alpha = computeTheta(iP);
158 *it = p_me;
159 }
160}
161
170{
171 // Two versions are available. If you change from one version to the other
172 // one, do not forget to change also the reciprocical function
173 // computeAngleOnEllipse() just below and, for a correct display of an arc
174 // of ellipse, adapt vpMeEllipse::display() below and
175 // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
176 // so that the two extremities of the arc are correctly shown.
177
178 // Version that gives a regular angular sampling on the ellipse, so less
179 // points at its extremities
180 /*
181 double co = cos(angle);
182 double si = sin(angle);
183 double coef = a * b / sqrt(b * b * co * co + a * a * si * si);
184 double u = co * coef;
185 double v = si * coef;
186 iP.set_u(uc + ce * u - se * v);
187 iP.set_v(vc + se * u + ce * v);
188 */
189
190 // Version from "the two concentric circles" method that gives more points
191 // at the ellipse extremities for a regular angle sampling. It is better to
192 // display an ellipse, not necessarily to track it
193
194 // (u,v) are the coordinates on the canonical centered ellipse;
195 double u = a * cos(angle);
196 double v = b * sin(angle);
197 // a rotation of e and a translation by (uc,vc) are done
198 // to get the coordinates of the point on the shifted ellipse
199 iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v);
200}
201
208{
209 // Two versions are available. If you change from one version to the other
210 // one, do not forget to change also the reciprocical function
211 // computePointOnEllipse() just above. Adapt also the display; see comment
212 // at the beginning of computePointOnEllipse()
213
214 // Regular angle smapling method
215 /*
216 double du = pt.get_u() - uc;
217 double dv = pt.get_v() - vc;
218 double ang = atan2(dv,du) - e;
219 if (ang > M_PI) {
220 ang -= 2.0 * M_PI;
221 }
222 else if (ang < -M_PI) {
223 ang += 2.0 * M_PI;
224 }
225 */
226 // for the "two concentric circles method" starting from the previous one
227 // (just to remember the link between both methods:
228 // tan(theta_2cc) = a/b tan(theta_rs))
229 /*
230 double co = cos(ang);
231 double si = sin(ang);
232 double coeff = 1.0/sqrt(b*b*co*co+a*a*si*si);
233 si *= a*coeff;
234 co *= b*coeff;
235 ang = atan2(si,co);
236 */
237 // For the "two concentric circles" method starting from scratch
238 double du = pt.get_u() - m_uc;
239 double dv = pt.get_v() - m_vc;
240 double co = (du * ce + dv * se)/a;
241 double si = (- du * se + dv * ce)/b;
242 double angle = atan2(si,co);
243
244 return(angle);
245}
246
254{
255 double num = m_n20 - m_n02;
256 double d = num * num + 4.0 * m_n11 * m_n11; // always >= 0
257 if (d <= std::numeric_limits<double>::epsilon()) {
258 e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
259 ce = 1.0;
260 se = 0.0;
261 a = b = 2.0*sqrt(m_n20); // = sqrt(2.0*(n20+n02))
262 }
263 else { // real ellipse
264 e = atan2(2.0*m_n11, num)/2.0; // e in [-Pi/2 ; Pi/2]
265 ce = cos(e);
266 se = sin(e);
267
268 d = sqrt(d); // d in sqrt always >= 0
269 num = m_n20 + m_n02;
270 a = sqrt(2.0*(num + d)); // term in sqrt always > 0
271 b = sqrt(2.0*(num - d)); // term in sqrt always > 0
272 }
273}
274
282{
283 K[0] = m_n02;
284 K[1] = m_n20;
285 K[2] = -m_n11;
286 K[3] = m_n11 * m_vc - m_n02 * m_uc;
287 K[4] = m_n11 * m_uc - m_n20 * m_vc;
288 K[5] = m_n02 * m_uc * m_uc + m_n20 * m_vc * m_vc - 2.0 * m_n11 * m_uc * m_vc
289 + 4.0 * (m_n11 * m_n11 - m_n20 * m_n02);
290}
291
298{
299 m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se);
300 m_n11 = 0.25 * se * ce * (a * a - b * b);
301 m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce);
302}
303
314{
315 // Equations below from Chaumette PhD and TRO 2004 paper
316 double num = K[0] * K[1] - K[2] * K[2]; // > 0 for an ellipse
317 if (num <= 0) {
318 throw(vpException(vpException::fatalError, "The points do not belong to an ellipse!"));
319 }
320
321 m_uc = (K[2] * K[4] - K[1] * K[3]) / num;
322 m_vc = (K[2] * K[3] - K[0] * K[4]) / num;
324
325 double d = (K[0] * m_uc * m_uc + K[1] * m_vc * m_vc + 2.0 * K[2] * m_uc * m_vc - K[5])
326 / (4.0 * num);
327 m_n20 = K[1] * d; // always > 0
328 m_n11 = -K[2] * d;
329 m_n02 = K[0] * d; // always > 0
330
332
333 // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
334 d = m_n02/K[0]; // fabs(K[0]) > 0
335 for (unsigned int i=0; i < 6; i++) {
336 K[i] *= d;
337 }
338 if (vpDEBUG_ENABLE(3)) {
340 }
341}
342
348{
349 std::cout << "K :" << K.t() << std::endl;
350 std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
351 std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 <<std::endl;
352 std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) <<std::endl;
353}
354
367void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
368{
369 // Warning: similar code in vpMbtMeEllipse::sample()
370 if (!me) {
371 throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
372 }
373 // Delete old lists
374 list.clear();
375 angle.clear();
376
377 int nbrows = static_cast<int>(I.getHeight());
378 int nbcols = static_cast<int>(I.getWidth());
379
380 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
381 std::cout << "In vpMeEllipse::sample: ";
382 std::cout << "function called with sample step = 0, set to 10 dg";
383 me->setSampleStep(10.0);
384 }
385 double incr = vpMath::rad(me->getSampleStep()); // angle increment
386 // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
387 m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
388#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
389 expecteddensity = static_cast<double>(m_expectedDensity);
390#endif
391
392 // starting angle for sampling
393 double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr)/2.0;
394 // sample positions
395 for (unsigned int i = 0; i < m_expectedDensity; i++) {
396 vpImagePoint iP;
397 computePointOnEllipse(ang,iP);
398 // If point is in the image, add to the sample list
399 // Check done in (i,j) frame)
400 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
402
403 double theta = computeTheta(iP);
404 vpMeSite pix;
405 // (i,j) frame used for vpMeSite
406 pix.init(iP.get_i(), iP.get_j(), theta);
409 list.push_back(pix);
410 angle.push_back(ang);
411 }
412 ang += incr;
413 }
414 if (!doNotTrack) {
416 }
417}
418
431{
432 if (!me) {
433 throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
434 }
435 unsigned int nb_pts_added = 0;
436 int nbrows = static_cast<int>(I.getHeight());
437 int nbcols = static_cast<int>(I.getWidth());
438
439 unsigned int memory_range = me->getRange();
440 me->setRange(2);
441 double memory_mu1 = me->getMu1();
442 me->setMu1(0.5);
443 double memory_mu2 = me->getMu2();
444 me->setMu2(0.5);
445
446 double incr = vpMath::rad(me->getSampleStep());
447 // Detect holes and try to complete them
448 // FC : Currently only one point is looked at the middle of each hole
449 // (to avoid multiple insertions that are time consuming).
450 // A different choice could be done.
451 std::list<double>::iterator angleList = angle.begin();
452 double ang = *angleList;
453 for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
454 ++angleList;
455 ++meList;
456 double nextang = *angleList;
457 // The minimal size of a hole (1 point lost for sure).
458 // could be increased to reduce time processing
459 if ((nextang - ang) > 1.6 * incr) {
460 ang = (nextang + ang) / 2.0; // mid angle
461 vpImagePoint iP;
462 computePointOnEllipse(ang,iP);
463 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
464 double theta = computeTheta(iP);
465 vpMeSite pix;
466 pix.init(iP.get_i(), iP.get_j(), theta);
469 pix.track(I, me, false);
470 if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
471 nb_pts_added ++;
472 iP.set_ij(pix.ifloat, pix.jfloat);
473 double new_ang = computeAngleOnEllipse(iP);
474 if ((new_ang - ang) > M_PI) {
475 new_ang -= 2.0 * M_PI;
476 }
477 else if ((ang - new_ang) > M_PI) {
478 new_ang += 2.0 * M_PI;
479 }
480 list.insert(meList,pix);
481 angle.insert(angleList,new_ang);
482 if (vpDEBUG_ENABLE(3)) {
484 }
485 }
486 }
487 }
488 ang = nextang;
489 }
490
491 // Try to fill the first extremity: from alpha_min - incr to alpha1
492 unsigned int nbpts = static_cast<unsigned int>(floor((m_alphamin-alpha1)/incr));
493 ang = m_alphamin - incr;
494 for (unsigned int i = 0; i < nbpts; i++) {
495 vpImagePoint iP;
496 computePointOnEllipse(ang ,iP);
497 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
498 double theta = computeTheta(iP);
499 vpMeSite pix;
500 pix.init(iP.get_i(), iP.get_j(), theta);
503 pix.track(I, me, false);
504 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
505 nb_pts_added ++;
506 iP.set_ij(pix.ifloat, pix.jfloat);
507 double new_ang = computeAngleOnEllipse(iP);
508 if ((new_ang - ang) > M_PI) {
509 new_ang -= 2.0 * M_PI;
510 }
511 else if ((ang - new_ang) > M_PI) {
512 new_ang += 2.0 * M_PI;
513 }
514 list.push_front(pix);
515 angle.push_front(new_ang);
516 if (vpDEBUG_ENABLE(3)) {
518 }
519 }
520 }
521 ang -= incr;
522 }
523
524 // Try to fill the second extremity: from alphamax + incr to alpha2
525 nbpts = static_cast<unsigned int>(floor((alpha2-m_alphamax)/incr));
526 ang = m_alphamax + incr;
527 for (unsigned int i = 0; i < nbpts; i++) {
528 vpImagePoint iP;
529 computePointOnEllipse(ang,iP);
530 if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
531 double theta = computeTheta(iP);
532 vpMeSite pix;
533 pix.init(iP.get_i(), iP.get_j(), theta);
536 pix.track(I, me, false);
537 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
538 nb_pts_added ++;
539 iP.set_ij(pix.ifloat, pix.jfloat);
540 double new_ang = computeAngleOnEllipse(iP);
541 if ((new_ang - ang) > M_PI) {
542 new_ang -= 2.0 * M_PI;
543 }
544 else if ((ang - new_ang) > M_PI) {
545 new_ang += 2.0 * M_PI;
546 }
547 list.push_back(pix);
548 angle.push_back(new_ang);
549 if (vpDEBUG_ENABLE(3)) {
551 }
552 }
553 }
554 ang += incr;
555 }
556 me->setRange(memory_range);
557 me->setMu1(memory_mu1);
558 me->setMu2(memory_mu2);
559
560 if (vpDEBUG_ENABLE(3)) {
561 printf("In plugHoles(): nb of added points : %d\n", nb_pts_added);
562 }
563 return nb_pts_added;
564}
565
573void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
574{
575 // Homogeneous system A x = 0 ; x is the nullspace of A
576 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
577 // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
578
579 // It would be a bad idea to solve the same system using A x = b where
580 // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
581 // cannot consider the case where the origin belongs to the ellipse.
582 // Another possibility would be to consider K0+K1=1 which is always valid,
583 // leading to the system A x = b where
584 // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
585
586 double um = I.getWidth() / 2.;
587 double vm = I.getHeight() / 2.;
588 unsigned int n = static_cast<unsigned int>(iP.size());
589
590 vpMatrix A(n, 6);
591
592 for (unsigned int k = 0; k < n; k++) {
593 // normalization so that (u,v) in [-1;1]
594 double u = (iP[k].get_u() - um) / um;
595 double v = (iP[k].get_v() - vm) / vm;
596 A[k][0] = u * u;
597 A[k][1] = v * v;
598 A[k][2] = 2.0 * u * v;
599 A[k][3] = 2.0 * u;
600 A[k][4] = 2.0 * v;
601 A[k][5] = 1.0;
602 }
603 vpMatrix KerA;
604 unsigned int dim = A.nullSpace(KerA, 1);
605 if (dim > 1) { // case with less than 5 independent points
606 // FC : should create a rankError exception
607 throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
608 }
609 // the term um*vm is for counterbalancing the bad conditioning of the
610 // inverse normalization below
611 for (unsigned int i = 0; i < 6 ; i++) K[i] = um * vm * KerA[i][0];
612
613 // Inverse normalization to go back to pixel values
614 K[0] /= um * um;
615 K[1] /= vm * vm;
616 K[2] /= um * vm;
617 K[3] = K[3]/um - K[0] * um - K[2] * vm;
618 K[4] = K[4]/vm - K[1] * vm - K[2] * um;
619 K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
620
622}
623
632{
633 // Homogeneous system A x = 0 ; x is the nullspace of A
634 // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
635 // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
636
637 // It would be a bad idea to solve the same system using A x = b where
638 // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
639 // cannot consider the case where the origin belongs to the ellipse.
640 // Another possibility would be to consider K0+K1=1 which is always valid,
641 // leading to the system A x = b where
642 // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
643
644 const unsigned int nos = numberOfSignal();
645
646 // Note that the (nos-k) last rows of A, xp and yp are not used.
647 // Hopefully, this is not an issue.
648
649 vpMatrix A(nos, 6);
650 // Useful to compute the weights in the robust estimation
651 vpColVector xp(nos), yp(nos);
652
653 unsigned int k = 0;
654 double um = I.getWidth() / 2.;
655 double vm = I.getHeight() / 2.;
656 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
657 vpMeSite p_me = *it;
658 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
659 // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
660 double u = (p_me.jfloat - um) / um;
661 double v = (p_me.ifloat - vm) / vm;
662 A[k][0] = u * u;
663 A[k][1] = v * v;
664 A[k][2] = 2.0 * u * v;
665 A[k][3] = 2.0 * u;
666 A[k][4] = 2.0 * v;
667 A[k][5] = 1.0;
668 // Useful to compute the weights in the robust estimation
669 xp[k] = p_me.jfloat;
670 yp[k] = p_me.ifloat;
671
672 k++;
673 }
674 }
675 if (k < 5) {
676 throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
677 }
678
679 vpRobust r;
680 // r.setThreshold(0.02); // Old version where this threshold was highly
681 // sensitive since the residues do not represent the Euclidean distance
682 // from the point to the ellipse
683 r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance
684 vpColVector w(k);
685 w = 1.0;
686 unsigned int iter = 0;
687 double var = 1.0;
688 vpColVector Kprev(6);
689 vpMatrix DA(k, 6);
690 vpMatrix KerDA;
691
692 // stop after 4 it or if variation of K between 2 iterations is more than 0.1 %
693 while ((iter < 4) && (var > 0.001)) {
694 for (unsigned int i = 0; i < k ; i++) {
695 for (unsigned int j = 0; j < 6 ; j++) {
696 DA[i][j] = w[i] * A[i][j];
697 }
698 }
699 unsigned int dim = DA.nullSpace(KerDA, 1);
700 if (dim > 1) { // case with less than 5 independent points
701 // FC : should create a rankError exception
702 throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
703 }
704
705 for (unsigned int i=0; i<6 ; i++) K[i] = KerDA[i][0]; // norm(K) = 1
706 var = (K-Kprev).frobeniusNorm();
707 Kprev = K;
708 // the term um*vm is for counterbalancing the bad conditioning of the
709 // inverse normalization just below
710 K *= (um * vm);
711 // vpColVector residu(k); // old version for considering the algebraic distance
712 // residu = A * K;
713 // Better version considering the geometric distance
714 // Inverse normalization to go back to pixels
715 K[0] /= um * um;
716 K[1] /= vm * vm;
717 K[2] /= um * vm;
718 K[3] = K[3]/um - K[0] * um - K[2] * vm;
719 K[4] = K[4]/vm - K[1] * vm - K[2] * um;
720 K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
721 getParameters(); // since a, b, and e are used just after
722
723 vpColVector residu(k);
724 for (unsigned int i=0; i < k ; i++) {
725 vpImagePoint ip1, ip2;
726 ip1.set_uv(xp[i],yp[i]);
727 double ang = computeAngleOnEllipse(ip1);
728 computePointOnEllipse(ang, ip2);
729 // residu = 0 if point is exactly on the ellipse, not otherwise
730 residu[i] = vpImagePoint::distance(ip1, ip2);
731 }
732 // end of new version
733 r.MEstimator(vpRobust::TUKEY, residu, w);
734 iter++;
735 }
736 /* FC : for old version with algebraic distance
737 // Inverse normalization to go back to pixels
738 K[0] /= um * um;
739 K[1] /= vm * vm;
740 K[2] /= um * vm;
741 K[3] = K[3]/um - K[0] * um - K[2] * vm;
742 K[4] = K[4]/vm - K[1] * vm - K[2] * um;
743 K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
744 getParameters();
745 */
746 double previous_ang = - 4.0 * M_PI;
747 double incr = vpMath::rad(me->getSampleStep());
748 // Remove bad points, too near points, and outliers from the lists
749 k = m_numberOfGoodPoints = 0;
750 std::list<double>::iterator angleList = angle.begin();
751 for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
752 vpMeSite p_me = *meList;
753 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
754 if (w[k] > thresholdWeight) { // inlier
755 // Management of the angle to keep only the points in the interval
756 // [alpha1 ; alpha2]
757 double ang = *angleList;
758 vpImagePoint iP;
759 iP.set_ij(p_me.ifloat,p_me.jfloat);
760 double new_ang = computeAngleOnEllipse(iP);
761 if ((new_ang - ang) > M_PI) {
762 new_ang -= 2.0 * M_PI;
763 }
764 else if ((ang - new_ang) > M_PI) {
765 new_ang += 2.0 * M_PI;
766 }
767 if ((new_ang >= alpha1) && (new_ang <= alpha2) ) {
768 // good point if not too near from the previous one in the list
769 // if so, udate of its angle
770 if ((new_ang - previous_ang) >= (0.6 * incr)) {
771 *angleList = previous_ang = new_ang;
773 ++meList;
774 ++angleList;
775 if (vpDEBUG_ENABLE(3)) {
777 printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n",
778 vpMath::deg(new_ang), iP.get_i(), iP.get_j());
779 }
780 }
781 else {
782 if (vpDEBUG_ENABLE(3)) {
784 printf("too near : angle %lf, i %.0f , j : %0.f\n",
785 vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
786 }
787 meList = list.erase(meList);
788 angleList = angle.erase(angleList);
789 }
790 }
791 else { // point not in the interval [alpha1 ; alpha2]
792 if (vpDEBUG_ENABLE(3)) {
794 printf("not in interval: angle : %lf, i %.0f , j : %0.f\n",
795 vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
796 }
797 meList = list.erase(meList);
798 angleList = angle.erase(angleList);
799 }
800 }
801 else { // outlier
802 if (vpDEBUG_ENABLE(3)) {
803 vpImagePoint iP;
804 iP.set_ij(p_me.ifloat,p_me.jfloat);
805 printf("point %d outlier i : %.0f , j : %0.f, poids : %lf\n",
806 k, p_me.ifloat, p_me.jfloat, w[k]);
808 }
809 meList = list.erase(meList);
810 angleList = angle.erase(angleList);
811 }
812 k++;
813 }
814 else { // points not selected as me
815 meList = list.erase(meList);
816 angleList = angle.erase(angleList);
817 if (vpDEBUG_ENABLE(3)) {
818 vpImagePoint iP;
819 iP.set_ij(p_me.ifloat,p_me.jfloat);
820 printf("point not me i : %.0f , j : %0.f\n", p_me.ifloat, p_me.jfloat);
822 }
823 }
824 }
825 // set extremities of the angle list
826 m_alphamin = angle.front();
827 m_alphamax = angle.back();
828
829 if (vpDEBUG_ENABLE(3)) {
830 printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
831 printf("dans leastSquareRobust : nb pts ok = %d \n", m_numberOfGoodPoints);
832 }
833}
834
845{
846 vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
847}
848
862{
863 const unsigned int n = 5;
864 std::vector<vpImagePoint> iP(n);
865 m_trackArc = trackArc;
866
867 if (m_trackArc) {
868 std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
869 }
870 for (unsigned int k = 0; k < n; k++) {
871 std::cout << "Click point " << k + 1 << "/" << n << " on the ellipse " << std::endl;
872 vpDisplay::getClick(I, iP[k], true);
875 std::cout << iP[k] << std::endl;
876 }
877 initTracking(I, iP, trackArc);
878}
879
896void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP, bool trackArc)
897{
898 m_trackArc = trackArc;
899 // useful for sample(I):
900 leastSquare(I, iP);
901 if (trackArc) {
902 // useful for track(I):
903 iP1 = iP.front();
904 iP2 = iP.back();
905 // useful for sample(I):
908 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
909 alpha2 += 2.0 * M_PI;
910 }
911 }
912 else {
913 alpha1 = 0.0;
914 alpha2 = 2 * M_PI;
915 // useful for track(I):
916 vpImagePoint ip;
918 iP1 = iP2 = ip;
919 }
920
921 sample(I);
922 track(I);
925}
926
942{
943 if (pt1 != NULL && pt2 != NULL) {
944 m_trackArc = true;
945 }
946 // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
947 m_uc = param[0];
948 m_vc = param[1];
949 m_n20 = param[2];
950 m_n11 = param[3];
951 m_n02 = param[4];
954
955 if (m_trackArc) {
958 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
959 alpha2 += 2.0 * M_PI;
960 }
961 // useful for track(I)
962 iP1 = *pt1;
963 iP2 = *pt2;
964 }
965 else {
966 alpha1 = 0.0;
967 alpha2 = 2.0 * M_PI;
968 // useful for track(I)
969 vpImagePoint ip;
971 iP1 = iP2 = ip;
972 }
973 // useful for display(I) so useless if no display before track(I)
975
976 sample(I);
977 track(I);
980}
981
988{
990
991 // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
992 if (m_trackArc) {
995 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
996 alpha2 += 2.0 * M_PI;
997 }
998 }
999 // Compute the ellipse parameters from the tracked points and manage the lists
1001 if (vpDEBUG_ENABLE(3)) {
1002 printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1005 }
1006
1007 // Try adding points at the extremities and in the holes if needed
1008 if (m_numberOfGoodPoints < m_expectedDensity) { // at least one point has been lost
1009 if (plugHoles(I) > 0) {
1010 leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists
1011 }
1012 }
1013 if (vpDEBUG_ENABLE(3)) {
1014 printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1017 }
1018
1019 // resample if needed in case of unsufficient number of points or
1020 // bad repartition
1021 // FC : (thresholds ad hoc and sensitive...)
1022 if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5) || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
1023 if (vpDEBUG_ENABLE(3)) {
1024 printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
1025 printf("A click to continue \n");
1029 }
1030
1031 sample(I);
1034 if (vpDEBUG_ENABLE(3)) {
1035 printf("nb of Good points %u, density %d, alphamax %lf, alphamin %lf\n",
1037 }
1038
1039 // Stop in case of failure after resample
1040 if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5) || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
1041 // FC : dimensionError pas vraiment le bon terme...
1042 throw(vpException(vpException::dimensionError, "Impossible to track the ellipse"));
1043 }
1044 }
1045
1046 if (vpDEBUG_ENABLE(3)) {
1048 }
1049 // remet a jour l'angle delta pour chaque vpMeSite de la liste
1050 updateTheta();
1051 // not in getParameters since computed only once for each image
1052 m00 = M_PI * a * b;
1053
1054 // Useful only for tracking an arc of ellipse, but done to give them a value
1057
1058#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1059 computeMoments();
1060#endif
1061
1062 if (vpDEBUG_ENABLE(3)) {
1066 }
1067}
1068
1069
1070#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1074void vpMeEllipse::computeMoments()
1075{
1076 // m00 = M_PI * a * b; // moved in track(I)
1077
1078 m10 = m_uc * m00;
1079 m01 = m_vc * m00;
1080
1081 mu20 = m_n20 * m00;
1082 mu11 = m_n11 * m00;
1083 mu02 = m_n02 * m00;
1084
1085 m20 = mu20 + m10 * m_uc;
1086 m11 = mu11 + m10 * m_vc;
1087 m02 = mu02 + m01 * m_vc;
1088}
1089
1104 double a_p, double b_p, double e_p, double alpha1_p, double alpha2_p)
1105{
1106 m_trackArc = true;
1107 // useful for sample(I): uc, vc, a, b, e, Ki, alpha1, alpha2
1108 m_uc = center_p.get_u();
1109 m_vc = center_p.get_v();
1110 a = a_p;
1111 b = b_p;
1112 e = e_p;
1113 ce = cos(e);
1114 se = sin(e);
1117
1118 alpha1 = alpha1_p;
1119 alpha2 = alpha2_p;
1120 if (alpha2 < alpha1) {
1121 alpha2 += 2 * M_PI;
1122 }
1123 // useful for track(I)
1124 vpImagePoint ip;
1126 iP1 = ip;
1128 iP2 = ip;
1129 // currently not used after, but done to be complete
1130 // would be needed for displaying the ellipse here
1131 iPc = center_p;
1132
1133 sample(I);
1134 track(I);
1137}
1138
1152{
1153 std::vector<vpImagePoint> v_iP(n);
1154
1155 for (unsigned int i = 0; i < n; i++) {
1156 v_iP[i] = iP[i];
1157 }
1158 initTracking(I, v_iP);
1159}
1160
1164void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, unsigned *i, unsigned *j)
1165{
1166 std::vector<vpImagePoint> v_iP(n);
1167
1168 for (unsigned int k=0; k < n; k++) {
1169 v_iP[k].set_ij(i[k],j[k]);
1170 }
1171 initTracking(I, v_iP);
1172}
1173#endif // Deprecated
1174
1198 const vpImagePoint &center, const double &A, const double &B, const double &E,
1199 const double &smallalpha, const double &highalpha,
1200 const vpColor &color, unsigned int thickness)
1201{
1202 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1203}
1204
1229 const vpImagePoint &center, const double &A, const double &B, const double &E,
1230 const double &smallalpha, const double &highalpha,
1231 const vpColor &color, unsigned int thickness)
1232{
1233 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1234}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
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 emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
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
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
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
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:110
static int round(double x)
Definition: vpMath.h:247
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6335
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:98
double m_n20
Second order centered and normalized moments.
Definition: vpMeEllipse.h:429
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
Definition: vpMeEllipse.h:437
double m02
Definition: vpMeEllipse.h:405
double a
is the semimajor axis of the ellipse.
Definition: vpMeEllipse.h:365
double se
Value of sin(e).
Definition: vpMeEllipse.h:394
double mu11
Second order centered moments.
Definition: vpMeEllipse.h:401
double m10
First order raw moments.
Definition: vpMeEllipse.h:403
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
vpImagePoint iP2
Definition: vpMeEllipse.h:381
void leastSquareRobust(const vpImage< unsigned char > &I)
vpColVector K
Definition: vpMeEllipse.h:361
void display(const vpImage< unsigned char > &I, vpColor col)
double m_vc
Value of v coordinate of iPc.
Definition: vpMeEllipse.h:427
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
Definition: vpMeEllipse.h:433
vpImagePoint iPc
The coordinates of the ellipse center.
Definition: vpMeEllipse.h:363
double m20
Definition: vpMeEllipse.h:405
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 .
Definition: vpMeEllipse.h:396
double b
is the semiminor axis of the ellipse.
Definition: vpMeEllipse.h:367
void getParameters()
double expecteddensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:413
void printParameters() const
double m_uc
Value of u coordinate of iPc.
Definition: vpMeEllipse.h:425
double m01
Definition: vpMeEllipse.h:403
void computeAbeFromNij()
double ce
Value of cos(e).
Definition: vpMeEllipse.h:392
double m00
Ellipse area.
Definition: vpMeEllipse.h:398
double m_alphamin
Definition: vpMeEllipse.h:419
void initTracking(const vpImage< unsigned char > &I, bool trackArc=false)
double m11
Second order raw moments.
Definition: vpMeEllipse.h:405
double mu20
Definition: vpMeEllipse.h:401
double mu02
Definition: vpMeEllipse.h:401
bool m_trackArc
Track an arc of ellipse (true) or a complete one (false).
Definition: vpMeEllipse.h:435
void computePointOnEllipse(const double ang, vpImagePoint &iP)
double m_alphamax
Definition: vpMeEllipse.h:423
double computeTheta(const vpImagePoint &iP) const
double m_n02
Definition: vpMeEllipse.h:429
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:431
void updateTheta()
virtual ~vpMeEllipse()
double alpha2
Definition: vpMeEllipse.h:390
vpImagePoint iP1
Definition: vpMeEllipse.h:377
double m_n11
Definition: vpMeEllipse.h:429
void track(const vpImage< unsigned char > &I)
double alpha1
Definition: vpMeEllipse.h:385
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false)
double thresholdWeight
Threshold on the weights for the robust least square.
Definition: vpMeEllipse.h:409
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:72
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
double ifloat
Definition: vpMeSite.h:89
double alpha
Definition: vpMeSite.h:93
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:66
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void setMu1(const double &mu_1)
Definition: vpMe.h:241
void setSampleStep(const double &s)
Definition: vpMe.h:278
void setRange(const unsigned int &r)
Definition: vpMe.h:271
double getMu1() const
Definition: vpMe.h:155
double getMu2() const
Definition: vpMe.h:161
void setMu2(const double &mu_2)
Definition: vpMe.h:248
double getSampleStep() const
Definition: vpMe.h:285
unsigned int getRange() const
Definition: vpMe.h:179
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538