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>
59 double computeDelta(
double deltai,
double deltaj);
62 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
66 double computeDelta(
double deltai,
double deltaj)
69 delta = atan2(deltai, deltaj);
71 while (delta > M_PI) {
82 static bool outOfImage(
const vpImagePoint &iP,
int half,
int rows,
int cols)
84 return ((iP.
get_i() < half + 1) || (iP.
get_i() > (rows - half - 3)) || (iP.
get_j() < half + 1) ||
85 (iP.
get_j() > (cols - half - 3)));
98 for (
int i = 0; i < 180; i++) {
103 if (outOfImage(iP, (
int)half + me->
getStrip(), Iheight, Iwidth)) {
114 unsigned int ihalf = (
unsigned int)(iP.
get_i() - half);
115 unsigned int jhalf = (
unsigned int)(iP.
get_j() - half);
119 unsigned int ihalfa = ihalf + a;
121 conv += me->
getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
130 while (angle > M_PI) {
151 for (
unsigned int i = 0; i <= Isub.
getHeight(); i++) {
152 for (
unsigned int j = 0; j <= Isub.
getWidth(); j++) {
154 if (Isub(i, j) > 0) {
156 if (dist <= 16 && dist < dist_1) {
169 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
171 for (std::list<vpImagePoint>::const_iterator it = ip_edges_list->begin(); it != ip_edges_list->end(); ++it) {
184 : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.),
189 :
vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0),
190 enableCannyDetection(false), cannyTh1(100.f), cannyTh2(200.f)
193 nbControlPoints = menurbs.nbControlPoints;
194 beginPtFound = menurbs.beginPtFound;
195 endPtFound = menurbs.endPtFound;
196 enableCannyDetection = menurbs.enableCannyDetection;
197 cannyTh1 = menurbs.cannyTh1;
198 cannyTh2 = menurbs.cannyTh2;
203 std::list<vpImagePoint> ptList;
210 ptList.push_back(pt);
217 if (ptList.size() > 3)
250 double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
256 pix.
init(pt[0].get_i(), pt[0].get_j(), delta);
270 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end();) {
286 std::list<vpMeSite>::iterator it =
list.begin();
291 while (u < 1 && it !=
list.end()) {
294 while (d <= d_1 && u < 1) {
308 s.
alpha = computeDelta(der[1].get_i(), der[1].get_j());
334 if (d > threshold ) {
338 P.
init(begin[0].get_i(), begin[0].get_j(), (
list.front()).alpha, 0, (
list.front()).mask_sign);
346 bool beginPtAdded =
false;
348 double angle = atan2(begin[1].get_i(), begin[1].get_j());
353 for (
int i = 0; i < 3; i++) {
383 P.
init(end[0].get_i(), end[0].get_j(), (
list.back()).alpha, 0, (
list.back()).mask_sign);
386 bool endPtAdded =
false;
387 angle = atan2(end[1].get_i(), end[1].get_j());
392 for (
int i = 0; i < 3; i++) {
434 if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
438 vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
439 vpRect rect(topLeft, 32, 32);
447 double step = 0.0001;
450 while (inRectangle(lastPtInSubIm, rect) && u < 1) {
463 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
465 std::list<vpImagePoint> ip_edges_list;
468 double fi =
static_cast<double>(firstBorder.
get_i());
469 double fj =
static_cast<double>(firstBorder.
get_j());
473 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
476 else if (std::fabs(fi - h) <= std::fabs(
vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
479 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
482 else if (std::fabs(fj - w) <= std::fabs(
vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
484 computeFreemanChainElement(Isub, firstBorder, dir);
485 unsigned int firstDir = dir;
486 ip_edges_list.push_back(firstBorder);
490 computeFreemanParameters(dir, dBorder);
491 border = border + dBorder;
494 ip_edges_list.push_back(border);
496 computeFreemanChainElement(Isub, border, dir);
497 }
while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
500 if (findCenterPoint(&ip_edges_list)) {
501 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end();
505 if (inRectangle(iP, rect))
511 std::list<vpMeSite>::iterator itList =
list.begin();
514 unsigned int nbr = 0;
515 std::list<vpMeSite> addedPt;
516 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
525 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
531 findAngle(I, iPtemp,
me, delta, convlt);
535 list.insert(itList, pix);
537 addedPt.push_front(pix);
545 std::list<vpMeSite>::iterator itList2 =
list.begin();
546 for (
unsigned int j = 0; j < nbr; j++) {
559 if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
564 vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
565 vpRect rect(topLeft, 32, 32);
573 double step = 0.0001;
576 while (inRectangle(lastPtInSubIm, rect) && u > 0) {
589 firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
591 std::list<vpImagePoint> ip_edges_list;
594 double fi = firstBorder.
get_i();
595 double fj = firstBorder.
get_j();
599 if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
602 else if (std::fabs(fi - h) <= std::fabs(
vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
605 else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
608 else if (std::fabs(fj - w) <= std::fabs(
vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
611 computeFreemanChainElement(Isub, firstBorder, dir);
612 unsigned int firstDir = dir;
613 ip_edges_list.push_back(firstBorder);
617 computeFreemanParameters(dir, dBorder);
618 border = border + dBorder;
621 ip_edges_list.push_back(border);
623 computeFreemanChainElement(Isub, border, dir);
624 }
while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
627 if (findCenterPoint(&ip_edges_list)) {
631 for (std::list<vpMeSite>::iterator it =
list.begin(); it!=
list.end(); ++it) {
634 if (inRectangle(iP, rect)) {
641 std::list<vpMeSite>::iterator itList =
list.end();
645 unsigned int nbr = 0;
646 std::list<vpMeSite> addedPt;
647 for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
656 for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
662 findAngle(I, iPtemp,
me, delta, convlt);
666 addedPt.push_back(pix);
674 std::list<vpMeSite>::iterator itList2 =
list.end();
676 for (
unsigned int j = 0; j < nbr; j++) {
695 if ((
double)n < 0.7 * nbPt) {
710 std::list<vpMeSite>::iterator it =
list.begin();
711 std::list<vpMeSite>::iterator itNext =
list.begin();
730 double dmin1_1 = 1e6;
731 double dmin2_1 = 1e6;
737 if (dmin1 < dmin1_1) {
742 if (dmin2 < dmin2_1) {
750 if ((std::fabs(u - 1.0) > std::fabs(
vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
751 (std::fabs(uend - 1.0) > std::fabs(
vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
763 double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
765 pix.
init(iP[0].get_i(), iP[0].get_j(), delta);
769 list.insert(it, pix);
791 while (!
list.nextOutside()) {
800 if (!
list.nextOutside())
list.next();
806 std::list<vpMeSite>::const_iterator it =
list.begin();
807 std::list<vpMeSite>::iterator itNext =
list.begin();
809 for (; itNext !=
list.end();) {
819 if (itNext !=
list.end()) {
842 if (
list.size() == 1)
853 if (enableCannyDetection)
866 if (std::fabs(u) > std::numeric_limits<double>::epsilon())
902 if (hasGoodLevel(I, iP)) {
904 computeFreemanParameters((element + 2) % 8, diP);
906 if (hasGoodLevel(I, iPtemp)) {
907 element = (element + 2) % 8;
910 computeFreemanParameters((element + 1) % 8, diP);
913 if (hasGoodLevel(I, iPtemp)) {
914 element = (element + 1) % 8;
917 computeFreemanParameters(element, diP);
920 if (hasGoodLevel(I, iPtemp)) {
924 computeFreemanParameters((element + 7) % 8, diP);
927 if (hasGoodLevel(I, iPtemp)) {
928 element = (element + 7) % 8;
931 computeFreemanParameters((element + 6) % 8, diP);
934 if (hasGoodLevel(I, iPtemp)) {
935 element = (element + 6) % 8;
938 computeFreemanParameters((element + 5) % 8, diP);
941 if (hasGoodLevel(I, iPtemp)) {
942 element = (element + 5) % 8;
945 computeFreemanParameters((element + 4) % 8, diP);
948 if (hasGoodLevel(I, iPtemp)) {
949 element = (element + 4) % 8;
952 computeFreemanParameters((element + 3) % 8, diP);
955 if (hasGoodLevel(I, iPtemp)) {
956 element = (element + 3) % 8;
991 if (!isInImage(I, iP))
1034 void vpMeNurbs::computeFreemanParameters(
unsigned int element,
vpImagePoint &diP)
1093 return (iP.
get_i() < height - 20 && iP.
get_j() < width - 20 && iP.
get_i() > 20 && iP.
get_j() > 20);
Class to define RGB colors available for display functionalities.
static const vpColor orange
static const vpColor blue
static const vpColor green
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.
@ notInitialized
Used to indicate that a parameter is not initialized.
@ divideByZeroError
Division by zero.
static void canny(const vpImage< unsigned char > &I, vpImage< unsigned char > &Ic, const unsigned int &gaussianFilterSize, const float &thresholdCanny, const unsigned int &apertureSobel)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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)
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static Type abs(const Type &x)
static int round(double x)
static int sign(double x)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int j
Coordinates along j of a site.
@ TOO_NEAR
Point removed because too near image borders.
@ NO_SUPPRESSION
Point used by the tracker.
void setDisplay(vpMeSiteDisplayType select)
double ifloat
Floating coordinates along i of a site.
int i
Coordinate along i of a site.
double alpha
Angle of tangent at site.
double jfloat
Floating coordinates along j of a site.
vpMeSiteState getState() const
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
void setState(const vpMeSiteState &flag)
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
std::list< vpMeSite > list
vpMe * me
Moving edges initialisation parameters.
void setRange(const unsigned int &range)
unsigned int getAngleStep() const
int getPointsToTrack() const
unsigned int getMaskSize() const
vpMatrix * getMask() const
double getSampleStep() const
unsigned int getRange() const
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
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)
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)
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)
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)
Defines a rectangle in the plane.
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
#define vpDEBUG_ENABLE(level)