45 #include <visp3/core/vpMath.h> 46 #include <visp3/core/vpQuaternionVector.h> 47 #include <visp3/core/vpRotationMatrix.h> 48 #include <visp3/io/vpParseArgv.h> 55 static unsigned int cpt = 0;
57 bool test(
const std::string &s,
const vpArray2D<double> &v,
const std::vector<double> &bench)
59 std::cout <<
"** Test " << ++cpt << std::endl;
60 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
61 if (bench.size() != v.
size()) {
62 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
65 for (
unsigned int i = 0; i < v.
size(); i++) {
66 if (std::fabs(v.
data[i] - bench[i]) > std::fabs(v.
data[i]) * std::numeric_limits<double>::epsilon()) {
67 std::cout <<
"Test fails: bad content" << std::endl;
75 bool test(
const std::string &s,
const vpRotationVector &v,
const double &bench)
77 std::cout <<
"** Test " << ++cpt << std::endl;
78 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
79 for (
unsigned int i = 0; i < v.
size(); i++) {
80 if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
81 std::cout <<
"Test fails: bad content" << std::endl;
95 if (test(
"r1", r1, bench1) ==
false)
99 if (test(
"r2", r2, bench1) ==
false)
107 if (test(
"r3", r3, bench1) ==
false)
110 std::cout <<
"** Test " << ++cpt << std::endl;
111 for (
unsigned int i = 0; i < r3.
size(); i++) {
112 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
113 std::cout <<
"Test fails: bad content" << std::endl;
120 if (test(
"r4", r4, bench2) ==
false)
124 if (test(
"r5", r5, bench1) ==
false)
130 if (test(
"r1", r1, bench1) ==
false)
134 if (test(
"r2", r2, bench1) ==
false)
142 if (test(
"r3", r3, bench1) ==
false)
145 std::cout <<
"** Test " << ++cpt << std::endl;
146 for (
unsigned int i = 0; i < r3.
size(); i++) {
147 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
148 std::cout <<
"Test fails: bad content" << std::endl;
155 if (test(
"r4", r4, bench2) ==
false)
159 if (test(
"r5", r5, bench1) ==
false)
165 if (test(
"r1", r1, bench1) ==
false)
169 if (test(
"r2", r2, bench1) ==
false)
177 if (test(
"r3", r3, bench1) ==
false)
180 std::cout <<
"** Test " << ++cpt << std::endl;
181 for (
unsigned int i = 0; i < r3.
size(); i++) {
182 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
183 std::cout <<
"Test fails: bad content" << std::endl;
190 if (test(
"r4", r4, bench2) ==
false)
194 if (test(
"r5", r5, bench1) ==
false)
200 if (test(
"r1", r1, bench1) ==
false)
204 if (test(
"r2", r2, bench1) ==
false)
212 if (test(
"r3", r3, bench1) ==
false)
215 std::cout <<
"** Test " << ++cpt << std::endl;
216 for (
unsigned int i = 0; i < r3.
size(); i++) {
217 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
218 std::cout <<
"Test fails: bad content" << std::endl;
225 if (test(
"r4", r4, bench2) ==
false)
229 if (test(
"r5", r5, bench1) ==
false)
235 if (test(
"r1", r1, bench1) ==
false)
239 if (test(
"r2", r2, bench1) ==
false)
247 if (test(
"r3", r3, bench1) ==
false)
250 std::cout <<
"** Test " << ++cpt << std::endl;
251 for (
unsigned int i = 0; i < r3.
size(); i++) {
252 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
253 std::cout <<
"Test fails: bad content" << std::endl;
260 if (test(
"r4", r4, bench2) ==
false)
264 if (test(
"r5", r5, bench1) ==
false)
269 for (
int i = -10; i < 10; i++) {
270 for (
int j = -10; j < 10; j++) {
274 std::cout <<
"Initialization " << std::endl;
280 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl;
281 std::cout <<
"u=" << u << std::endl;
283 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
286 std::cout <<
"Matrix R";
288 std::cout <<
" is a rotation matrix " << std::endl;
290 std::cout <<
" is not a rotation matrix " << std::endl;
292 std::cout << R << std::endl;
294 std::cout <<
"From vpRotationMatrix to vpQuaternionVector " << std::endl;
296 std::cout << q << std::endl;
299 std::cout <<
"From vpQuaternionVector to vpRotationMatrix " << std::endl;
301 std::cout <<
"From vpRotationMatrix to vpRxyzVector " << std::endl;
303 std::cout << RxyzBuildFromR << std::endl;
305 std::cout <<
"From vpRxyzVector to vpThetaUVector " << std::endl;
306 std::cout <<
" use From vpRxyzVector to vpRotationMatrix " << std::endl;
307 std::cout <<
" use From vpRotationMatrix to vpThetaUVector " << std::endl;
312 std::cout << std::endl;
313 std::cout <<
"result : should equivalent to the first one " << std::endl;
318 tuBuildFromEu.
extract(theta2, u2);
319 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl;
320 std::cout <<
"u=" << u2 << std::endl;
322 assert(
vpMath::abs(theta2 - theta) < std::numeric_limits<double>::epsilon() * 1e10);
323 assert(
vpMath::abs(u[0] - u2[0]) < std::numeric_limits<double>::epsilon() * 1e10);
324 assert(
vpMath::abs(u[1] - u2[1]) < std::numeric_limits<double>::epsilon() * 1e10);
325 assert(
vpMath::abs(u[2] - u2[2]) < std::numeric_limits<double>::epsilon() * 1e10);
328 std::cout <<
"Initialization vpRzyzVector " << std::endl;
329 std::cout << rzyz << std::endl;
330 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl;
332 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl;
335 std::cout << rzyz_final << std::endl;
338 std::cout <<
"Initialization vpRzyxVector " << std::endl;
339 std::cout << rzyx << std::endl;
340 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl;
342 std::cout << R << std::endl;
343 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl;
346 std::cout << rzyx_final << std::endl;
349 std::cout <<
"All tests succeed" << std::endl;
352 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of a generic rotation vector.
void set(const double x, const double y, const double z, const double w)
void extract(double &theta, vpColVector &u) const
vpColVector extract(unsigned int r, unsigned int colsize) const
vpRzyzVector buildFrom(const vpRotationMatrix &R)
error that can be emited by ViSP classes.
unsigned int getRows() const
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 a rotation vector as Euler angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static Type abs(const Type &x)
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as quaternion angle minimal representation.
vpRzyxVector buildFrom(const vpRotationMatrix &R)
static double rad(double deg)
static double deg(double rad)
Implementation of column vector and the associated operations.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
bool isARotationMatrix() const