Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMeNurbs.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 * Description:
31 * Moving edges.
32 */
33
39#include <cmath> // std::fabs
40#include <limits> // numeric_limits
41#include <stdlib.h>
42#include <visp3/core/vpImageConvert.h>
43#include <visp3/core/vpImageFilter.h>
44#include <visp3/core/vpImagePoint.h>
45#include <visp3/core/vpImageTools.h>
46#include <visp3/core/vpMath.h>
47#include <visp3/core/vpRect.h>
48#include <visp3/core/vpRobust.h>
49#include <visp3/core/vpTrackingException.h>
50#include <visp3/me/vpMe.h>
51#include <visp3/me/vpMeNurbs.h>
52#include <visp3/me/vpMeSite.h>
53#include <visp3/me/vpMeTracker.h>
54#if defined(HAVE_OPENCV_IMGPROC)
55#include <opencv2/imgproc/imgproc.hpp>
56#include <opencv2/imgproc/imgproc_c.h>
57#endif
58
59double computeDelta(double deltai, double deltaj);
60void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt);
61vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP);
62bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
63#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
64vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
65#endif
66
67// Compute the angle delta = arctan(deltai/deltaj)
68// and normalize it between 0 and pi
69double computeDelta(double deltai, double deltaj)
70{
71 double delta;
72 delta = atan2(deltai, deltaj);
73 delta -= M_PI / 2.0;
74 while (delta > M_PI) {
75 delta -= M_PI;
76 }
77 while (delta < 0) {
78 delta += M_PI;
79 }
80 return (delta);
81}
82
83// Check if the image point is in the image and not to close to
84// its edge to enable the computation of a convolution with a mask.
85static bool outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
86{
87 return ((iP.get_i() < half + 1) || (iP.get_i() > (rows - half - 3)) || (iP.get_j() < half + 1) ||
88 (iP.get_j() > (cols - half - 3)));
89}
90
91// if iP is a edge point, it computes the angle corresponding to the
92// highest convolution result. the angle is between 0 an 179.
93// The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg
94// alpha angle) and the corresponding convolution result.
95void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt)
96{
97 int Iheight = (int)I.getHeight();
98 int Iwidth = (int)I.getWidth();
99 angle = 0.0;
100 convlt = 0.0;
101 for (int i = 0; i < 180; i++) {
102 double conv = 0.0;
103 unsigned int half;
104 half = (me->getMaskSize() - 1) >> 1;
105
106 if (outOfImage(iP, (int)half + me->getStrip(), Iheight, Iwidth)) {
107 conv = 0.0;
108 }
109 else {
110 int index_mask;
111
112 if (me->getAngleStep() != 0)
113 index_mask = (int)(i / (double)me->getAngleStep());
114 else
115 throw(vpException(vpException::divideByZeroError, "angle step = 0"));
116
117 unsigned int ihalf = (unsigned int)(iP.get_i() - half);
118 unsigned int jhalf = (unsigned int)(iP.get_j() - half);
119 unsigned int a;
120 unsigned int b;
121 for (a = 0; a < me->getMaskSize(); a++) {
122 unsigned int ihalfa = ihalf + a;
123 for (b = 0; b < me->getMaskSize(); b++) {
124 conv += me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
125 }
126 }
127 }
128 conv = fabs(conv);
129 if (conv > convlt) {
130 convlt = conv;
131 angle = vpMath::rad(i);
132 angle += M_PI / 2;
133 while (angle > M_PI) {
134 angle -= M_PI;
135 }
136 while (angle < 0) {
137 angle += M_PI;
138 }
139 }
140 }
141}
142
143// Find the point belonging to the edge of the sub image which respects the
144// following hypotheses:
145//- the value of the pixel is upper than zero.
146//- the distantce between the point and iP is less than 4 pixels.
147// The function returns the nearest point of iP which respect the hypotheses
148// If no point is found the returned point is (-1,-1)
149vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP)
150{
151 double dist = 1e6;
152 double dist_1 = 1e6;
153 vpImagePoint index(-1, -1);
154 for (unsigned int i = 0; i <= Isub.getHeight(); i++) {
155 for (unsigned int j = 0; j <= Isub.getWidth(); j++) {
156 if (i == 0 || i == Isub.getHeight() - 1 || j == 0 || j == Isub.getWidth() - 1) {
157 if (Isub(i, j) > 0) {
159 if (dist <= 16 && dist < dist_1) {
160 dist_1 = dist;
161 index.set_ij(i, j);
162 }
163 }
164 }
165 }
166 }
167 return index;
168}
169
170#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
171// Check if the list of vpImagePoint contains a distant point of less tha 4
172// pixels from the center of the sub image (ie the point (15,15).
173vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
174{
175 ip_edges_list->front();
176 while (!ip_edges_list->outside()) {
177 vpImagePoint iP = ip_edges_list->value();
178 double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
179 if (dist <= 16) {
180 return true;
181 }
182 ip_edges_list->next();
183 }
184 return false;
185}
186#endif
187
188// Check if the list of vpImagePoint contains a distant point of less tha 4
189// pixels from the center of the sub image (ie the point (15,15).
190bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
191{
192 for (std::list<vpImagePoint>::const_iterator it = ip_edges_list->begin(); it != ip_edges_list->end(); ++it) {
193 vpImagePoint iP = *it;
194 double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
195 if (dist <= 16) {
196 return true;
197 }
198 }
199 return false;
200}
201
202/***************************************/
203
205 : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.),
206 cannyTh2(200.)
207{ }
208
210 : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0),
211 enableCannyDetection(false), cannyTh1(100.f), cannyTh2(200.f)
212{
213 dist = menurbs.dist;
214 nbControlPoints = menurbs.nbControlPoints;
215 beginPtFound = menurbs.beginPtFound;
216 endPtFound = menurbs.endPtFound;
217 enableCannyDetection = menurbs.enableCannyDetection;
218 cannyTh1 = menurbs.cannyTh1;
219 cannyTh2 = menurbs.cannyTh2;
220}
221
223
225{
226 std::list<vpImagePoint> ptList;
227 vpImagePoint pt;
229
230 while (vpDisplay::getClick(I, pt, b)) {
231 if (b == vpMouseButton::button1) {
232 // std::cout<<pt<<std::endl;
233 ptList.push_back(pt);
236 }
237 if (b == vpMouseButton::button3)
238 break;
239 }
240 if (ptList.size() > 3)
241 initTracking(I, ptList);
242 else
243 throw(vpException(vpException::notInitialized, "Not enough points to initialize the Nurbs"));
244}
245
246void vpMeNurbs::initTracking(const vpImage<unsigned char> &I, const std::list<vpImagePoint> &ptList)
247{
248 nurbs.globalCurveInterp(ptList);
249
250 sample(I);
251
253 track(I);
254}
255
256void vpMeNurbs::sample(const vpImage<unsigned char> &I, bool doNotTrack)
257{
258 (void)doNotTrack;
259 int rows = (int)I.getHeight();
260 int cols = (int)I.getWidth();
261 double step = 1.0 / (double)me->getPointsToTrack();
262
263 // Delete old list
264 list.clear();
265
266 double u = 0.0;
267 vpImagePoint *pt = NULL;
268 vpImagePoint pt_1(-rows, -cols);
269 while (u <= 1.0) {
270 if (pt != NULL)
271 delete[] pt;
272 pt = nurbs.computeCurveDersPoint(u, 1);
273 double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
274
275 // If point is in the image, add to the sample list
276 if (!outOfImage(pt[0], 0, rows, cols) &&
278 vpMeSite pix; //= list.value();
279 pix.init(pt[0].get_i(), pt[0].get_j(), delta);
281
282 list.push_back(pix);
283 pt_1 = pt[0];
284 }
285 u = u + step;
286 }
287 if (pt != NULL)
288 delete[] pt;
289}
290
292{
293 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
294 vpMeSite s = *it; // current reference pixel
295
297 it = list.erase(it);
298 }
299 else
300 ++it;
301 }
302}
303
305{
306 double u = 0.0;
307 double d = 1e6;
308 double d_1 = 1e6;
309 std::list<vpMeSite>::iterator it = list.begin();
310
311 vpImagePoint Cu;
312 vpImagePoint *der = NULL;
313 double step = 0.01;
314 while (u < 1 && it != list.end()) {
315 vpMeSite s = *it;
316 vpImagePoint pt(s.i, s.j);
317 while (d <= d_1 && u < 1) {
318 Cu = nurbs.computeCurvePoint(u);
319 d_1 = d;
320 d = vpImagePoint::distance(pt, Cu);
321 u += step;
322 }
323
324 u -= step;
325 if (der != NULL)
326 delete[] der;
327 der = nurbs.computeCurveDersPoint(u, 1);
328 // vpImagePoint toto(der[0].get_i(),der[0].get_j());
329 // vpDisplay::displayCross(I,toto,4,vpColor::red);
330
331 s.alpha = computeDelta(der[1].get_i(), der[1].get_j());
332 *it = s;
333 ++it;
334 d = 1e6;
335 d_1 = 1.5e6;
336 }
337 if (der != NULL)
338 delete[] der;
339}
340
342{
343 int rows = (int)I.getHeight();
344 int cols = (int)I.getWidth();
345
346 vpImagePoint *begin = NULL;
347 vpImagePoint *end = NULL;
348
349 begin = nurbs.computeCurveDersPoint(0.0, 1);
350 end = nurbs.computeCurveDersPoint(1.0, 1);
351
352 // Check if the two extremities are not to close to eachother.
353 double d = vpImagePoint::distance(begin[0], end[0]);
354 double threshold = 3 * me->getSampleStep();
355 double sample_step = me->getSampleStep();
356 vpImagePoint pt;
357 if (d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) {
358 vpMeSite P;
359
360 // Init vpMeSite
361 P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign);
363
364 // Set the range
365 unsigned int memory_range = me->getRange();
366 me->setRange(2);
367
368 // Point at the beginning of the list
369 bool beginPtAdded = false;
370 vpImagePoint pt_max = begin[0];
371 double angle = atan2(begin[1].get_i(), begin[1].get_j());
372 double co = vpMath::abs(cos(angle));
373 co = co * vpMath::sign(begin[1].get_j());
374 double si = vpMath::abs(sin(angle));
375 si = si * vpMath::sign(begin[1].get_i());
376 for (int i = 0; i < 3; i++) {
377 P.ifloat = P.ifloat - si * sample_step;
378 P.i = (int)P.ifloat;
379 P.jfloat = P.jfloat - co * sample_step;
380 P.j = (int)P.jfloat;
381 pt.set_ij(P.ifloat, P.jfloat);
382 if (vpImagePoint::distance(end[0], pt) < threshold)
383 break;
384 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
385 P.track(I, me, false);
386
388 list.push_front(P);
389 beginPtAdded = true;
390 pt_max = pt;
391 if (vpDEBUG_ENABLE(3)) {
393 }
394 }
395 else {
396 if (vpDEBUG_ENABLE(3)) {
398 }
399 }
400 }
401 }
402
403 if (!beginPtAdded)
404 beginPtFound++;
405
406 P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
408
409 bool endPtAdded = false;
410 angle = atan2(end[1].get_i(), end[1].get_j());
411 co = vpMath::abs(cos(angle));
412 co = co * vpMath::sign(end[1].get_j());
413 si = vpMath::abs(sin(angle));
414 si = si * vpMath::sign(end[1].get_i());
415 for (int i = 0; i < 3; i++) {
416 P.ifloat = P.ifloat + si * sample_step;
417 P.i = (int)P.ifloat;
418 P.jfloat = P.jfloat + co * sample_step;
419 P.j = (int)P.jfloat;
420 pt.set_ij(P.ifloat, P.jfloat);
421 if (vpImagePoint::distance(begin[0], pt) < threshold)
422 break;
423 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
424 P.track(I, me, false);
425
427 list.push_back(P);
428 endPtAdded = true;
429 if (vpDEBUG_ENABLE(3)) {
431 }
432 }
433 else {
434 if (vpDEBUG_ENABLE(3)) {
436 }
437 }
438 }
439 }
440 if (!endPtAdded)
441 endPtFound++;
442 me->setRange(memory_range);
443 }
444 else {
445 list.pop_front();
446 }
447 /*if(begin != NULL)*/ delete[] begin;
448 /*if(end != NULL) */ delete[] end;
449}
450
452{
453 vpMeSite pt = list.front();
454 vpImagePoint firstPoint(pt.ifloat, pt.jfloat);
455 pt = list.back();
456 vpImagePoint lastPoint(pt.ifloat, pt.jfloat);
457 if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
458 vpImagePoint *begin = NULL;
459 begin = nurbs.computeCurveDersPoint(0.0, 1);
460 vpImage<unsigned char> Isub(32, 32); // Sub image.
461 vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
462 vpRect rect(topLeft, 32, 32);
463
465
466 vpImageTools::crop(I, rect, Isub);
467
468 vpImagePoint lastPtInSubIm(begin[0]);
469 double u = 0.0;
470 double step = 0.0001;
471 // Find the point of the nurbs closest from the edge of the subImage and
472 // in the subImage.
473 while (inRectangle(lastPtInSubIm, rect) && u < 1) {
474 u += step;
475 lastPtInSubIm = nurbs.computeCurvePoint(u);
476 }
477
478 u -= step;
479 if (u > 0)
480 lastPtInSubIm = nurbs.computeCurvePoint(u);
481
482 vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
483
484 vpImagePoint firstBorder(-1, -1);
485
486 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
487
488 std::list<vpImagePoint> ip_edges_list;
489 if (firstBorder != vpImagePoint(-1, -1)) {
490 unsigned int dir;
491 double fi = static_cast<double>(firstBorder.get_i());
492 double fj = static_cast<double>(firstBorder.get_j());
493 double w = Isub.getWidth() - 1;
494 double h = Isub.getHeight() - 1;
495 // if (firstBorder.get_i() == 0) dir = 4;
496 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
497 dir = 4;
498 // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
499 else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
500 dir = 0;
501 // else if (firstBorder.get_j() == 0) dir = 2;
502 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
503 dir = 2;
504 // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
505 else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
506 dir = 6;
507 computeFreemanChainElement(Isub, firstBorder, dir);
508 unsigned int firstDir = dir;
509 ip_edges_list.push_back(firstBorder);
510 vpImagePoint border(firstBorder);
511 vpImagePoint dBorder;
512 do {
513 computeFreemanParameters(dir, dBorder);
514 border = border + dBorder;
515 vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
516
517 ip_edges_list.push_back(border);
518
519 computeFreemanChainElement(Isub, border, dir);
520 } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
521 }
522
523 if (findCenterPoint(&ip_edges_list)) {
524 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();
525 /*++it*/) {
526 vpMeSite s = *it;
527 vpImagePoint iP(s.ifloat, s.jfloat);
528 if (inRectangle(iP, rect))
529 it = list.erase(it);
530 else
531 break;
532 }
533
534 std::list<vpMeSite>::iterator itList = list.begin();
535 double convlt;
536 double delta = 0;
537 unsigned int nbr = 0;
538 std::list<vpMeSite> addedPt;
539 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
540 ++itEdges) {
541 vpMeSite s = *itList;
542 vpImagePoint iPtemp = *itEdges + topLeft;
543 vpMeSite pix;
544 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
545 dist = vpMeSite::sqrDistance(s, pix);
546 if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) {
547 bool exist = false;
548 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
549 dist = vpMeSite::sqrDistance(pix, *itAdd);
550 if (dist < vpMath::sqr(me->getSampleStep()) /*25*/)
551 exist = true;
552 }
553 if (!exist) {
554 findAngle(I, iPtemp, me, delta, convlt);
555 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
557 --itList;
558 list.insert(itList, pix);
559 ++itList;
560 addedPt.push_front(pix);
561 nbr++;
562 }
563 }
564 }
565
566 unsigned int memory_range = me->getRange();
567 me->setRange(3);
568 std::list<vpMeSite>::iterator itList2 = list.begin();
569 for (unsigned int j = 0; j < nbr; j++) {
570 vpMeSite s = *itList2;
571 s.track(I, me, false);
572 *itList2 = s;
573 ++itList2;
574 }
575 me->setRange(memory_range);
576 }
577
578 /* if (begin != NULL) */ delete[] begin;
579 beginPtFound = 0;
580 }
581
582 if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
583 vpImagePoint *end = NULL;
584 end = nurbs.computeCurveDersPoint(1.0, 1);
585
586 vpImage<unsigned char> Isub(32, 32); // Sub image.
587 vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
588 vpRect rect(topLeft, 32, 32);
589
591
592 vpImageTools::crop(I, rect, Isub);
593
594 vpImagePoint lastPtInSubIm(end[0]);
595 double u = 1.0;
596 double step = 0.0001;
597 // Find the point of the nurbs closest from the edge of the subImage and
598 // in the subImage.
599 while (inRectangle(lastPtInSubIm, rect) && u > 0) {
600 u -= step;
601 lastPtInSubIm = nurbs.computeCurvePoint(u);
602 }
603
604 u += step;
605 if (u < 1.0)
606 lastPtInSubIm = nurbs.computeCurvePoint(u);
607
608 vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
609
610 vpImagePoint firstBorder(-1, -1);
611
612 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
613
614 std::list<vpImagePoint> ip_edges_list;
615 if (firstBorder != vpImagePoint(-1, -1)) {
616 unsigned int dir;
617 double fi = firstBorder.get_i();
618 double fj = firstBorder.get_j();
619 double w = Isub.getWidth() - 1;
620 double h = Isub.getHeight() - 1;
621 // if (firstBorder.get_i() == 0) dir = 4;
622 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
623 dir = 4;
624 // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
625 else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
626 dir = 0;
627 // else if (firstBorder.get_j() == 0) dir = 2;
628 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
629 dir = 2;
630 // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
631 else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
632 dir = 6;
633
634 computeFreemanChainElement(Isub, firstBorder, dir);
635 unsigned int firstDir = dir;
636 ip_edges_list.push_back(firstBorder);
637 vpImagePoint border(firstBorder);
638 vpImagePoint dBorder;
639 do {
640 computeFreemanParameters(dir, dBorder);
641 border = border + dBorder;
642 vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
643
644 ip_edges_list.push_back(border);
645
646 computeFreemanChainElement(Isub, border, dir);
647 } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
648 }
649
650 if (findCenterPoint(&ip_edges_list)) {
651 // list.end();
652 vpMeSite s;
653
654 for (std::list<vpMeSite>::iterator it = list.begin(); it!=list.end(); ++it) {
655 s = *it;
656 vpImagePoint iP(s.ifloat, s.jfloat);
657 if (inRectangle(iP, rect)) {
658 list.erase(it);
659 }
660 else
661 break;
662 }
663
664 std::list<vpMeSite>::iterator itList = list.end();
665 --itList; // Move on the last element
666 double convlt;
667 double delta;
668 unsigned int nbr = 0;
669 std::list<vpMeSite> addedPt;
670 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
671 ++itEdges) {
672 s = *itList;
673 vpImagePoint iPtemp = *itEdges + topLeft;
674 vpMeSite pix;
675 pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
676 dist = vpMeSite::sqrDistance(s, pix);
677 if (dist >= vpMath::sqr(me->getSampleStep())) {
678 bool exist = false;
679 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
680 dist = vpMeSite::sqrDistance(pix, *itAdd);
681 if (dist < vpMath::sqr(me->getSampleStep()))
682 exist = true;
683 }
684 if (!exist) {
685 findAngle(I, iPtemp, me, delta, convlt);
686 pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
688 list.push_back(pix);
689 addedPt.push_back(pix);
690 nbr++;
691 }
692 }
693 }
694
695 unsigned int memory_range = me->getRange();
696 me->setRange(3);
697 std::list<vpMeSite>::iterator itList2 = list.end();
698 --itList2; // Move to the last element
699 for (unsigned int j = 0; j < nbr; j++) {
700 vpMeSite me_s = *itList2;
701 me_s.track(I, me, false);
702 *itList2 = me_s;
703 --itList2;
704 }
705 me->setRange(memory_range);
706 }
707
708 /* if (end != NULL) */ delete[] end;
709 endPtFound = 0;
710 }
711}
712
714{
715 unsigned int n = numberOfSignal();
716 double nbPt = floor(dist / me->getSampleStep());
717
718 if ((double)n < 0.7 * nbPt) {
719 sample(I);
721 }
722}
723
725{
726 int rows = (int)I.getHeight();
727 int cols = (int)I.getWidth();
728 vpImagePoint *iP = NULL;
729
730 int n = (int)numberOfSignal();
731
732 // list.front();
733 std::list<vpMeSite>::iterator it = list.begin();
734 std::list<vpMeSite>::iterator itNext = list.begin();
735 ++itNext;
736
737 unsigned int range_tmp = me->getRange();
738 me->setRange(2);
739
740 while (itNext != list.end() && n <= me->getPointsToTrack()) {
741 vpMeSite s = *it; // current reference pixel
742 vpMeSite s_next = *itNext; // current reference pixel
743
744 double d = vpMeSite::sqrDistance(s, s_next);
745 if (d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600) {
746 vpImagePoint iP0(s.ifloat, s.jfloat);
747 vpImagePoint iPend(s_next.ifloat, s_next.jfloat);
748 vpImagePoint iP_1(s.ifloat, s.jfloat);
749
750 double u = 0.0;
751 double ubegin = 0.0;
752 double uend = 0.0;
753 double dmin1_1 = 1e6;
754 double dmin2_1 = 1e6;
755 while (u < 1) {
756 u += 0.01;
757 double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
758 double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
759
760 if (dmin1 < dmin1_1) {
761 dmin1_1 = dmin1;
762 ubegin = u;
763 }
764
765 if (dmin2 < dmin2_1) {
766 dmin2_1 = dmin2;
767 uend = u;
768 }
769 }
770 u = ubegin;
771
772 // if(( u != 1.0 || uend != 1.0)
773 if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
774 (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
775 iP = nurbs.computeCurveDersPoint(u, 1);
776
777 while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(me->getSampleStep()) && u < uend) {
778 u += 0.01;
779 /*if (iP!=NULL)*/ {
780 delete[] iP;
781 iP = NULL;
782 }
783 iP = nurbs.computeCurveDersPoint(u, 1);
784 if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(me->getSampleStep()) &&
785 !outOfImage(iP[0], 0, rows, cols)) {
786 double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
787 vpMeSite pix; //= list.value();
788 pix.init(iP[0].get_i(), iP[0].get_j(), delta);
790 pix.track(I, me, false);
791 if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
792 list.insert(it, pix);
793 iP_1 = iP[0];
794 }
795 }
796 }
797 /*if (iP!=NULL)*/ {
798 delete[] iP;
799 iP = NULL;
800 }
801 }
802 }
803 ++it;
804 ++itNext;
805 }
806 me->setRange(range_tmp);
807}
808
810{
811#if 0
812 // Loop through list of sites to track
813 list.front();
814 while (!list.nextOutside()) {
815 vpMeSite s = list.value();//current reference pixel
816 vpMeSite s_next = list.nextValue();//current reference pixel
817
818 if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
820
821 list.next();
822 list.modify(s_next);
823 if (!list.nextOutside()) list.next();
824 }
825 else
826 list.next();
827 }
828#endif
829 std::list<vpMeSite>::const_iterator it = list.begin();
830 std::list<vpMeSite>::iterator itNext = list.begin();
831 ++itNext;
832 for (; itNext != list.end();) {
833 vpMeSite s = *it; // current reference pixel
834 vpMeSite s_next = *itNext; // current reference pixel
835
836 if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
838
839 *itNext = s_next;
840 ++it;
841 ++itNext;
842 if (itNext != list.end()) {
843 ++it;
844 ++itNext;
845 }
846 }
847 else {
848 ++it;
849 ++itNext;
850 }
851 }
852}
853
855{
856 // Tracking des vpMeSites
858
859 // Suppress points which are too close to each other
861
862 // Suppressions des points ejectes par le tracking
864
865 if (list.size() == 1)
866 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
867
868 // Recalcule les parametres
869 // nurbs.globalCurveInterp(list);
870 nurbs.globalCurveApprox(list, nbControlPoints);
871
872 // On resample localement
873 localReSample(I);
874
876 if (enableCannyDetection)
878
879 // nurbs.globalCurveInterp(list);
880 nurbs.globalCurveApprox(list, nbControlPoints);
881
882 double u = 0.0;
883 vpImagePoint pt;
884 vpImagePoint pt_1;
885 dist = 0;
886 while (u <= 1.0) {
887 pt = nurbs.computeCurvePoint(u);
888 // if(u!=0)
889 if (std::fabs(u) > std::numeric_limits<double>::epsilon())
890 dist = dist + vpImagePoint::distance(pt, pt_1);
891 pt_1 = pt;
892 u = u + 0.01;
893 }
894
895 updateDelta();
896
897 reSample(I);
898}
899
900void vpMeNurbs::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
901{
902 vpMeNurbs::display(I, nurbs, color, thickness);
903}
904
921bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
922{
923 vpImagePoint diP;
924 vpImagePoint iPtemp;
925 if (hasGoodLevel(I, iP)) {
926 // get the point on the right of the point passed in
927 computeFreemanParameters((element + 2) % 8, diP);
928 iPtemp = iP + diP;
929 if (hasGoodLevel(I, iPtemp)) {
930 element = (element + 2) % 8; // turn right
931 }
932 else {
933 computeFreemanParameters((element + 1) % 8, diP);
934 iPtemp = iP + diP;
935
936 if (hasGoodLevel(I, iPtemp)) {
937 element = (element + 1) % 8; // turn diag right
938 }
939 else {
940 computeFreemanParameters(element, diP);
941 iPtemp = iP + diP;
942
943 if (hasGoodLevel(I, iPtemp)) {
944 // element = element; // keep same dir
945 }
946 else {
947 computeFreemanParameters((element + 7) % 8, diP);
948 iPtemp = iP + diP;
949
950 if (hasGoodLevel(I, iPtemp)) {
951 element = (element + 7) % 8; // turn diag left
952 }
953 else {
954 computeFreemanParameters((element + 6) % 8, diP);
955 iPtemp = iP + diP;
956
957 if (hasGoodLevel(I, iPtemp)) {
958 element = (element + 6) % 8; // turn left
959 }
960 else {
961 computeFreemanParameters((element + 5) % 8, diP);
962 iPtemp = iP + diP;
963
964 if (hasGoodLevel(I, iPtemp)) {
965 element = (element + 5) % 8; // turn diag down
966 }
967 else {
968 computeFreemanParameters((element + 4) % 8, diP);
969 iPtemp = iP + diP;
970
971 if (hasGoodLevel(I, iPtemp)) {
972 element = (element + 4) % 8; // turn down
973 }
974 else {
975 computeFreemanParameters((element + 3) % 8, diP);
976 iPtemp = iP + diP;
977
978 if (hasGoodLevel(I, iPtemp)) {
979 element = (element + 3) % 8; // turn diag right down
980 }
981 else {
982 // No neighbor with a good level
983 //
984 return false;
985 }
986 }
987 }
988 }
989 }
990 }
991 }
992 }
993 }
994 else {
995 return false;
996 }
997 return true;
998}
999
1012bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1013{
1014 if (!isInImage(I, iP))
1015 return false;
1016
1017 if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) {
1018 return true;
1019 }
1020 else {
1021 return false;
1022 }
1023}
1024
1035bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1036{
1037 return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
1038}
1039
1057void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
1058{
1059 /*
1060 5 6 7
1061 \ | /
1062 \|/
1063 4 ------- 0
1064 /|\
1065 / | \
1066 3 2 1
1067 */
1068 switch (element) {
1069 case 0: // go right
1070 diP.set_ij(0, 1);
1071 break;
1072
1073 case 1: // go right top
1074 diP.set_ij(1, 1);
1075 break;
1076
1077 case 2: // go top
1078 diP.set_ij(1, 0);
1079 break;
1080
1081 case 3:
1082 diP.set_ij(1, -1);
1083 break;
1084
1085 case 4:
1086 diP.set_ij(0, -1);
1087 break;
1088
1089 case 5:
1090 diP.set_ij(-1, -1);
1091 break;
1092
1093 case 6:
1094 diP.set_ij(-1, 0);
1095 break;
1096
1097 case 7:
1098 diP.set_ij(-1, 1);
1099 break;
1100 }
1101}
1102
1112bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
1113{
1114 unsigned int height = I.getHeight();
1115 unsigned int width = I.getWidth();
1116 return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
1117}
1118
1119void vpMeNurbs::display(const vpImage<unsigned char> &I, vpNurbs &n, const vpColor &color, unsigned int thickness)
1120{
1121 double u = 0.0;
1122 vpImagePoint pt;
1123 while (u <= 1) {
1124 pt = n.computeCurvePoint(u);
1125 vpDisplay::displayCross(I, pt, 4, color, thickness);
1126 u += 0.01;
1127 }
1128}
1129
1130void vpMeNurbs::display(const vpImage<vpRGBa> &I, vpNurbs &n, const vpColor &color, unsigned int thickness)
1131{
1132 double u = 0.0;
1133 vpImagePoint pt;
1134 while (u <= 1) {
1135 pt = n.computeCurvePoint(u);
1136 vpDisplay::displayCross(I, pt, 4, color, thickness);
1137 u += 0.01;
1138 }
1139}
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
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 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 emitted by ViSP classes.
Definition vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
@ divideByZeroError
Division by zero.
Definition vpException.h:82
static void canny(const vpImage< unsigned char > &I, vpImage< unsigned char > &Ic, unsigned int gaussianFilterSize, float thresholdCanny, unsigned int apertureSobel)
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)
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
double get_i() const
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 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
Provide simple list management.
Definition vpList.h:108
void next(void)
position the current element on the next one
Definition vpList.h:244
void front(void)
Position the current element on the first element of the list.
Definition vpList.h:317
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition vpList.h:350
type & value(void)
return the value of the current element
Definition vpList.h:263
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static double sqr(double x)
Definition vpMath.h:124
static Type abs(const Type &x)
Definition vpMath.h:187
static int round(double x)
Definition vpMath.h:323
static int sign(double x)
Definition vpMath.h:342
Class that tracks in an image a edge defined by a Nurbs.
Definition vpMeNurbs.h:129
void track(const vpImage< unsigned char > &I)
void reSample(const vpImage< unsigned char > &I)
void seekExtremities(const vpImage< unsigned char > &I)
vpNurbs nurbs
The Nurbs which represents the tracked edge.
Definition vpMeNurbs.h:136
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false)
virtual ~vpMeNurbs()
void initTracking(const vpImage< unsigned char > &I)
void updateDelta()
void localReSample(const vpImage< unsigned char > &I)
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
void suppressPoints()
void supressNearPoints()
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition vpMeSite.h:65
int j
Coordinates along j of a site.
Definition vpMeSite.h:98
@ TOO_NEAR
Point removed because too near image borders.
Definition vpMeSite.h:90
@ 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
int i
Coordinate along i of a site.
Definition vpMeSite.h:96
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
static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
Definition vpMeSite.h:320
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
vpMe * me
Moving edges initialisation parameters.
Definition vpMeTracker.h:74
Definition vpMe.h:122
void setRange(const unsigned int &r)
Definition vpMe.h:383
unsigned int getAngleStep() const
Definition vpMe.h:208
vpMatrix * getMask() const
Definition vpMe.h:214
int getPointsToTrack() const
Definition vpMe.h:267
int getStrip() const
Definition vpMe.h:279
unsigned int getMaskSize() const
Definition vpMe.h:236
double getSampleStep() const
Definition vpMe.h:397
unsigned int getRange() const
Definition vpMe.h:273
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
Definition vpNurbs.h:93
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:465
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:55
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:155
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:594
Defines a rectangle in the plane.
Definition vpRect.h:76
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
#define vpDEBUG_ENABLE(level)
Definition vpDebug.h:533