Visual Servoing Platform version 3.5.0
vpMeNurbs.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 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
44#include <cmath> // std::fabs
45#include <limits> // numeric_limits
46#include <stdlib.h>
47#include <visp3/core/vpImageConvert.h>
48#include <visp3/core/vpImageFilter.h>
49#include <visp3/core/vpImagePoint.h>
50#include <visp3/core/vpImageTools.h>
51#include <visp3/core/vpMath.h>
52#include <visp3/core/vpRect.h>
53#include <visp3/core/vpRobust.h>
54#include <visp3/core/vpTrackingException.h>
55#include <visp3/me/vpMe.h>
56#include <visp3/me/vpMeNurbs.h>
57#include <visp3/me/vpMeSite.h>
58#include <visp3/me/vpMeTracker.h>
59#ifdef VISP_HAVE_OPENCV
60#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
61//# include <opencv2/imgproc/imgproc.hpp>
62#include <opencv2/imgproc/imgproc_c.h>
63#else
64#include <cv.h>
65#endif
66#endif
67
68double computeDelta(double deltai, double deltaj);
69void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt);
70vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP);
71bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
72#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
73vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
74#endif
75
76// Compute the angle delta = arctan(deltai/deltaj)
77// and normalize it between 0 and pi
78double computeDelta(double deltai, double deltaj)
79{
80 double delta;
81 delta = atan2(deltai, deltaj);
82 delta -= M_PI / 2.0;
83 while (delta > M_PI) {
84 delta -= M_PI;
85 }
86 while (delta < 0) {
87 delta += M_PI;
88 }
89 return (delta);
90}
91
92// Check if the image point is in the image and not to close to
93// its edge to enable the computation of a convolution whith a mask.
94static bool outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
95{
96 return ((iP.get_i() < half + 1) || (iP.get_i() > (rows - half - 3)) || (iP.get_j() < half + 1) ||
97 (iP.get_j() > (cols - half - 3)));
98}
99
100// if iP is a edge point, it computes the angle corresponding to the
101// highest convolution result. the angle is between 0 an 179.
102// The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg
103// alpha angle) and the corresponding convolution result.
104void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt)
105{
106 int Iheight = (int)I.getHeight();
107 int Iwidth = (int)I.getWidth();
108 angle = 0.0;
109 convlt = 0.0;
110 for (int i = 0; i < 180; i++) {
111 double conv = 0.0;
112 unsigned int half;
113 half = (me->getMaskSize() - 1) >> 1;
114
115 if (outOfImage(iP, (int)half + me->getStrip(), Iheight, Iwidth)) {
116 conv = 0.0;
117 } else {
118 int index_mask;
119
120 if (me->getAngleStep() != 0)
121 index_mask = (int)(i / (double)me->getAngleStep());
122 else
123 throw(vpException(vpException::divideByZeroError, "angle step = 0"));
124
125 unsigned int ihalf = (unsigned int)(iP.get_i() - half);
126 unsigned int jhalf = (unsigned int)(iP.get_j() - half);
127 unsigned int a;
128 unsigned int b;
129 for (a = 0; a < me->getMaskSize(); a++) {
130 unsigned int ihalfa = ihalf + a;
131 for (b = 0; b < me->getMaskSize(); b++) {
132 conv += me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
133 }
134 }
135 }
136 conv = fabs(conv);
137 if (conv > convlt) {
138 convlt = conv;
139 angle = vpMath::rad(i);
140 angle += M_PI / 2;
141 while (angle > M_PI) {
142 angle -= M_PI;
143 }
144 while (angle < 0) {
145 angle += M_PI;
146 }
147 }
148 }
149}
150
151// Find the point belonging to the edge of the sub image which respects the
152// following hypotheses:
153//- the value of the pixel is upper than zero.
154//- the distantce between the point and iP is less than 4 pixels.
155// The function returns the nearest point of iP which respect the hypotheses
156// If no point is found the returned point is (-1,-1)
157vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP)
158{
159 double dist = 1e6;
160 double dist_1 = 1e6;
161 vpImagePoint index(-1, -1);
162 for (unsigned int i = 0; i <= Isub.getHeight(); i++) {
163 for (unsigned int j = 0; j <= Isub.getWidth(); j++) {
164 if (i == 0 || i == Isub.getHeight() - 1 || j == 0 || j == Isub.getWidth() - 1) {
165 if (Isub(i, j) > 0) {
167 if (dist <= 16 && dist < dist_1) {
168 dist_1 = dist;
169 index.set_ij(i, j);
170 }
171 }
172 }
173 }
174 }
175 return index;
176}
177
178#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
179// Check if the list of vpImagePoint contains a distant point of less tha 4
180// pixels from the center of the sub image (ie the point (15,15).
181vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
182{
183 ip_edges_list->front();
184 while (!ip_edges_list->outside()) {
185 vpImagePoint iP = ip_edges_list->value();
186 double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
187 if (dist <= 16) {
188 return true;
189 }
190 ip_edges_list->next();
191 }
192 return false;
193}
194#endif
195
196// Check if the list of vpImagePoint contains a distant point of less tha 4
197// pixels from the center of the sub image (ie the point (15,15).
198bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
199{
200 for (std::list<vpImagePoint>::const_iterator it = ip_edges_list->begin(); it != ip_edges_list->end(); ++it) {
201 vpImagePoint iP = *it;
202 double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
203 if (dist <= 16) {
204 return true;
205 }
206 }
207 return false;
208}
209
210/***************************************/
211
216 : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.),
217 cannyTh2(200.)
218{
219}
220
225 : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0),
226 enableCannyDetection(false), cannyTh1(100.), cannyTh2(200.)
227{
228 dist = menurbs.dist;
229 nbControlPoints = menurbs.nbControlPoints;
230 beginPtFound = menurbs.beginPtFound;
231 endPtFound = menurbs.endPtFound;
232 enableCannyDetection = menurbs.enableCannyDetection;
233 cannyTh1 = menurbs.cannyTh1;
234 cannyTh2 = menurbs.cannyTh2;
235}
236
241
249{
250 std::list<vpImagePoint> ptList;
251 vpImagePoint pt;
253
254 while (vpDisplay::getClick(I, pt, b)) {
255 if (b == vpMouseButton::button1) {
256 // std::cout<<pt<<std::endl;
257 ptList.push_back(pt);
260 }
261 if (b == vpMouseButton::button3)
262 break;
263 }
264 if (ptList.size() > 3)
265 initTracking(I, ptList);
266 else
267 throw(vpException(vpException::notInitialized, "Not enough points to initialize the Nurbs"));
268}
269
277void vpMeNurbs::initTracking(const vpImage<unsigned char> &I, const std::list<vpImagePoint> &ptList)
278{
279 nurbs.globalCurveInterp(ptList);
280
281 sample(I);
282
284 track(I);
285}
286
294void vpMeNurbs::sample(const vpImage<unsigned char> &I, bool doNotTrack)
295{
296 (void)doNotTrack;
297 int rows = (int)I.getHeight();
298 int cols = (int)I.getWidth();
299 double step = 1.0 / (double)me->getPointsToTrack();
300
301 // Delete old list
302 list.clear();
303
304 double u = 0.0;
305 vpImagePoint *pt = NULL;
306 vpImagePoint pt_1(-rows, -cols);
307 while (u <= 1.0) {
308 if (pt != NULL)
309 delete[] pt;
310 pt = nurbs.computeCurveDersPoint(u, 1);
311 double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
312
313 // If point is in the image, add to the sample list
314 if (!outOfImage(pt[0], 0, rows, cols) &&
316 vpMeSite pix; //= list.value();
317 pix.init(pt[0].get_i(), pt[0].get_j(), delta);
319
320 list.push_back(pix);
321 pt_1 = pt[0];
322 }
323 u = u + step;
324 }
325 if (pt != NULL)
326 delete[] pt;
327}
328
336{
337 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
338 vpMeSite s = *it; // current reference pixel
339
341 it = list.erase(it);
342 } else
343 ++it;
344 }
345}
346
352{
353 double u = 0.0;
354 double d = 1e6;
355 double d_1 = 1e6;
356 std::list<vpMeSite>::iterator it = list.begin();
357
358 vpImagePoint Cu;
359 vpImagePoint *der = NULL;
360 double step = 0.01;
361 while (u < 1 && it != list.end()) {
362 vpMeSite s = *it;
363 vpImagePoint pt(s.i, s.j);
364 while (d <= d_1 && u < 1) {
365 Cu = nurbs.computeCurvePoint(u);
366 d_1 = d;
367 d = vpImagePoint::distance(pt, Cu);
368 u += step;
369 }
370
371 u -= step;
372 if (der != NULL)
373 delete[] der;
374 der = nurbs.computeCurveDersPoint(u, 1);
375 // vpImagePoint toto(der[0].get_i(),der[0].get_j());
376 // vpDisplay::displayCross(I,toto,4,vpColor::red);
377
378 s.alpha = computeDelta(der[1].get_i(), der[1].get_j());
379 *it = s;
380 ++it;
381 d = 1e6;
382 d_1 = 1.5e6;
383 }
384 if (der != NULL)
385 delete[] der;
386}
387
396{
397 int rows = (int)I.getHeight();
398 int cols = (int)I.getWidth();
399
400 vpImagePoint *begin = NULL;
401 vpImagePoint *end = NULL;
402
403 begin = nurbs.computeCurveDersPoint(0.0, 1);
404 end = nurbs.computeCurveDersPoint(1.0, 1);
405
406 // Check if the two extremities are not to close to eachother.
407 double d = vpImagePoint::distance(begin[0], end[0]);
408 double threshold = 3 * me->getSampleStep();
409 double sample_step = me->getSampleStep();
410 vpImagePoint pt;
411 if (d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) {
412 vpMeSite P;
413
414 // Init vpMeSite
415 P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign);
417
418 // Set the range
419 unsigned int memory_range = me->getRange();
420 me->setRange(2);
421
422 // Point at the beginning of the list
423 bool beginPtAdded = false;
424 vpImagePoint pt_max = begin[0];
425 double angle = atan2(begin[1].get_i(), begin[1].get_j());
426 double co = vpMath::abs(cos(angle));
427 co = co * vpMath::sign(begin[1].get_j());
428 double si = vpMath::abs(sin(angle));
429 si = si * vpMath::sign(begin[1].get_i());
430 for (int i = 0; i < 3; i++) {
431 P.ifloat = P.ifloat - si * sample_step;
432 P.i = (int)P.ifloat;
433 P.jfloat = P.jfloat - co * sample_step;
434 P.j = (int)P.jfloat;
435 pt.set_ij(P.ifloat, P.jfloat);
436 if (vpImagePoint::distance(end[0], pt) < threshold)
437 break;
438 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
439 P.track(I, me, false);
440
442 list.push_front(P);
443 beginPtAdded = true;
444 pt_max = pt;
445 if (vpDEBUG_ENABLE(3)) {
447 }
448 } else {
449 if (vpDEBUG_ENABLE(3)) {
451 }
452 }
453 }
454 }
455
456 if (!beginPtAdded)
457 beginPtFound++;
458
459 P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
461
462 bool endPtAdded = false;
463 angle = atan2(end[1].get_i(), end[1].get_j());
464 co = vpMath::abs(cos(angle));
465 co = co * vpMath::sign(end[1].get_j());
466 si = vpMath::abs(sin(angle));
467 si = si * vpMath::sign(end[1].get_i());
468 for (int i = 0; i < 3; i++) {
469 P.ifloat = P.ifloat + si * sample_step;
470 P.i = (int)P.ifloat;
471 P.jfloat = P.jfloat + co * sample_step;
472 P.j = (int)P.jfloat;
473 pt.set_ij(P.ifloat, P.jfloat);
474 if (vpImagePoint::distance(begin[0], pt) < threshold)
475 break;
476 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
477 P.track(I, me, false);
478
480 list.push_back(P);
481 endPtAdded = true;
482 if (vpDEBUG_ENABLE(3)) {
484 }
485 } else {
486 if (vpDEBUG_ENABLE(3)) {
488 }
489 }
490 }
491 }
492 if (!endPtAdded)
493 endPtFound++;
494 me->setRange(memory_range);
495 } else {
496 list.pop_front();
497 }
498 /*if(begin != NULL)*/ delete[] begin;
499 /*if(end != NULL) */ delete[] end;
500}
501
513#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
515#else
517#endif
518{
519#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
520 vpMeSite pt = list.front();
521 vpImagePoint firstPoint(pt.ifloat, pt.jfloat);
522 pt = list.back();
523 vpImagePoint lastPoint(pt.ifloat, pt.jfloat);
524 if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
525 vpImagePoint *begin = NULL;
526 begin = nurbs.computeCurveDersPoint(0.0, 1);
527 vpImage<unsigned char> Isub(32, 32); // Sub image.
528 vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
529 vpRect rect(topLeft, 32, 32);
530
532
533 vpImageTools::crop(I, rect, Isub);
534
535 vpImagePoint lastPtInSubIm(begin[0]);
536 double u = 0.0;
537 double step = 0.0001;
538 // Find the point of the nurbs closest from the edge of the subImage and
539 // in the subImage.
540 while (inRectangle(lastPtInSubIm, rect) && u < 1) {
541 u += step;
542 lastPtInSubIm = nurbs.computeCurvePoint(u);
543 }
544
545 u -= step;
546 if (u > 0)
547 lastPtInSubIm = nurbs.computeCurvePoint(u);
548
549#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
550 vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
551#else
552 IplImage *Ip = NULL;
553 vpImageConvert::convert(Isub, Ip);
554
555 IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
556 cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
557
558 vpImageConvert::convert(dst, Isub);
559#endif
560
561 vpImagePoint firstBorder(-1, -1);
562
563 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
564
565 std::list<vpImagePoint> ip_edges_list;
566 if (firstBorder != vpImagePoint(-1, -1)) {
567 unsigned int dir;
568 double fi = firstBorder.get_i();
569 double fj = firstBorder.get_j();
570 double w = Isub.getWidth() - 1;
571 double h = Isub.getHeight() - 1;
572 // if (firstBorder.get_i() == 0) dir = 4;
573 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
574 dir = 4;
575 // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
576 else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
577 dir = 0;
578 // else if (firstBorder.get_j() == 0) dir = 2;
579 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
580 dir = 2;
581 // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
582 else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
583 dir = 6;
584 computeFreemanChainElement(Isub, firstBorder, dir);
585 unsigned int firstDir = dir;
586 ip_edges_list.push_back(firstBorder);
587 vpImagePoint border(firstBorder);
588 vpImagePoint dBorder;
589 do {
590 computeFreemanParameters(dir, dBorder);
591 border = border + dBorder;
592 vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
593
594 ip_edges_list.push_back(border);
595
596 computeFreemanChainElement(Isub, border, dir);
597 } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
598 }
599
600 if (findCenterPoint(&ip_edges_list)) {
601 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();
602 /*++it*/) {
603 vpMeSite s = *it;
604 vpImagePoint iP(s.ifloat, s.jfloat);
605 if (inRectangle(iP, rect))
606 it = list.erase(it);
607 else
608 break;
609 }
610
611 std::list<vpMeSite>::iterator itList = list.begin();
612 double convlt;
613 double delta = 0;
614 int nbr = 0;
615 std::list<vpMeSite> addedPt;
616 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
617 ++itEdges) {
618 vpMeSite s = *itList;
619 vpImagePoint iPtemp = *itEdges + topLeft;
620 vpMeSite pix;
621 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
622 dist = vpMeSite::sqrDistance(s, pix);
623 if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) {
624 bool exist = false;
625 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
626 dist = vpMeSite::sqrDistance(pix, *itAdd);
627 if (dist < vpMath::sqr(me->getSampleStep()) /*25*/)
628 exist = true;
629 }
630 if (!exist) {
631 findAngle(I, iPtemp, me, delta, convlt);
632 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
633 pix.setDisplay(selectDisplay);
634 --itList;
635 list.insert(itList, pix);
636 ++itList;
637 addedPt.push_front(pix);
638 nbr++;
639 }
640 }
641 }
642
643 unsigned int memory_range = me->getRange();
644 me->setRange(3);
645 std::list<vpMeSite>::iterator itList2 = list.begin();
646 for (int j = 0; j < nbr; j++) {
647 vpMeSite s = *itList2;
648 s.track(I, me, false);
649 *itList2 = s;
650 ++itList2;
651 }
652 me->setRange(memory_range);
653 }
654
655 /* if (begin != NULL) */ delete[] begin;
656 beginPtFound = 0;
657 }
658
659 if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
660 vpImagePoint *end = NULL;
661 end = nurbs.computeCurveDersPoint(1.0, 1);
662
663 vpImage<unsigned char> Isub(32, 32); // Sub image.
664 vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
665 vpRect rect(topLeft, 32, 32);
666
668
669 vpImageTools::crop(I, rect, Isub);
670
671 vpImagePoint lastPtInSubIm(end[0]);
672 double u = 1.0;
673 double step = 0.0001;
674 // Find the point of the nurbs closest from the edge of the subImage and
675 // in the subImage.
676 while (inRectangle(lastPtInSubIm, rect) && u > 0) {
677 u -= step;
678 lastPtInSubIm = nurbs.computeCurvePoint(u);
679 }
680
681 u += step;
682 if (u < 1.0)
683 lastPtInSubIm = nurbs.computeCurvePoint(u);
684
685#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
686 vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
687#else
688 IplImage *Ip = NULL;
689 vpImageConvert::convert(Isub, Ip);
690
691 IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
692 cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
693
694 vpImageConvert::convert(dst, Isub);
695#endif
696
697 vpImagePoint firstBorder(-1, -1);
698
699 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
700
701 std::list<vpImagePoint> ip_edges_list;
702 if (firstBorder != vpImagePoint(-1, -1)) {
703 unsigned int dir;
704 double fi = firstBorder.get_i();
705 double fj = firstBorder.get_j();
706 double w = Isub.getWidth() - 1;
707 double h = Isub.getHeight() - 1;
708 // if (firstBorder.get_i() == 0) dir = 4;
709 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
710 dir = 4;
711 // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
712 else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
713 dir = 0;
714 // else if (firstBorder.get_j() == 0) dir = 2;
715 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
716 dir = 2;
717 // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
718 else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
719 dir = 6;
720
721 computeFreemanChainElement(Isub, firstBorder, dir);
722 unsigned int firstDir = dir;
723 ip_edges_list.push_back(firstBorder);
724 vpImagePoint border(firstBorder);
725 vpImagePoint dBorder;
726 do {
727 computeFreemanParameters(dir, dBorder);
728 border = border + dBorder;
729 vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
730
731 ip_edges_list.push_back(border);
732
733 computeFreemanChainElement(Isub, border, dir);
734 } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
735 }
736
737 if (findCenterPoint(&ip_edges_list)) {
738 // list.end();
739 vpMeSite s;
740 while (true) //{//!list.outside())
741 {
742 s = list.back(); // list.value() ;
743 vpImagePoint iP(s.ifloat, s.jfloat);
744 if (inRectangle(iP, rect)) {
745 list.erase(list.end());
746 // list.end();
747 } else
748 break;
749 }
750
751 std::list<vpMeSite>::iterator itList = list.end();
752 --itList; // Move on the last element
753 double convlt;
754 double delta;
755 int nbr = 0;
756 std::list<vpMeSite> addedPt;
757 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
758 ++itEdges) {
759 s = *itList;
760 vpImagePoint iPtemp = *itEdges + topLeft;
761 vpMeSite pix;
762 pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
763 dist = vpMeSite::sqrDistance(s, pix);
764 if (dist >= vpMath::sqr(me->getSampleStep())) {
765 bool exist = false;
766 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
767 dist = vpMeSite::sqrDistance(pix, *itAdd);
768 if (dist < vpMath::sqr(me->getSampleStep()))
769 exist = true;
770 }
771 if (!exist) {
772 findAngle(I, iPtemp, me, delta, convlt);
773 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
774 pix.setDisplay(selectDisplay);
775 list.push_back(pix);
776 addedPt.push_back(pix);
777 nbr++;
778 }
779 }
780 }
781
782 unsigned int memory_range = me->getRange();
783 me->setRange(3);
784 std::list<vpMeSite>::iterator itList2 = list.end();
785 --itList2; // Move to the last element
786 for (int j = 0; j < nbr; j++) {
787 vpMeSite me_s = *itList2;
788 me_s.track(I, me, false);
789 *itList2 = me_s;
790 --itList2;
791 }
792 me->setRange(memory_range);
793 }
794
795 /* if (end != NULL) */ delete[] end;
796 endPtFound = 0;
797 }
798#else
799 vpTRACE("To use the canny detection, OpenCV has to be installed.");
800#endif
801}
802
814{
815 unsigned int n = numberOfSignal();
816 double nbPt = floor(dist / me->getSampleStep());
817
818 if ((double)n < 0.7 * nbPt) {
819 sample(I);
821 }
822}
823
832{
833 int rows = (int)I.getHeight();
834 int cols = (int)I.getWidth();
835 vpImagePoint *iP = NULL;
836
837 int n = (int)numberOfSignal();
838
839 // list.front();
840 std::list<vpMeSite>::iterator it = list.begin();
841 std::list<vpMeSite>::iterator itNext = list.begin();
842 ++itNext;
843
844 unsigned int range_tmp = me->getRange();
845 me->setRange(2);
846
847 while (itNext != list.end() && n <= me->getPointsToTrack()) {
848 vpMeSite s = *it; // current reference pixel
849 vpMeSite s_next = *itNext; // current reference pixel
850
851 double d = vpMeSite::sqrDistance(s, s_next);
852 if (d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600) {
853 vpImagePoint iP0(s.ifloat, s.jfloat);
854 vpImagePoint iPend(s_next.ifloat, s_next.jfloat);
855 vpImagePoint iP_1(s.ifloat, s.jfloat);
856
857 double u = 0.0;
858 double ubegin = 0.0;
859 double uend = 0.0;
860 double dmin1_1 = 1e6;
861 double dmin2_1 = 1e6;
862 while (u < 1) {
863 u += 0.01;
864 double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
865 double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
866
867 if (dmin1 < dmin1_1) {
868 dmin1_1 = dmin1;
869 ubegin = u;
870 }
871
872 if (dmin2 < dmin2_1) {
873 dmin2_1 = dmin2;
874 uend = u;
875 }
876 }
877 u = ubegin;
878
879 // if(( u != 1.0 || uend != 1.0)
880 if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
881 (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
882 iP = nurbs.computeCurveDersPoint(u, 1);
883
884 while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(me->getSampleStep()) && u < uend) {
885 u += 0.01;
886 /*if (iP!=NULL)*/ {
887 delete[] iP;
888 iP = NULL;
889 }
890 iP = nurbs.computeCurveDersPoint(u, 1);
891 if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(me->getSampleStep()) &&
892 !outOfImage(iP[0], 0, rows, cols)) {
893 double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
894 vpMeSite pix; //= list.value();
895 pix.init(iP[0].get_i(), iP[0].get_j(), delta);
897 pix.track(I, me, false);
898 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
899 list.insert(it, pix);
900 iP_1 = iP[0];
901 }
902 }
903 }
904 /*if (iP!=NULL)*/ {
905 delete[] iP;
906 iP = NULL;
907 }
908 }
909 }
910 ++it;
911 ++itNext;
912 }
913 me->setRange(range_tmp);
914}
915
922{
923#if 0
924 // Loop through list of sites to track
925 list.front();
926 while(!list.nextOutside())
927 {
928 vpMeSite s = list.value() ;//current reference pixel
929 vpMeSite s_next = list.nextValue() ;//current reference pixel
930
932 {
934
935 list.next();
936 list.modify(s_next);
937 if (!list.nextOutside()) list.next();
938 }
939 else
940 list.next() ;
941 }
942#endif
943 std::list<vpMeSite>::const_iterator it = list.begin();
944 std::list<vpMeSite>::iterator itNext = list.begin();
945 ++itNext;
946 for (; itNext != list.end();) {
947 vpMeSite s = *it; // current reference pixel
948 vpMeSite s_next = *itNext; // current reference pixel
949
950 if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
952
953 *itNext = s_next;
954 ++it;
955 ++itNext;
956 if (itNext != list.end()) {
957 ++it;
958 ++itNext;
959 }
960 } else {
961 ++it;
962 ++itNext;
963 }
964 }
965}
966
973{
974 // Tracking des vpMeSites
976
977 // Suppress points which are too close to each other
979
980 // Suppressions des points ejectes par le tracking
982
983 if (list.size() == 1)
984 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
985
986 // Recalcule les parametres
987 // nurbs.globalCurveInterp(list);
988 nurbs.globalCurveApprox(list, nbControlPoints);
989
990 // On resample localement
991 localReSample(I);
992
994 if (enableCannyDetection)
996
997 // nurbs.globalCurveInterp(list);
998 nurbs.globalCurveApprox(list, nbControlPoints);
999
1000 double u = 0.0;
1001 vpImagePoint pt;
1002 vpImagePoint pt_1;
1003 dist = 0;
1004 while (u <= 1.0) {
1005 pt = nurbs.computeCurvePoint(u);
1006 // if(u!=0)
1007 if (std::fabs(u) > std::numeric_limits<double>::epsilon())
1008 dist = dist + vpImagePoint::distance(pt, pt_1);
1009 pt_1 = pt;
1010 u = u + 0.01;
1011 }
1012
1013 updateDelta();
1014
1015 reSample(I);
1016}
1017
1029
1046bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
1047{
1048 vpImagePoint diP;
1049 vpImagePoint iPtemp;
1050 if (hasGoodLevel(I, iP)) {
1051 // get the point on the right of the point passed in
1052 computeFreemanParameters((element + 2) % 8, diP);
1053 iPtemp = iP + diP;
1054 if (hasGoodLevel(I, iPtemp)) {
1055 element = (element + 2) % 8; // turn right
1056 } else {
1057 computeFreemanParameters((element + 1) % 8, diP);
1058 iPtemp = iP + diP;
1059
1060 if (hasGoodLevel(I, iPtemp)) {
1061 element = (element + 1) % 8; // turn diag right
1062 } else {
1063 computeFreemanParameters(element, diP);
1064 iPtemp = iP + diP;
1065
1066 if (hasGoodLevel(I, iPtemp)) {
1067 // element = element; // keep same dir
1068 } else {
1069 computeFreemanParameters((element + 7) % 8, diP);
1070 iPtemp = iP + diP;
1071
1072 if (hasGoodLevel(I, iPtemp)) {
1073 element = (element + 7) % 8; // turn diag left
1074 } else {
1075 computeFreemanParameters((element + 6) % 8, diP);
1076 iPtemp = iP + diP;
1077
1078 if (hasGoodLevel(I, iPtemp)) {
1079 element = (element + 6) % 8; // turn left
1080 } else {
1081 computeFreemanParameters((element + 5) % 8, diP);
1082 iPtemp = iP + diP;
1083
1084 if (hasGoodLevel(I, iPtemp)) {
1085 element = (element + 5) % 8; // turn diag down
1086 } else {
1087 computeFreemanParameters((element + 4) % 8, diP);
1088 iPtemp = iP + diP;
1089
1090 if (hasGoodLevel(I, iPtemp)) {
1091 element = (element + 4) % 8; // turn down
1092 } else {
1093 computeFreemanParameters((element + 3) % 8, diP);
1094 iPtemp = iP + diP;
1095
1096 if (hasGoodLevel(I, iPtemp)) {
1097 element = (element + 3) % 8; // turn diag right down
1098 } else {
1099 // No neighbor with a good level
1100 //
1101 return false;
1102 }
1103 }
1104 }
1105 }
1106 }
1107 }
1108 }
1109 }
1110 } else {
1111 return false;
1112 }
1113 return true;
1114}
1115
1128bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1129{
1130 if (!isInImage(I, iP))
1131 return false;
1132
1133 if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) {
1134 return true;
1135 } else {
1136 return false;
1137 }
1138}
1139
1150bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1151{
1152 return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
1153}
1154
1172void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
1173{
1174 /*
1175 5 6 7
1176 \ | /
1177 \|/
1178 4 ------- 0
1179 /|\
1180 / | \
1181 3 2 1
1182 */
1183 switch (element) {
1184 case 0: // go right
1185 diP.set_ij(0, 1);
1186 break;
1187
1188 case 1: // go right top
1189 diP.set_ij(1, 1);
1190 break;
1191
1192 case 2: // go top
1193 diP.set_ij(1, 0);
1194 break;
1195
1196 case 3:
1197 diP.set_ij(1, -1);
1198 break;
1199
1200 case 4:
1201 diP.set_ij(0, -1);
1202 break;
1203
1204 case 5:
1205 diP.set_ij(-1, -1);
1206 break;
1207
1208 case 6:
1209 diP.set_ij(-1, 0);
1210 break;
1211
1212 case 7:
1213 diP.set_ij(-1, 1);
1214 break;
1215 }
1216}
1217
1227bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
1228{
1229 unsigned int height = I.getHeight();
1230 unsigned int width = I.getWidth();
1231 return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
1232}
1233
1245{
1246 double u = 0.0;
1247 vpImagePoint pt;
1248 while (u <= 1) {
1249 pt = n.computeCurvePoint(u);
1250 vpDisplay::displayCross(I, pt, 4, color);
1251 u += 0.01;
1252 }
1253}
1254
1266{
1267 double u = 0.0;
1268 vpImagePoint pt;
1269 while (u <= 1) {
1270 pt = n.computeCurvePoint(u);
1271 vpDisplay::displayCross(I, pt, 4, color);
1272 u += 0.01;
1273 }
1274}
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 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)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void canny(const vpImage< unsigned char > &I, vpImage< unsigned char > &Ic, unsigned int gaussianFilterSize, double thresholdCanny, unsigned int apertureSobel)
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
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
double get_i() const
Definition: vpImagePoint.h:203
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:305
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Provide simple list management.
Definition: vpList.h:114
void next(void)
position the current element on the next one
Definition: vpList.h:250
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:323
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:356
type & value(void)
return the value of the current element
Definition: vpList.h:269
static int() sign(double x)
static double rad(double deg)
Definition: vpMath.h:110
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static double sqr(double x)
Definition: vpMath.h:116
static Type abs(const Type &x)
Definition: vpMath.h:160
static int round(double x)
Definition: vpMath.h:247
Class that tracks in an image a edge defined by a Nurbs.
Definition: vpMeNurbs.h:131
void track(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:972
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:813
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:395
vpNurbs nurbs
The Nurbs which represents the tracked edge.
Definition: vpMeNurbs.h:138
virtual void sample(const vpImage< unsigned char > &image, bool doNotTrack=false)
Definition: vpMeNurbs.cpp:294
virtual ~vpMeNurbs()
Definition: vpMeNurbs.cpp:240
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:248
void updateDelta()
Definition: vpMeNurbs.cpp:351
void localReSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:831
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:516
void suppressPoints()
Definition: vpMeNurbs.cpp:335
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeNurbs.cpp:1028
void supressNearPoints()
Definition: vpMeNurbs.cpp:921
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:82
@ 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
int i
Definition: vpMeSite.h:87
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
static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
Definition: vpMeSite.h:234
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
Definition: vpMe.h:61
void setRange(const unsigned int &r)
Definition: vpMe.h:271
unsigned int getAngleStep() const
Definition: vpMe.h:114
vpMatrix * getMask() const
Definition: vpMe.h:120
int getPointsToTrack() const
Definition: vpMe.h:173
int getStrip() const
Definition: vpMe.h:185
unsigned int getMaskSize() const
Definition: vpMe.h:142
double getSampleStep() const
Definition: vpMe.h:285
unsigned int getRange() const
Definition: vpMe.h:179
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
Definition: vpNurbs.h:98
static void globalCurveInterp(std::vector< vpImagePoint > &l_crossingPoints, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:685
static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:85
static vpImagePoint * computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:260
static void globalCurveApprox(std::vector< vpImagePoint > &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:866
Defines a rectangle in the plane.
Definition: vpRect.h:80
Error that can be emited by the vpTracker class and its derivates.
#define vpTRACE
Definition: vpDebug.h:416
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538