35 #include <visp3/core/vpDebug.h>
36 #include <visp3/core/vpHomogeneousMatrix.h>
37 #include <visp3/core/vpMomentCommon.h>
38 #include <visp3/core/vpMomentDatabase.h>
39 #include <visp3/core/vpMomentObject.h>
40 #include <visp3/core/vpPlane.h>
41 #include <visp3/visual_features/vpFeatureMomentCommon.h>
42 #include <visp3/vs/vpServo.h>
53 void planeToABC(
const vpPlane &pl,
double &A,
double &B,
double &C);
54 int test(
double x,
double y,
double z,
double alpha);
60 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
63 for (
double i = -0.2; i < 0.2; i += 0.1) {
64 for (
double j = -0.2; j < 0.2; j += 0.1) {
66 for (
double l = 0.5; l < 1.5; l += 0.1) {
67 sum += test(i, j, l, k);
78 std::cout <<
"Catch an exception: " << e << std::endl;
82 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
87 int test(
double x,
double y,
double z,
double alpha)
99 initScene(cMo, cdMo, src, dst);
102 vpMatrix mat = execute(cMo, cdMo, src, dst);
104 if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
106 if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
108 if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
111 if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
113 if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
115 if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
118 if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
120 if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
122 if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
124 if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
127 if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
129 if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
131 if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
133 if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
136 if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
138 if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
140 if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
142 if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
145 if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
147 if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
149 if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
151 if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
160 std::vector<vpPoint> src_pts;
161 std::vector<vpPoint> dst_pts;
163 double x[5] = { 0.2, 0.2, -0.2, -0.2, 0.2 };
164 double y[5] = { -0.1, 0.1, 0.1, -0.1, -0.1 };
167 for (
int i = 0; i < nbpoints; i++) {
170 src_pts.push_back(p);
175 for (
int i = 0; i < nbpoints; i++) {
178 dst_pts.push_back(p);
204 planeToABC(pl, A, B, C);
208 planeToABC(pl, Ad, Bd, Cd);
226 moments.updateAll(src);
227 momentsDes.updateAll(dst);
229 featureMoments.updateAll(A, B, C);
230 featureMomentsDes.updateAll(Ad, Bd, Cd);
236 task.
addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
237 task.
addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
239 task.
addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
240 (1 << 10) | (1 << 11));
241 task.
addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
250 void planeToABC(
const vpPlane &pl,
double &A,
double &B,
double &C)
error that can be emitted by ViSP classes.
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)
Implementation of a matrix and operations on matrices.
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.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setABCD(double a, double b, double c, double d)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void setServo(const vpServoType &servo_type)
vpMatrix computeInteractionMatrix()
vpColVector computeControlLaw()
vpServoIteractionMatrixType
Class that consider the case of a translation vector.