46 #include <visp3/core/vpMath.h> 47 #include <visp3/core/vpRotationMatrix.h> 48 #include <visp3/io/vpParseArgv.h> 49 #include <visp3/core/vpQuaternionVector.h> 56 static unsigned int cpt = 0;
58 bool test(
const std::string &s,
const vpArray2D<double> &v,
const std::vector<double> &bench)
60 std::cout <<
"** Test " << ++cpt << std::endl;
61 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
62 if(bench.size() != v.
size()) {
63 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
66 for (
unsigned int i=0; i<v.
size(); i++) {
67 if (std::fabs(v.
data[i]-bench[i]) > std::fabs(v.
data[i])*std::numeric_limits<double>::epsilon()) {
68 std::cout <<
"Test fails: bad content" << std::endl;
76 bool test(
const std::string &s,
const vpRotationVector &v,
const double &bench)
78 std::cout <<
"** Test " << ++cpt << std::endl;
79 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
80 for (
unsigned int i=0; i<v.
size(); i++) {
81 if (std::fabs(v[i]-bench) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
82 std::cout <<
"Test fails: bad content" << std::endl;
97 if (test(
"r1", r1, bench1) ==
false)
101 if (test(
"r2", r2, bench1) ==
false)
109 if (test(
"r3", r3, bench1) ==
false)
112 std::cout <<
"** Test " << ++cpt << std::endl;
113 for(
unsigned int i=0; i<r3.
size(); i++) {
114 if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
115 std::cout <<
"Test fails: bad content" << std::endl;
122 if (test(
"r4", r4, bench2) ==
false)
126 if (test(
"r5", r5, bench1) ==
false)
132 if (test(
"r1", r1, bench1) ==
false)
136 if (test(
"r2", r2, bench1) ==
false)
144 if (test(
"r3", r3, bench1) ==
false)
147 std::cout <<
"** Test " << ++cpt << std::endl;
148 for(
unsigned int i=0; i<r3.
size(); i++) {
149 if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
150 std::cout <<
"Test fails: bad content" << std::endl;
157 if (test(
"r4", r4, bench2) ==
false)
161 if (test(
"r5", r5, bench1) ==
false)
167 if (test(
"r1", r1, bench1) ==
false)
171 if (test(
"r2", r2, bench1) ==
false)
179 if (test(
"r3", r3, bench1) ==
false)
182 std::cout <<
"** Test " << ++cpt << std::endl;
183 for(
unsigned int i=0; i<r3.
size(); i++) {
184 if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
185 std::cout <<
"Test fails: bad content" << std::endl;
192 if (test(
"r4", r4, bench2) ==
false)
196 if (test(
"r5", r5, bench1) ==
false)
202 if (test(
"r1", r1, bench1) ==
false)
206 if (test(
"r2", r2, bench1) ==
false)
214 if (test(
"r3", r3, bench1) ==
false)
217 std::cout <<
"** Test " << ++cpt << std::endl;
218 for(
unsigned int i=0; i<r3.
size(); i++) {
219 if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
220 std::cout <<
"Test fails: bad content" << std::endl;
227 if (test(
"r4", r4, bench2) ==
false)
231 if (test(
"r5", r5, bench1) ==
false)
237 if (test(
"r1", r1, bench1) ==
false)
241 if (test(
"r2", r2, bench1) ==
false)
249 if (test(
"r3", r3, bench1) ==
false)
252 std::cout <<
"** Test " << ++cpt << std::endl;
253 for(
unsigned int i=0; i<r3.
size(); i++) {
254 if (std::fabs(r3[i]-bench1[i]) > std::fabs(r3[i])*std::numeric_limits<double>::epsilon()) {
255 std::cout <<
"Test fails: bad content" << std::endl;
262 if (test(
"r4", r4, bench2) ==
false)
266 if (test(
"r5", r5, bench1) ==
false)
271 for(
int i=-10;i<10;i++){
272 for(
int j=-10;j<10;j++){
276 std::cout <<
"Initialization " <<std::endl ;
282 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl ;
283 std::cout <<
"u=" << u << std::endl ;
285 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl ;
288 std::cout <<
"Matrix R" ;
289 if (R.
isARotationMatrix()==1) std::cout <<
" is a rotation matrix " << std::endl ;
290 else 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 ;
306 std::cout <<
"From vpRxyzVector to vpThetaUVector " << std::endl ;
307 std::cout <<
" use From vpRxyzVector to vpRotationMatrix " << std::endl ;
308 std::cout <<
" use From vpRotationMatrix to vpThetaUVector " << std::endl ;
314 std::cout << std::endl ;
315 std::cout <<
"result : should equivalent to the first one " << std::endl ;
321 tuBuildFromEu.
extract(theta2, u2);
322 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl ;
323 std::cout <<
"u=" << u2 << std::endl ;
325 assert(
vpMath::abs(theta2-theta)<std::numeric_limits<double>::epsilon()*1e10);
326 assert(
vpMath::abs(u[0]-u2[0])<std::numeric_limits<double>::epsilon()*1e10);
327 assert(
vpMath::abs(u[1]-u2[1])<std::numeric_limits<double>::epsilon()*1e10);
328 assert(
vpMath::abs(u[2]-u2[2])<std::numeric_limits<double>::epsilon()*1e10);
331 std::cout <<
"Initialization vpRzyzVector " <<std::endl ;
332 std::cout << rzyz << std::endl ;
333 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl ;
335 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl ;
338 std::cout << rzyz_final << std::endl ;
341 std::cout <<
"Initialization vpRzyxVector " <<std::endl ;
342 std::cout << rzyx << std::endl ;
343 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl ;
345 std::cout << R << std::endl ;
346 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl ;
349 std::cout << rzyx_final << std::endl ;
352 std::cout <<
"All tests succeed" << std::endl;
356 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
Return the number of rows of the 2D array.
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
Return the number of columns of the 2D array.
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