43 #include <visp/vpConfig.h>
50 #include <visp/vpMbtDistanceLine.h>
51 #include <visp/vpPlane.h>
52 #include <visp/vpMeterPixelConversion.h>
53 #include <visp/vpFeatureBuilder.h>
80 if (
p1 != NULL)
delete p1 ;
81 if (
p2 != NULL)
delete p2 ;
133 double norm = sqrt(A*A+B*B+C*C) ;
157 buildPlane(P1,P2,P3,plane1) ;
158 buildPlane(P1,P2,P4,plane2) ;
194 if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
197 V3[0]=double(rand()%1000)/100;
198 V3[1]=double(rand()%1000)/100;
199 V3[2]=double(rand()%1000)/100;
269 while (theta > M_PI) { theta -= M_PI ; }
270 while (theta < -M_PI) { theta += M_PI ; }
272 if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
273 else theta = M_PI/2.0 - theta;
367 while (theta > M_PI) { theta -= M_PI ; }
368 while (theta < -M_PI) { theta += M_PI ; }
370 if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
371 else theta = M_PI/2.0 - theta;
424 if (
isvisible ==
true || displayFullModel)
455 if (
isvisible ==
true || displayFullModel)
523 double rho = featureline.
getRho() ;
524 double theta = featureline.
getTheta() ;
526 double co = cos(theta);
527 double si = sin(theta);
529 double mx = 1.0/cam.
get_px() ;
530 double my = 1.0/cam.
get_py() ;
531 double xc = cam.
get_u0() ;
532 double yc = cam.
get_v0() ;
550 double *Lrho = H[0] ;
551 double *Ltheta = H[1] ;
553 for (
unsigned int k=0 ; k < 6 ; k++)
555 L[j][k] = (Lrho[k] + alpha*Ltheta[k]);
557 error[j] = rho - ( x*co + y*si) ;
586 if( ((
unsigned int)i > (I.
getHeight()- threshold) ) || (
unsigned int)i < threshold ||
587 ((
unsigned int)j > (I.
getWidth ()- threshold) ) || (
unsigned int)j < threshold ) {