Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
testFeatureMoment.cpp
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 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:
* Example of visual servoing with moments using a polygon as object container
*/
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/vs/vpServo.h>
#include <iostream>
#include <limits>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
// initialize scene in the interface
void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
vpMomentObject &dst); // launch the test
void planeToABC(const vpPlane &pl, double &A, double &B, double &C);
int test(double x, double y, double z, double alpha);
// Compute a set of parallel positions and check if the matrix is in the right
// form;
int main()
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
try {
int sum = 0;
for (double i = -0.2; i < 0.2; i += 0.1) {
for (double j = -0.2; j < 0.2; j += 0.1) {
for (double k = -vpMath::rad(30); k < vpMath::rad(30); k += vpMath::rad(10)) {
for (double l = 0.5; l < 1.5; l += 0.1) {
sum += test(i, j, l, k);
}
}
}
}
if (sum < 0)
return EXIT_FAILURE;
else
return EXIT_SUCCESS;
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
#else
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
int test(double x, double y, double z, double alpha)
{
// intial pose
vpHomogeneousMatrix cMo(x, y, z, -vpMath::rad(0), vpMath::rad(0), alpha);
// Desired pose
// source and destination objects for moment manipulation
// init and run the simulation
initScene(cMo, cdMo, src, dst); // initialize graphical scene (for
// interface)
vpMatrix mat = execute(cMo, cdMo, src, dst);
if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
return -1;
return 0;
}
void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
{
std::vector<vpPoint> src_pts;
std::vector<vpPoint> dst_pts;
double x[5] = { 0.2, 0.2, -0.2, -0.2, 0.2 };
double y[5] = { -0.1, 0.1, 0.1, -0.1, -0.1 };
int nbpoints = 4;
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i], y[i], 0.0);
p.track(cMo);
src_pts.push_back(p);
}
src.fromVector(src_pts);
for (int i = 0; i < nbpoints; i++) {
vpPoint p(x[i], y[i], 0.0);
p.track(cdMo);
dst_pts.push_back(p);
}
dst.fromVector(dst_pts);
}
vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
{
vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; // current or desired
vpServo task;
// A,B,C parameters of source and destination plane
double A;
double B;
double C;
double Ad;
double Bd;
double Cd;
// init main object: using moments up to order 6
// Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
planeToABC(pl, A, B, C);
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
planeToABC(pl, Ad, Bd, Cd);
// extracting initial position (actually we only care about Zdst)
cdMo.extract(vec);
// don't need to be specific, vpMomentCommon automatically loads
// Xg,Yg,An,Ci,Cj,Alpha moments
vec[2]);
vec[2]);
// same thing with common features
vpFeatureMomentCommon featureMoments(moments);
vpFeatureMomentCommon featureMomentsDes(momentsDes);
moments.updateAll(src);
momentsDes.updateAll(dst);
featureMoments.updateAll(A, B, C);
featureMomentsDes.updateAll(Ad, Bd, Cd);
// setup the interaction type
task.setInteractionMatrixType(interaction_type);
task.addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
task.addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
// the moments are different in case of a symmetric object
task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
(1 << 10) | (1 << 11));
task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
task.setLambda(0.4);
return mat;
}
void planeToABC(const vpPlane &pl, double &A, double &B, double &C)
{
A = -pl.getA() / pl.getD();
B = -pl.getB() / pl.getD();
C = -pl.getC() / pl.getD();
}
error that can be emitted by ViSP classes.
Definition: vpException.h:60
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition: vpMath.h:129
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromVector(std::vector< vpPoint > &points)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:391
double getD() const
Definition: vpPlane.h:106
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:88
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:380
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void setLambda(double c)
Definition: vpServo.h:986
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:452
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
vpServoIteractionMatrixType
Definition: vpServo.h:196
@ CURRENT
Definition: vpServo.h:202
Class that consider the case of a translation vector.