41 #include <visp3/core/vpConfig.h>
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPlane.h>
53 #include <visp3/mbt/vpMbtDistanceCylinder.h>
54 #include <visp3/visual_features/vpFeatureBuilder.h>
55 #include <visp3/visual_features/vpFeatureEllipse.h>
57 #include <visp3/vision/vpPose.h>
63 : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
64 meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(), error(),
65 nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL), hiddenface(NULL), index_polygon(-1),
227 double i11, i12, i21, i22, j11, j12, j21, j22;
273 while (theta1 > M_PI) {
276 while (theta1 < -M_PI) {
280 if (theta1 < -M_PI / 2.0)
281 theta1 = -theta1 - 3 * M_PI / 2.0;
283 theta1 = M_PI / 2.0 - theta1;
285 while (theta2 > M_PI) {
288 while (theta2 < -M_PI) {
292 if (theta2 < -M_PI / 2.0)
293 theta2 = -theta2 - 3 * M_PI / 2.0;
295 theta2 = M_PI / 2.0 - theta2;
298 meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
304 meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
365 std::cout <<
"Probleme projection cercle 1\n";
370 std::cout <<
"Probleme projection cercle 2\n";
383 double i11, i12, i21, i22, j11, j12, j21, j22;
431 while (theta1 > M_PI) {
434 while (theta1 < -M_PI) {
438 if (theta1 < -M_PI / 2.0)
439 theta1 = -theta1 - 3 * M_PI / 2.0;
441 theta1 = M_PI / 2.0 - theta1;
443 while (theta2 > M_PI) {
446 while (theta2 < -M_PI) {
450 if (theta2 < -M_PI / 2.0)
451 theta2 = -theta2 - 3 * M_PI / 2.0;
453 theta2 = M_PI / 2.0 - theta2;
457 meline1->updateParameters(I, ip11, ip12, rho1, theta1);
463 meline2->updateParameters(I, ip21, ip22, rho2, theta2);
515 bool displayFullModel)
517 std::vector<std::vector<double> > models =
520 for (
size_t i = 0; i < models.size(); i++) {
540 bool displayFullModel)
542 std::vector<std::vector<double> > models =
545 for (
size_t i = 0; i < models.size(); i++) {
559 std::vector<std::vector<double> > features;
562 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
565 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
566 std::vector<double> params = {0,
569 std::vector<double> params;
573 params.push_back(
static_cast<double>(p_me.
getState()));
575 features.push_back(params);
580 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
583 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
584 std::vector<double> params = {0,
587 std::vector<double> params;
591 params.push_back(
static_cast<double>(p_me.
getState()));
593 features.push_back(params);
613 bool displayFullModel)
615 std::vector<std::vector<double> > models;
617 if ((
isvisible && isTrackedCylinder) || displayFullModel) {
630 std::cout <<
"Problem projection circle 1";
635 std::cout <<
"Problem projection circle 2";
647 double i11, i12, i21, i22, j11, j12, j21, j22;
662 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
667 std::vector<double> params1, params2;
668 params1.push_back(0);
669 params1.push_back(ip11.
get_i());
670 params1.push_back(ip11.
get_j());
671 params1.push_back(ip12.
get_i());
672 params1.push_back(ip12.
get_j());
674 params2.push_back(0);
675 params2.push_back(ip11.
get_i());
676 params2.push_back(ip11.
get_j());
677 params2.push_back(ip12.
get_i());
678 params2.push_back(ip12.
get_j());
681 models.push_back(params1);
682 models.push_back(params2);
756 std::cout <<
"Problem projection circle 1\n";
761 std::cout <<
"Problem projection circle 2\n";
773 double rho1 = featureline1.
getRho();
774 double theta1 = featureline1.
getTheta();
775 double rho2 = featureline2.
getRho();
776 double theta2 = featureline2.
getTheta();
778 double co1 = cos(theta1);
779 double si1 = sin(theta1);
780 double co2 = cos(theta2);
781 double si2 = sin(theta2);
783 double mx = 1.0 / cam.
get_px();
784 double my = 1.0 / cam.
get_py();
795 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
797 double x = (double)it->
j;
798 double y = (
double)it->i;
803 double alpha1 = x * si1 - y * co1;
805 double *Lrho = H1[0];
806 double *Ltheta = H1[1];
808 for (
unsigned int k = 0; k < 6; k++) {
809 L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
811 error[j] = rho1 - (x * co1 + y * si1);
819 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
821 double x = (double)it->j;
822 double y = (
double)it->i;
827 double alpha2 = x * si2 - y * co2;
829 double *Lrho = H2[0];
830 double *Ltheta = H2[1];
832 for (
unsigned int k = 0; k < 6; k++) {
833 L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
835 error[j] = rho2 - (x * co2 + y * si2);
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
void setWorldCoordinates(const vpColVector &oP)
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
static const vpColor orange
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
void setWorldCoordinates(const vpColVector &oP)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_ij(double ii, double jj)
unsigned int getWidth() const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpCylinder * c
The cylinder.
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpCircle * cercle1
The upper circle limiting the cylinder.
void initInteractionMatrixError()
void setMovingEdge(vpMe *Me)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< std::vector< double > > getFeaturesForDisplay()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
unsigned int nbFeature
The number of moving edges.
vpCircle * cercle2
The lower circle limiting the cylinder.
bool isvisible
Indicates if the cylinder is visible or not.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual ~vpMbtDistanceCylinder()
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int j
Coordinates along j of a site.
vpMeSiteState getState() const
double get_ifloat() const
double get_jfloat() const
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_oZ() const
Get the point oZ coordinate in the object frame.
void projection(const vpColVector &_cP, vpColVector &_p) const
double get_oY() const
Get the point oY coordinate in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const