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(nullptr), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
64 meline1(nullptr), meline2(nullptr), cercle1(nullptr), cercle2(nullptr), radius(0), p1(nullptr), p2(nullptr), L(), error(),
65 nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(nullptr), hiddenface(nullptr), index_polygon(-1),
228 double i11, i12, i21, i22, j11, j12, j21, j22;
278 while (theta1 > M_PI) {
281 while (theta1 < -M_PI) {
285 if (theta1 < -M_PI / 2.0)
286 theta1 = -theta1 - 3 * M_PI / 2.0;
288 theta1 = M_PI / 2.0 - theta1;
290 while (theta2 > M_PI) {
293 while (theta2 < -M_PI) {
297 if (theta2 < -M_PI / 2.0)
298 theta2 = -theta2 - 3 * M_PI / 2.0;
300 theta2 = M_PI / 2.0 - theta2;
303 meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
310 meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
375 std::cout <<
"Probleme projection cercle 1\n";
381 std::cout <<
"Probleme projection cercle 2\n";
394 double i11, i12, i21, i22, j11, j12, j21, j22;
446 while (theta1 > M_PI) {
449 while (theta1 < -M_PI) {
453 if (theta1 < -M_PI / 2.0)
454 theta1 = -theta1 - 3 * M_PI / 2.0;
456 theta1 = M_PI / 2.0 - theta1;
458 while (theta2 > M_PI) {
461 while (theta2 < -M_PI) {
465 if (theta2 < -M_PI / 2.0)
466 theta2 = -theta2 - 3 * M_PI / 2.0;
468 theta2 = M_PI / 2.0 - theta2;
472 meline1->updateParameters(I, ip11, ip12, rho1, theta1);
479 meline2->updateParameters(I, ip21, ip22, rho2, theta2);
532 bool displayFullModel)
534 std::vector<std::vector<double> > models =
537 for (
size_t i = 0; i < models.size(); i++) {
557 bool displayFullModel)
559 std::vector<std::vector<double> > models =
562 for (
size_t i = 0; i < models.size(); i++) {
576 std::vector<std::vector<double> > features;
579 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
582 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
583 std::vector<double> params = { 0,
586 static_cast<double>(p_me.
getState()) };
588 std::vector<double> params;
592 params.push_back(
static_cast<double>(p_me.
getState()));
595 features.push_back(params);
600 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
603 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
604 std::vector<double> params = { 0,
607 static_cast<double>(p_me.
getState()) };
609 std::vector<double> params;
613 params.push_back(
static_cast<double>(p_me.
getState()));
616 features.push_back(params);
636 bool displayFullModel)
638 std::vector<std::vector<double> > models;
640 if ((
isvisible && isTrackedCylinder) || displayFullModel) {
654 std::cout <<
"Problem projection circle 1";
660 std::cout <<
"Problem projection circle 2";
672 double i11, i12, i21, i22, j11, j12, j21, j22;
687 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
688 std::vector<double> params1 = { 0,
694 std::vector<double> params2 = { 0,
700 std::vector<double> params1, params2;
701 params1.push_back(0);
702 params1.push_back(ip11.
get_i());
703 params1.push_back(ip11.
get_j());
704 params1.push_back(ip12.
get_i());
705 params1.push_back(ip12.
get_j());
707 params2.push_back(0);
708 params2.push_back(ip11.
get_i());
709 params2.push_back(ip11.
get_j());
710 params2.push_back(ip12.
get_i());
711 params2.push_back(ip12.
get_j());
714 models.push_back(params1);
715 models.push_back(params2);
791 std::cout <<
"Problem projection circle 1\n";
797 std::cout <<
"Problem projection circle 2\n";
809 double rho1 = featureline1.
getRho();
810 double theta1 = featureline1.
getTheta();
811 double rho2 = featureline2.
getRho();
812 double theta2 = featureline2.
getTheta();
814 double co1 = cos(theta1);
815 double si1 = sin(theta1);
816 double co2 = cos(theta2);
817 double si2 = sin(theta2);
819 double mx = 1.0 / cam.
get_px();
820 double my = 1.0 / cam.
get_py();
831 for (std::list<vpMeSite>::const_iterator it =
meline1->getMeList().begin(); it !=
meline1->getMeList().end();
833 double x = (double)it->
m_j;
834 double y = (
double)it->m_i;
839 double alpha1 = x * si1 - y * co1;
841 double *Lrho = H1[0];
842 double *Ltheta = H1[1];
844 for (
unsigned int k = 0; k < 6; k++) {
845 L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
847 error[j] = rho1 - (x * co1 + y * si1);
855 for (std::list<vpMeSite>::const_iterator it =
meline2->getMeList().begin(); it !=
meline2->getMeList().end();
857 double x = (double)it->m_j;
858 double y = (
double)it->m_i;
863 double alpha2 = x * si2 - y * co2;
865 double *Lrho = H2[0];
866 double *Ltheta = H2[1];
868 for (
unsigned int k = 0; k < 6; k++) {
869 L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
871 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 projection() vp_override
void setWorldCoordinates(const vpColVector &oP) vp_override
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
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 setWorldCoordinates(const vpColVector &oP) vp_override
void projection() vp_override
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
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) vp_override
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)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
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'....
vpMeSiteState getState() const
int m_j
Integer coordinates along j of a site.
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 vp_override
double get_oY() const
Get the point oY coordinate in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override