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>
130 double norm = sqrt(A*A+B*B+C*C) ;
154 buildPlane(P1,P2,P3,plane1) ;
155 buildPlane(P1,P2,P4,plane2) ;
192 if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
195 V3[0]=double(rand()%1000)/100;
196 V3[1]=double(rand()%1000)/100;
197 V3[2]=double(rand()%1000)/100;
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;
378 while (theta > M_PI) { theta -= M_PI ; }
379 while (theta < -M_PI) { theta += M_PI ; }
381 if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
382 else theta = M_PI/2.0 - theta;
558 double rho = featureline.
getRho() ;
559 double theta = featureline.
getTheta() ;
561 double co = cos(theta);
562 double si = sin(theta);
564 double mx = 1.0/cam.
get_px() ;
565 double my = 1.0/cam.
get_py() ;
566 double xc = cam.
get_u0() ;
567 double yc = cam.
get_v0() ;
585 double *Lrho = H[0] ;
586 double *Ltheta = H[1] ;
588 for (
unsigned int k=0 ; k < 6 ; k++)
590 L[j][k] = (Lrho[k] + alpha*Ltheta[k]);
592 error[j] = rho - ( x*co + y*si) ;
621 if( ((
unsigned int)i > (I.
getHeight()- threshold) ) || (
unsigned int)i < threshold ||
622 ((
unsigned int)j > (I.
getWidth ()- threshold) ) || (
unsigned int)j < threshold ) {