54 #include <visp/vpImage.h>
55 #include <visp/vpDisplayX.h>
56 #include <visp/vpDisplayOpenCV.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpRobotAfma6.h>
59 #include <visp/vpCameraParameters.h>
60 #include <visp/vpPixelMeterConversion.h>
61 #include <visp/vp1394TwoGrabber.h>
62 #include <visp/vpPoint.h>
63 #include <visp/vpDot.h>
64 #include <visp/vpPose.h>
65 #include <visp/vpDebug.h>
67 #if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394_2)
83 vpDisplayX display(I,100,100,
"Current image") ;
84 #elif defined(VISP_HAVE_OPENCV)
86 #elif defined(VISP_HAVE_GTK)
114 for (
int i=0; i < 4; i ++) {
116 std::cout <<
"Click on dot " << i << std::endl;
124 std::cout <<
" Coordinates: " << dot[i].
getCog() << std::endl;
146 for (
int i=0; i < 4; i ++) {
165 for (
int i=0; i < 4; i ++) {
178 std::cout <<
"Pose cMo: " << std::endl << cMo;
181 std::cout <<
" rotation: "
184 <<
vpMath::deg(r[2]) <<
" deg" << std::endl << std::endl;
190 std::cout <<
"Robot pose in reference frame: " << p << std::endl;
192 t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
193 r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
196 std::cout <<
"Pose rMc: " << std::endl << rMc;
199 std::cout <<
" rotation: "
202 <<
vpMath::deg(r[2]) <<
" deg" << std::endl << std::endl;
205 std::cout <<
"Robot pose in articular: " << p << std::endl;
208 std::cout <<
"Pose rMc from MGD: " << std::endl << rMc;
211 std::cout <<
" rotation: "
214 <<
vpMath::deg(r[2]) <<
" deg" << std::endl << std::endl;
218 std::cout <<
"Pose rMo = rMc * cMo: " << std::endl << rMo;
221 std::cout <<
" rotation: "
224 <<
vpMath::deg(r[2]) <<
" deg" << std::endl << std::endl;
229 std::cout <<
"Test failed" << std::endl;
236 std::cout <<
"Sorry, test not valid. You should have an Afma6 robot..."