38 #include <visp3/core/vpCameraParameters.h>
39 #include <visp3/core/vpConfig.h>
40 #include <visp3/core/vpHomogeneousMatrix.h>
41 #include <visp3/core/vpImage.h>
42 #include <visp3/core/vpPoint.h>
43 #include <visp3/vision/vpPose.h>
44 #include <visp3/vision/vpPoseFeatures.h>
53 #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55 class vp_createPointClass
60 vp_createPointClass() : value(0) { }
75 int test_pose(
bool use_robust)
78 std::cout <<
"** Test robust pose estimation from features\n" << std::endl;
80 std::cout <<
"** Test pose estimation from features\n" << std::endl;
87 std::cout <<
"Reference pose used to create the visual features : " << std::endl;
88 std::cout << pose_ref.
t() << std::endl;
92 std::vector<vpPoint> pts;
98 pts.push_back(
vpPoint(0.0, -val, val2));
99 pts.push_back(
vpPoint(0.0, val, val2));
100 pts.push_back(
vpPoint(-val, val, val2));
103 pts.push_back(
vpPoint(-val, -val / 2.0, val2));
104 pts.push_back(
vpPoint(val, val / 2.0, val2));
107 pts.push_back(
vpPoint(0.0, 0.0, -1.5));
124 pts[0].project(cMo_ref);
125 pts[1].project(cMo_ref);
126 pts[2].project(cMo_ref);
128 pts[3].project(cMo_ref);
129 pts[4].project(cMo_ref);
131 pts[5].project(cMo_ref);
158 vp_createPointClass cpClass;
159 int (vp_createPointClass::*ptrClass)(
vpFeaturePoint &,
const vpPoint &) = &vp_createPointClass::vp_createPoint;
171 std::cout <<
"\nPose used as initialisation of the pose computation : " << std::endl;
172 std::cout << pose_est.
t() << std::endl;
180 std::cout <<
"\nEstimated pose from visual features : " << std::endl;
182 std::cout <<
"\nRobust estimated pose from visual features : " << std::endl;
185 std::cout << pose_est.
t() << std::endl;
187 std::cout <<
"\nResulting covariance (Diag): " << std::endl;
189 std::cout << covariance[0][0] <<
" " << covariance[1][1] <<
" " << covariance[2][2] <<
" " << covariance[3][3] <<
" "
190 << covariance[4][4] <<
" " << covariance[5][5] <<
" " << std::endl;
193 for (
unsigned int i = 0; i < 6; i++) {
194 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
198 std::cout <<
"\nPose is " << (test_fail ?
"badly" :
"well") <<
" estimated\n" << std::endl;
206 #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) \
207 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
209 if (test_pose(
false))
222 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV and enable c++11 min standard " << std::endl;
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
void setWorldCoordinates(const vpColVector &oP) vp_override
error that can be emitted by ViSP classes.
const std::string & getStringMessage() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
static double rad(double deg)
Implementation of a matrix and operations on matrices.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Tools for pose computation from any feature.
void setVerbose(const bool &mode)
void addFeaturePoint3D(const vpPoint &p)
void setVVSIterMax(const unsigned int &val)
void addFeaturePoint(const vpPoint &p)
void setCovarianceComputation(const bool &flag)
void addFeatureEllipse(const vpCircle &)
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
void addFeatureVanishingPoint(const vpPoint &p)
void setLambda(const double &val)
void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type=VIRTUAL_VS)
vpMatrix getCovarianceMatrix() const
Implementation of a pose vector and operations on poses.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)