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#ifndef DOXYGEN_SHOULD_SKIP_THIS
55#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
56class vp_createPointClass
61 vp_createPointClass() : value(0) {}
77int test_pose(
bool use_robust)
80 std::cout <<
"** Test robust pose estimation from features\n" << std::endl;
82 std::cout <<
"** Test pose estimation from features\n" << std::endl;
89 std::cout <<
"Reference pose used to create the visual features : " << std::endl;
90 std::cout << pose_ref.
t() << std::endl;
94 std::vector<vpPoint> pts;
100 pts.push_back(
vpPoint(0.0, -val, val2));
101 pts.push_back(
vpPoint(0.0, val, val2));
102 pts.push_back(
vpPoint(-val, val, val2));
105 pts.push_back(
vpPoint(-val, -val / 2.0, val2));
106 pts.push_back(
vpPoint(val, val / 2.0, val2));
109 pts.push_back(
vpPoint(0.0, 0.0, -1.5));
126 pts[0].project(cMo_ref);
127 pts[1].project(cMo_ref);
128 pts[2].project(cMo_ref);
130 pts[3].project(cMo_ref);
131 pts[4].project(cMo_ref);
133 pts[5].project(cMo_ref);
156#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
161 vp_createPointClass cpClass;
162 int (vp_createPointClass::*ptrClass)(
vpFeaturePoint &,
const vpPoint &) = &vp_createPointClass::vp_createPoint;
175 std::cout <<
"\nPose used as initialisation of the pose computation : " << std::endl;
176 std::cout << pose_est.
t() << std::endl;
184 std::cout <<
"\nEstimated pose from visual features : " << std::endl;
186 std::cout <<
"\nRobust estimated pose from visual features : " << std::endl;
189 std::cout << pose_est.
t() << std::endl;
191 std::cout <<
"\nResulting covariance (Diag): " << std::endl;
193 std::cout << covariance[0][0] <<
" " << covariance[1][1] <<
" " << covariance[2][2] <<
" " << covariance[3][3] <<
" "
194 << covariance[4][4] <<
" " << covariance[5][5] <<
" " << std::endl;
197 for (
unsigned int i = 0; i < 6; i++) {
198 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
202 std::cout <<
"\nPose is " << (test_fail ?
"badly" :
"well") <<
" estimated\n" << std::endl;
209#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
211 if (test_pose(
false))
223 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << 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)
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.
Definition of the vpImage class member functions.
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 addFeatureVanishingPoint(const vpPoint &)
void setVerbose(const bool &mode)
void setVVSIterMax(const unsigned int &val)
void addFeaturePoint3D(const vpPoint &)
void addFeaturePoint(const vpPoint &)
void setCovarianceComputation(const bool &flag)
void addFeatureEllipse(const vpCircle &)
void addSpecificFeature(RetType(*fct_ptr)(ArgsFunc...), Args &&...args)
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)