38 #include <visp3/core/vpDebug.h> 39 #include <visp3/core/vpHomogeneousMatrix.h> 40 #include <visp3/core/vpMomentCommon.h> 41 #include <visp3/core/vpMomentDatabase.h> 42 #include <visp3/core/vpMomentObject.h> 43 #include <visp3/core/vpPlane.h> 44 #include <visp3/visual_features/vpFeatureMomentCommon.h> 45 #include <visp3/vs/vpServo.h> 56 void planeToABC(
const vpPlane &pl,
double &A,
double &B,
double &C);
57 int test(
double x,
double y,
double z,
double alpha);
65 for (
double i = -0.2; i < 0.2; i += 0.1) {
66 for (
double j = -0.2; j < 0.2; j += 0.1) {
68 for (
double l = 0.5; l < 1.5; l += 0.1) {
69 sum += test(i, j, l, k);
79 std::cout <<
"Catch an exception: " << e << std::endl;
84 int test(
double x,
double y,
double z,
double alpha)
96 initScene(cMo, cdMo, src, dst);
99 vpMatrix mat = execute(cMo, cdMo, src, dst);
101 if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
103 if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
105 if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
108 if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
110 if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
112 if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
115 if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
117 if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
119 if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
121 if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
124 if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
126 if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
128 if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
130 if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
133 if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
135 if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
137 if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
139 if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
142 if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
144 if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
146 if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
148 if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
157 std::vector<vpPoint> src_pts;
158 std::vector<vpPoint> dst_pts;
160 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
161 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
164 for (
int i = 0; i < nbpoints; i++) {
167 src_pts.push_back(p);
172 for (
int i = 0; i < nbpoints; i++) {
175 dst_pts.push_back(p);
202 planeToABC(pl, A, B, C);
206 planeToABC(pl, Ad, Bd, Cd);
238 (1 << 10) | (1 << 11));
249 void planeToABC(
const vpPlane &pl,
double &A,
double &B,
double &C)
Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
void setABCD(const double a, const double b, const double c, const double d)
Class for generic objects.
void extract(vpRotationMatrix &R) const
Class that defines what is a point.
vpMatrix computeInteractionMatrix()
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
vpServoIteractionMatrixType
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setType(vpObjectType input_type)
This class defines the container for a plane geometrical structure.
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Class that consider the case of a translation vector.