43 #include <visp3/core/vpConfig.h>
52 #include <visp3/core/vpMeterPixelConversion.h>
53 #include <visp3/core/vpPixelMeterConversion.h>
54 #include <visp3/core/vpPlane.h>
55 #include <visp3/mbt/vpMbtDistanceCylinder.h>
56 #include <visp3/visual_features/vpFeatureBuilder.h>
57 #include <visp3/visual_features/vpFeatureEllipse.h>
59 #include <visp3/vision/vpPose.h>
65 : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
66 meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(), error(),
67 nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL), hiddenface(NULL), index_polygon(-1),
231 double i11, i12, i21, i22, j11, j12, j21, j22;
277 while (theta1 > M_PI) {
280 while (theta1 < -M_PI) {
284 if (theta1 < -M_PI / 2.0)
285 theta1 = -theta1 - 3 * M_PI / 2.0;
287 theta1 = M_PI / 2.0 - theta1;
289 while (theta2 > M_PI) {
292 while (theta2 < -M_PI) {
296 if (theta2 < -M_PI / 2.0)
297 theta2 = -theta2 - 3 * M_PI / 2.0;
299 theta2 = M_PI / 2.0 - theta2;
302 meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
308 meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
369 std::cout <<
"Probleme projection cercle 1\n";
374 std::cout <<
"Probleme projection cercle 2\n";
387 double i11, i12, i21, i22, j11, j12, j21, j22;
435 while (theta1 > M_PI) {
438 while (theta1 < -M_PI) {
442 if (theta1 < -M_PI / 2.0)
443 theta1 = -theta1 - 3 * M_PI / 2.0;
445 theta1 = M_PI / 2.0 - theta1;
447 while (theta2 > M_PI) {
450 while (theta2 < -M_PI) {
454 if (theta2 < -M_PI / 2.0)
455 theta2 = -theta2 - 3 * M_PI / 2.0;
457 theta2 = M_PI / 2.0 - theta2;
461 meline1->updateParameters(I, ip11, ip12, rho1, theta1);
467 meline2->updateParameters(I, ip21, ip22, rho2, theta2);
518 const bool displayFullModel)
520 if ((
isvisible && isTrackedCylinder) || displayFullModel) {
533 std::cout <<
"Problem projection circle 1";
538 std::cout <<
"Problem projection circle 2";
550 double i11, i12, i21, i22, j11, j12, j21, j22;
583 const bool displayFullModel)
585 if ((
isvisible && isTrackedCylinder) || displayFullModel) {
598 std::cout <<
"Problem projection circle 1";
603 std::cout <<
"Problem projection circle 2";
615 double i11, i12, i21, i22, j11, j12, j21, j22;
694 std::cout <<
"Problem projection circle 1\n";
699 std::cout <<
"Problem projection circle 2\n";
711 double rho1 = featureline1.
getRho();
712 double theta1 = featureline1.
getTheta();
713 double rho2 = featureline2.
getRho();
714 double theta2 = featureline2.
getTheta();
716 double co1 = cos(theta1);
717 double si1 = sin(theta1);
718 double co2 = cos(theta2);
719 double si2 = sin(theta2);
721 double mx = 1.0 / cam.
get_px();
722 double my = 1.0 / cam.
get_py();
733 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
735 double x = (double)it->
j;
736 double y = (
double)it->i;
741 double alpha1 = x * si1 - y * co1;
743 double *Lrho = H1[0];
744 double *Ltheta = H1[1];
746 for (
unsigned int k = 0; k < 6; k++) {
747 L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
749 error[j] = rho1 - (x * co1 + y * si1);
757 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
759 double x = (double)it->j;
760 double y = (
double)it->i;
765 double alpha2 = x * si2 - y * co2;
767 double *Lrho = H2[0];
768 double *Ltheta = H2[1];
770 for (
unsigned int k = 0; k < 6; k++) {
771 L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
773 error[j] = rho2 - (x * co2 + y * si2);