Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testPoseFeatures.cpp

Compute the pose from different visual features.

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Compute the pose from visual features by virtual visual servoing.
*/
#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) {}
int vp_createPoint(vpFeaturePoint &fp, const vpPoint &v)
{
value += 1;
return value;
}
};
void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) { vpFeatureBuilder::create(fp, v); }
void vp_createLine(vpFeatureLine &fp, const vpLine &v) { vpFeatureBuilder::create(fp, v); }
#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;
vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
vpPoseVector pose_ref = vpPoseVector(cMo_ref);
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;
// 2D Point Feature
pts.push_back(vpPoint(0.0, -val, val2));
pts.push_back(vpPoint(0.0, val, val2));
pts.push_back(vpPoint(-val, val, val2));
// Segment Feature
pts.push_back(vpPoint(-val, -val / 2.0, val2));
pts.push_back(vpPoint(val, val / 2.0, val2));
// 3D point Feature
pts.push_back(vpPoint(0.0, 0.0, -1.5));
// Line Feature
vpLine line;
line.setWorldCoordinates(0.0, 1.0, 0.0, .0, 0.0, 0.0, 1.0, 0.0);
// Vanishing Point Feature
vpLine l1;
l1.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, 1.0, 0.0, 0.0, -0.25);
vpLine l2;
l2.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, -1.0, 0.0, 0.0, -0.25);
// Ellipse Feature
vpCircle circle;
circle.setWorldCoordinates(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.25);
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);
line.project(cMo_ref);
l1.project(cMo_ref);
l2.project(cMo_ref);
circle.project(cMo_ref);
pose.addFeaturePoint(pts[0]);
// pose.addFeaturePoint(pts[1]);
pose.addFeaturePoint(pts[2]);
pose.addFeaturePoint3D(pts[5]);
// pose.addFeatureSegment(pts[3],pts[4]);
//
// pose.addFeatureLine(line);
pose.addFeatureEllipse(circle);
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vp_createPointClass cpClass;
int (vp_createPointClass::*ptrClass)(vpFeaturePoint &, const vpPoint &) = &vp_createPointClass::vp_createPoint;
pose.addSpecificFeature(&cpClass, ptrClass, fp, pts[1]);
pose.addSpecificFeature(&vp_createLine, fl, line);
pose.addSpecificFeature(ptr, fs, pts[3], pts[4]);
#endif
pose.setVerbose(true);
pose.setLambda(0.6);
pose.setVVSIterMax(200);
vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
vpPoseVector pose_est = vpPoseVector(cMo_est);
std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
std::cout << pose_est.t() << std::endl;
if (!use_robust)
pose.computePose(cMo_est);
else
if (!use_robust)
std::cout << "\nEstimated pose from visual features : " << std::endl;
else
std::cout << "\nRobust estimated pose from visual features : " << std::endl;
pose_est.buildFrom(cMo_est);
std::cout << pose_est.t() << std::endl;
std::cout << "\nResulting covariance (Diag): " << std::endl;
vpMatrix covariance = pose.getCovarianceMatrix();
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;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
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...
Definition vpCircle.h:87
void setWorldCoordinates(const vpColVector &oP)
Definition vpCircle.cpp:57
error that can be emitted by ViSP classes.
Definition vpException.h:59
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.
Definition vpImage.h:135
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
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)
Definition vpLine.cpp:82
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
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.
vpRowVector t() const
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)