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)