 |
Visual Servoing Platform
version 3.2.0
|
49 #include <visp3/core/vpPoseVector.h>
51 bool test(
const std::string &s,
const vpArray2D<double> &A,
const std::vector<double> &bench)
53 static unsigned int cpt = 0;
54 std::cout <<
"** Test " << ++cpt << std::endl;
55 std::cout << s <<
"(" << A.
getRows() <<
"," << A.
getCols() <<
") =" << A << std::endl;
56 if (bench.size() != A.
size()) {
57 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
60 for (
unsigned int i = 0; i < A.
size(); i++) {
61 if (std::fabs(A.
data[i] - bench[i]) > std::fabs(A.
data[i]) * std::numeric_limits<double>::epsilon()) {
62 std::cout <<
"Test fails: bad content" << std::endl;
73 std::vector<double> bench(6, 0);
75 if (test(
"p", p, bench) ==
false)
77 p[0] = bench[0] = 0.1;
78 p[1] = bench[1] = 0.2;
79 p[2] = bench[2] = 0.3;
84 if (test(
"p", p, bench) ==
false)
88 if (test(
"p1", p1, bench) ==
false)
91 if (test(
"p2", p2, bench) ==
false)
94 if (test(
"p3", p3, bench) ==
false)
97 p4.
set(p[0], p[1], p[2], p[3], p[4], p[5]);
98 if (test(
"p4", p4, bench) ==
false)
104 if (test(
"p5", p5, bench) ==
false)
108 if (test(
"p6", p6, bench) ==
false)
113 if (test(
"p7", p7, bench) ==
false)
117 if (test(
"p8", p8, bench) ==
false)
122 if (test(
"p9", p9, bench) ==
false)
126 if (test(
"p10", p10, bench) ==
false)
129 std::cout <<
"All tests succeed" << std::endl;
static double rad(double deg)
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
unsigned int getCols() const
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a pose vector and operations on poses.
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
unsigned int getRows() const