43 #include <visp/vpConfig.h>
44 #ifndef DOXYGEN_SHOULD_SKIP_THIS
51 #include <visp/vpMbtDistanceLine.h>
52 #include <visp/vpPlane.h>
53 #include <visp/vpMeterPixelConversion.h>
54 #include <visp/vpFeatureBuilder.h>
60 vpMbtDistanceLine::vpMbtDistanceLine()
77 vpMbtDistanceLine::~vpMbtDistanceLine()
81 if (p1 != NULL)
delete p1 ;
82 if (p2 != NULL)
delete p2 ;
83 if (line != NULL)
delete line ;
84 if (meline != NULL)
delete meline ;
134 double norm = sqrt(A*A+B*B+C*C) ;
158 buildPlane(P1,P2,P3,plane1) ;
159 buildPlane(P1,P2,P4,plane2) ;
187 V1[0] = p1->get_oX();
188 V1[1] = p1->get_oY();
189 V1[2] = p1->get_oZ();
190 V2[0] = p2->get_oX();
191 V2[1] = p2->get_oY();
192 V2[2] = p2->get_oZ();
195 if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
198 V3[0]=double(rand()%1000)/100;
199 V3[1]=double(rand()%1000)/100;
200 V3[2]=double(rand()%1000)/100;
213 buildLine(*p1,*p2, P3,P4, *line) ;
221 buildLine(*p1,*p2,P3,P4,*line) ;
232 vpMbtDistanceLine::setMovingEdge(
vpMe *_me)
254 p1->changeFrame(cMo);
255 p2->changeFrame(cMo);
256 line->changeFrame(cMo);
270 while (theta > M_PI) { theta -= M_PI ; }
271 while (theta < -M_PI) { theta += M_PI ; }
273 if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
274 else theta = M_PI/2.0 - theta;
276 meline =
new vpMbtMeLine ;
280 meline->setInitRange(0);
283 if (ip1.
get_j()<ip2.
get_j()) { meline->jmin = (int)ip1.
get_j()-marge ; meline->jmax = (int)ip2.
get_j()+marge ; }
else{ meline->jmin = (int)ip2.
get_j()-marge ; meline->jmax = (int)ip1.
get_j()+marge ; }
284 if (ip1.
get_i()<ip2.
get_i()) { meline->imin = (int)ip1.
get_i()-marge ; meline->imax = (int)ip2.
get_i()+marge ; }
else{ meline->imin = (int)ip2.
get_i()-marge ; meline->imax = (int)ip1.
get_i()+marge ; }
288 meline->initTracking(I,ip1,ip2,rho,theta);
337 nbFeature = meline->getMeList().size();
353 p1->changeFrame(cMo) ;
354 p2->changeFrame(cMo) ;
355 line->changeFrame(cMo) ;
368 while (theta > M_PI) { theta -= M_PI ; }
369 while (theta < -M_PI) { theta += M_PI ; }
371 if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
372 else theta = M_PI/2.0 - theta;
375 if (ip1.
get_j()<ip2.
get_j()) { meline->jmin = (int)ip1.
get_j()-marge ; meline->jmax = (int)ip2.
get_j()+marge ; }
else{ meline->jmin = (int)ip2.
get_j()-marge ; meline->jmax = (int)ip1.
get_j()+marge ; }
376 if (ip1.
get_i()<ip2.
get_i()) { meline->imin = (int)ip1.
get_i()-marge ; meline->imax = (int)ip2.
get_i()+marge ; }
else{ meline->imin = (int)ip2.
get_i()-marge ; meline->imax = (int)ip1.
get_i()+marge ; }
381 meline->updateParameters(I,ip1,ip2,rho,theta) ;
387 nbFeature = meline->getMeList().size();
406 initMovingEdge(I,cMo);
425 if (isvisible ==
true || displayFullModel)
427 p1->changeFrame(cMo) ;
428 p2->changeFrame(cMo) ;
429 line->changeFrame(cMo) ;
458 if (isvisible ==
true || displayFullModel)
460 p1->changeFrame(cMo) ;
461 p2->changeFrame(cMo) ;
462 line->changeFrame(cMo) ;
501 vpMbtDistanceLine::initInteractionMatrixError()
503 if (isvisible ==
true)
505 L.resize(meline->getMeList().size(),6) ;
506 error.resize(meline->getMeList().size()) ;
507 nbFeature = meline->getMeList().size() ;
523 line->changeFrame(cMo) ;
528 double rho = featureline.getRho() ;
529 double theta = featureline.getTheta() ;
531 double co = cos(theta);
532 double si = sin(theta);
534 double mx = 1.0/cam.
get_px() ;
535 double my = 1.0/cam.
get_py() ;
536 double xc = cam.
get_u0() ;
537 double yc = cam.
get_v0() ;
541 H = featureline.interaction() ;
546 for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
555 double *Lrho = H[0] ;
556 double *Ltheta = H[1] ;
558 for (
unsigned int k=0 ; k < 6 ; k++)
560 L[j][k] = (Lrho[k] + alpha*Ltheta[k]);
562 error[j] = rho - ( x*co + y*si) ;
583 for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
591 if( ((
unsigned int)i > (I.
getHeight()- threshold) ) || (
unsigned int)i < threshold ||
592 ((
unsigned int)j > (I.
getWidth ()- threshold) ) || (
unsigned int)j < threshold ) {
Definition of the vpMatrix class.
unsigned int getWidth() const
static vpColVector cross(const vpColVector &a, const vpColVector &b)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
double get_oY() const
Get the point Y coordinate in the object frame.
Contains predetermined masks for sites and holds moving edges tracking parameters.
Class that defines what is a point.
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
void setA(const double A)
void setC(const double C)
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
line coordinates conversion (rho,theta)
void setD(const double D)
double get_oX() const
Get the point X coordinate in the object frame.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
unsigned int getHeight() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
This class defines the container for a plane geometrical structure.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setB(const double B)
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...