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);
304 while (theta1 > M_PI) { theta1 -= M_PI ; }
305 while (theta1 < -M_PI) { theta1 += M_PI ; }
307 if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
308 else theta1 = M_PI/2.0 - theta1;
310 while (theta2 > M_PI) { theta2 -= M_PI ; }
311 while (theta2 < -M_PI) { theta2 += M_PI ; }
313 if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
314 else theta2 = M_PI/2.0 - theta2;
322 vpTRACE(
"the line can't be initialized");
330 vpTRACE(
"the line can't be initialized");
351 std::cout <<
"Track meline1 failed" << std::endl;
360 std::cout <<
"Track meline2 failed" << std::endl;
392 catch(...){std::cout<<
"Probleme projection cercle 1\n";}
396 catch(...){std::cout<<
"Probleme projection cercle 2\n";}
408 double i11,i12,i21,i22,j11,j12,j21,j22;
410 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
411 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
413 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
414 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
432 while (theta1 > M_PI) { theta1 -= M_PI ; }
433 while (theta1 < -M_PI) { theta1 += M_PI ; }
435 if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
436 else theta1 = M_PI/2.0 - theta1;
438 while (theta2 > M_PI) { theta2 -= M_PI ; }
439 while (theta2 < -M_PI) { theta2 += M_PI ; }
441 if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
442 else theta2 = M_PI/2.0 - theta2;
519 catch(...){std::cout<<
"Problem projection circle 1";}
523 catch(...){std::cout<<
"Problem projection circle 2";}
534 double i11,i12,i21,i22,j11,j12,j21,j22;
536 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
537 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
539 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
540 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
578 catch(...){std::cout<<
"Problem projection circle 1";}
582 catch(...){std::cout<<
"Problem projection circle 2";}
593 double i11,i12,i21,i22,j11,j12,j21,j22;
595 getCylinderLineExtremity(i11, j11, rho1, theta1,
cercle1);
596 getCylinderLineExtremity(i12, j12, rho1, theta1,
cercle2);
598 getCylinderLineExtremity(i21, j21, rho2, theta2,
cercle1);
599 getCylinderLineExtremity(i22, j22, rho2, theta2,
cercle2);
664 catch(...){std::cout<<
"Problem projection circle 1\n";}
668 catch(...){std::cout<<
"Problem projection circle 2\n";}
678 double rho1 = featureline1.
getRho() ;
679 double theta1 = featureline1.
getTheta() ;
680 double rho2 = featureline2.
getRho() ;
681 double theta2 = featureline2.
getTheta() ;
683 double co1 = cos(theta1);
684 double si1 = sin(theta1);
685 double co2 = cos(theta2);
686 double si2 = sin(theta2);
688 double mx = 1.0/cam.
get_px() ;
689 double my = 1.0/cam.
get_py() ;
690 double xc = cam.
get_u0() ;
691 double yc = cam.
get_v0() ;
710 alpha1 = x*si1 - y*co1;
712 double *Lrho = H1[0] ;
713 double *Ltheta = H1[1] ;
715 for (
unsigned int k=0 ; k < 6 ; k++){
716 L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
718 error[j] = rho1 - ( x*co1 + y*si1) ;
732 alpha2 = x*si2 - y*co2;
734 double *Lrho = H2[0] ;
735 double *Ltheta = H2[1] ;
737 for (
unsigned int k=0 ; k < 6 ; k++){
738 L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
740 error[j] = rho2 - ( x*co2 + y*si2) ;