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 bool displayFullModel)
521 cMo, camera, displayFullModel);
523 for (
size_t i = 0; i < models.size(); i++) {
543 bool displayFullModel)
546 cMo, camera, displayFullModel);
548 for (
size_t i = 0; i < models.size(); i++) {
562 std::vector<std::vector<double> > features;
565 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end(); ++it) {
567 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
568 std::vector<double> params = {0,
571 static_cast<double>(p_me.
getState())};
573 std::vector<double> params;
577 params.push_back(
static_cast<double>(p_me.
getState()));
579 features.push_back(params);
584 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end(); ++it) {
586 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
587 std::vector<double> params = {0,
590 static_cast<double>(p_me.
getState())};
592 std::vector<double> params;
596 params.push_back(
static_cast<double>(p_me.
getState()));
598 features.push_back(params);
618 bool displayFullModel)
620 std::vector<std::vector<double> > models;
622 if ((
isvisible && isTrackedCylinder) || displayFullModel) {
635 std::cout <<
"Problem projection circle 1";
640 std::cout <<
"Problem projection circle 2";
652 double i11, i12, i21, i22, j11, j12, j21, j22;
667 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
668 std::vector<double> params1 = {0,
674 std::vector<double> params2 = {0,
680 std::vector<double> params1, params2;
681 params1.push_back(0);
682 params1.push_back(ip11.
get_i());
683 params1.push_back(ip11.
get_j());
684 params1.push_back(ip12.
get_i());
685 params1.push_back(ip12.
get_j());
687 params2.push_back(0);
688 params2.push_back(ip11.
get_i());
689 params2.push_back(ip11.
get_j());
690 params2.push_back(ip12.
get_i());
691 params2.push_back(ip12.
get_j());
694 models.push_back(params1);
695 models.push_back(params2);
769 std::cout <<
"Problem projection circle 1\n";
774 std::cout <<
"Problem projection circle 2\n";
786 double rho1 = featureline1.
getRho();
787 double theta1 = featureline1.
getTheta();
788 double rho2 = featureline2.
getRho();
789 double theta2 = featureline2.
getTheta();
791 double co1 = cos(theta1);
792 double si1 = sin(theta1);
793 double co2 = cos(theta2);
794 double si2 = sin(theta2);
796 double mx = 1.0 / cam.
get_px();
797 double my = 1.0 / cam.
get_py();
808 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
810 double x = (double)it->
j;
811 double y = (
double)it->i;
816 double alpha1 = x * si1 - y * co1;
818 double *Lrho = H1[0];
819 double *Ltheta = H1[1];
821 for (
unsigned int k = 0; k < 6; k++) {
822 L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
824 error[j] = rho1 - (x * co1 + y * si1);
832 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
834 double x = (double)it->j;
835 double y = (
double)it->i;
840 double alpha2 = x * si2 - y * co2;
842 double *Lrho = H2[0];
843 double *Ltheta = H2[1];
845 for (
unsigned int k = 0; k < 6; k++) {
846 L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
848 error[j] = rho2 - (x * co2 + y * si2);