45 #include <visp/vpConfig.h>
52 #include <visp/vpMbtDistanceCylinder.h>
53 #include <visp/vpPlane.h>
54 #include <visp/vpMeterPixelConversion.h>
55 #include <visp/vpPixelMeterConversion.h>
56 #include <visp/vpFeatureBuilder.h>
57 #include <visp/vpFeatureEllipse.h>
61 #include <visp/vpPose.h>
92 if (
p1 != NULL)
delete p1 ;
93 if (
p2 != NULL)
delete p2 ;
94 if (
c != NULL)
delete c ;
193 vpMbtDistanceCylinder::getCylinderLineExtremity(
double &i,
double &j,
double rho,
double theta,
197 double px = cam.
get_px() ;
198 double py = cam.
get_py() ;
199 double u0 = cam.
get_u0() ;
200 double v0 = cam.
get_v0() ;
202 double mu11 = circle->
p[3];
203 double mu02 = circle->
p[4];
204 double mu20 = circle->
p[2];
205 double Xg = u0 + circle->
p[0]*px;
206 double Yg = v0 + circle->
p[1]*py;
211 double stheta = sin(theta);
212 double ctheta = cos(theta);
213 double sctheta = stheta*ctheta;
214 double m11yg = mu11*Yg;
216 double m02xg = mu02*Xg;
217 double m11stheta = mu11*stheta;
218 j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
219 -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
220 m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
221 +mu02-mu02*ctheta2));
223 double rhom02 = rho*mu02;
224 double sctheta2 = stheta*ctheta2;
225 double ctheta3 = ctheta2*ctheta;
226 i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
227 +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
228 +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
229 (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
230 mu02*ctheta2)/stheta);
257 catch(...){std::cout<<
"Problem when projecting circle 1\n";}
261 catch(...){std::cout<<
"Problem when projecting circle 2\n";}
282 double i11,i12,i21,i22,j11,j12,j21,j22;
283 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
284 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
285 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
286 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
310 vpTRACE(
"the line can't be initialized");
318 vpTRACE(
"the line can't be initialized");
339 std::cout <<
"Track meline1 failed" << std::endl;
348 std::cout <<
"Track meline2 failed" << std::endl;
380 catch(...){std::cout<<
"Probleme projection cercle 1\n";}
384 catch(...){std::cout<<
"Probleme projection cercle 2\n";}
396 double i11,i12,i21,i22,j11,j12,j21,j22;
398 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
399 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
401 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
402 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
494 catch(...){std::cout<<
"Problem projection circle 1";}
498 catch(...){std::cout<<
"Problem projection circle 2";}
509 double i11,i12,i21,i22,j11,j12,j21,j22;
511 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
512 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
514 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
515 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
553 catch(...){std::cout<<
"Problem projection circle 1";}
557 catch(...){std::cout<<
"Problem projection circle 2";}
568 double i11,i12,i21,i22,j11,j12,j21,j22;
570 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
571 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
573 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
574 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
639 catch(...){std::cout<<
"Problem projection circle 1\n";}
643 catch(...){std::cout<<
"Problem projection circle 2\n";}
653 double rho1 = featureline1.
getRho() ;
654 double theta1 = featureline1.
getTheta() ;
655 double rho2 = featureline2.
getRho() ;
656 double theta2 = featureline2.
getTheta() ;
658 double co1 = cos(theta1);
659 double si1 = sin(theta1);
660 double co2 = cos(theta2);
661 double si2 = sin(theta2);
663 double mx = 1.0/cam.
get_px() ;
664 double my = 1.0/cam.
get_py() ;
665 double xc = cam.
get_u0() ;
666 double yc = cam.
get_v0() ;
685 alpha1 = x*si1 - y*co1;
687 double *Lrho = H1[0] ;
688 double *Ltheta = H1[1] ;
690 for (
unsigned int k=0 ; k < 6 ; k++){
691 L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
693 error[j] = rho1 - ( x*co1 + y*si1) ;
707 alpha2 = x*si2 - y*co2;
709 double *Lrho = H2[0] ;
710 double *Ltheta = H2[1] ;
712 for (
unsigned int k=0 ; k < 6 ; k++){
713 L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
715 error[j] = rho2 - ( x*co2 + y*si2) ;