Compute the pose from different visual features.
#include <iostream>
#include <limits>
#include <vector>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpPose.h>
#include <visp3/vision/vpPoseFeatures.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
class vp_createPointClass
{
public:
int value;
vp_createPointClass() : value(0) {}
{
value += 1;
return value;
}
};
#endif
#endif
int test_pose(bool use_robust)
{
if (use_robust)
std::cout << "** Test robust pose estimation from features\n" << std::endl;
else
std::cout << "** Test pose estimation from features\n" << std::endl;
std::cout << "Reference pose used to create the visual features : " << std::endl;
std::cout << pose_ref.
t() << std::endl;
std::vector<vpPoint> pts;
double val = 0.25;
double val2 = 0.0;
pts.push_back(
vpPoint(0.0, -val, val2));
pts.push_back(
vpPoint(0.0, val, val2));
pts.push_back(
vpPoint(-val, val, val2));
pts.push_back(
vpPoint(-val, -val / 2.0, val2));
pts.push_back(
vpPoint(val, val / 2.0, val2));
pts.push_back(
vpPoint(0.0, 0.0, -1.5));
pts[0].project(cMo_ref);
pts[1].project(cMo_ref);
pts[2].project(cMo_ref);
pts[3].project(cMo_ref);
pts[4].project(cMo_ref);
pts[5].project(cMo_ref);
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vp_createPointClass cpClass;
int (vp_createPointClass::*ptrClass)(
vpFeaturePoint &,
const vpPoint &) = &vp_createPointClass::vp_createPoint;
#endif
std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
std::cout << pose_est.
t() << std::endl;
if (!use_robust)
else
if (!use_robust)
std::cout << "\nEstimated pose from visual features : " << std::endl;
else
std::cout << "\nRobust estimated pose from visual features : " << std::endl;
std::cout << pose_est.
t() << std::endl;
std::cout << "\nResulting covariance (Diag): " << std::endl;
std::cout << covariance[0][0] << " " << covariance[1][1] << " " << covariance[2][2] << " " << covariance[3][3] << " "
<< covariance[4][4] << " " << covariance[5][5] << " " << std::endl;
int test_fail = 0;
for (unsigned int i = 0; i < 6; i++) {
if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
test_fail = 1;
}
std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated\n" << std::endl;
return test_fail;
}
int main()
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
try {
if (test_pose(false))
return EXIT_FAILURE;
if (test_pose(true))
return EXIT_FAILURE;
return EXIT_SUCCESS;
return EXIT_FAILURE;
}
#else
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
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)