 |
Visual Servoing Platform
version 3.3.0
|
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpRotationMatrix.h>
53 #include <visp3/core/vpThetaUVector.h>
54 #include <visp3/vision/vpHomography.h>
56 bool test(
const std::string &s,
const vpHomography &H,
const std::vector<double> &bench)
58 static unsigned int cpt = 0;
59 std::cout <<
"** Test " << ++cpt << std::endl;
60 std::cout << s <<
"(" << H.
getRows() <<
"," << H.
getCols() <<
") = \n[" << H <<
"]" << std::endl;
61 if (bench.size() != H.
size()) {
62 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
65 for (
unsigned int i = 0; i < H.
size(); i++) {
66 if (std::fabs(H.
data[i] - bench[i]) > std::fabs(H.
data[i]) * std::numeric_limits<double>::epsilon()) {
67 std::cout <<
"Test fails: bad content" << std::endl;
81 std::vector<double> bench(9, 0);
82 bench[0] = bench[4] = bench[8] = 1.;
84 if (test(
"H", H, bench) ==
false)
86 if (test(
"H", H / H[2][2], bench) ==
false)
92 std::cout <<
"Initialization " << std::endl;
95 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
102 std::cout <<
"M" << std::endl << M << std::endl;
107 std::cout <<
"H" << std::endl << H << std::endl;
114 std::cout <<
"R" << std::endl << R;
115 std::cout <<
"T" << std::endl << T.
t() << std::endl;
116 std::cout <<
"n" << std::endl << n.
t() << std::endl;
118 std::cout <<
"------------------------------------------------------" << std::endl;
123 std::cout <<
"Initialization " << std::endl;
126 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
137 std::cout <<
"M" << std::endl << M << std::endl;
142 std::cout <<
"H" << std::endl << H << std::endl;
149 std::cout <<
"R" << std::endl << R;
150 std::cout <<
"T" << std::endl << T.
t() << std::endl;
151 std::cout <<
"n" << std::endl << n.
t() << std::endl;
154 std::cout <<
"------------------------------------------------------" << std::endl;
168 std::cout <<
"M" << std::endl << M << std::endl;
173 std::cout <<
"H" << std::endl << H << std::endl;
179 std::cout <<
"R" << std::endl << R;
180 std::cout <<
"T" << std::endl << T.
t() << std::endl;
181 std::cout <<
"n" << std::endl << n.
t() << std::endl;
183 vpPlane p1(n[0], n[1], n[2], 1.0);
185 std::cout <<
"H" << std::endl << H << std::endl;
187 std::cout <<
"All tests succeed" << std::endl;
190 std::cout <<
"Catch an exception: " << e << std::endl;
static double rad(double deg)
void insert(const vpRotationMatrix &R)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
Implementation of column vector and the associated operations.
unsigned int getCols() const
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
Implementation of a rotation matrix and operations on such kind of matrices.
Type * data
Address of the first element of the data array.
Implementation of an homography and operations on homographies.
unsigned int size() const
Return the number of elements of the 2D array.
This class defines the container for a plane geometrical structure.
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
unsigned int getRows() const