50 #include <visp/vpMath.h>
51 #include <visp/vpRotationMatrix.h>
52 #include <visp/vpParseArgv.h>
53 #include <visp/vpQuaternionVector.h>
60 #define GETOPTARGS "h"
67 void usage(
const char *name,
const char *badparam)
70 Tests transformation within various representations of rotation.\n\
81 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
90 bool getOptions(
int argc,
const char **argv)
97 case 'h': usage(argv[0], NULL);
return false;
break;
100 usage(argv[0], optarg);
105 if ((c == 1) || (c == -1)) {
107 usage(argv[0], NULL);
108 std::cerr <<
"ERROR: " << std::endl;
109 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
118 main(
int argc,
const char ** argv)
121 if (getOptions(argc, argv) ==
false) {
125 for(
int i=-10;i<10;i++){
126 for(
int j=-10;j<10;j++){
130 std::cout <<
"Initialization " <<std::endl ;
134 tu.extract(theta, u);
136 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl ;
137 std::cout <<
"u=" << u << std::endl ;
139 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl ;
142 std::cout <<
"Matrix R" ;
143 if (R.
isARotationMatrix()==1) std::cout <<
" is a rotation matrix " << std::endl ;
144 else std::cout <<
" is not a rotation matrix " << std::endl ;
146 std::cout << R << std::endl ;
148 std::cout <<
"From vpRotationMatrix to vpQuaternionVector " << std::endl ;
150 std::cout << q <<std::endl ;
153 std::cout <<
"From vpQuaternionVector to vpRotationMatrix " << std::endl ;
155 std::cout <<
"From vpRotationMatrix to vpRxyzVector " << std::endl ;
157 std::cout << RxyzBuildFromR <<std::endl ;
160 std::cout <<
"From vpRxyzVector to vpThetaUVector " << std::endl ;
161 std::cout <<
" use From vpRxyzVector to vpRotationMatrix " << std::endl ;
162 std::cout <<
" use From vpRotationMatrix to vpThetaUVector " << std::endl ;
168 std::cout << std::endl ;
169 std::cout <<
"result : should equivalent to the first one " << std::endl ;
175 tuBuildFromEu.
extract(theta2, u2);
176 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl ;
177 std::cout <<
"u=" << u2 << std::endl ;
179 assert(
vpMath::abs(theta2-theta)<std::numeric_limits<double>::epsilon()*1e10);
180 assert(
vpMath::abs(u[0]-u2[0])<std::numeric_limits<double>::epsilon()*1e10);
181 assert(
vpMath::abs(u[1]-u2[1])<std::numeric_limits<double>::epsilon()*1e10);
182 assert(
vpMath::abs(u[2]-u2[2])<std::numeric_limits<double>::epsilon()*1e10);
185 std::cout <<
"Initialization vpRzyzVector " <<std::endl ;
186 std::cout << rzyz << std::endl ;
187 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl ;
189 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl ;
192 std::cout << rzyz_final << std::endl ;
196 std::cout <<
"Initialization vpRzyxVector " <<std::endl ;
197 std::cout << rzyx << std::endl ;
198 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl ;
200 std::cout << R << std::endl ;
201 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl ;
204 std::cout << rzyx_final << std::endl ;