ViSP  2.6.2
testRobotAfma6Pose.cpp
1 /****************************************************************************
2  *
3  * $Id: testRobotAfma6Pose.cpp 3619 2012-03-09 17:28:57Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Test for Afma 6 dof robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
54 #include <visp/vpImage.h>
55 #include <visp/vpDisplayX.h>
56 #include <visp/vpRobotAfma6.h>
57 #include <visp/vpCameraParameters.h>
58 #include <visp/vpPixelMeterConversion.h>
59 #include <visp/vp1394TwoGrabber.h>
60 #include <visp/vpPoint.h>
61 #include <visp/vpDot.h>
62 #include <visp/vpPose.h>
63 #include <visp/vpDebug.h>
64 #include <iostream>
65 #if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394_2)
66 
67 int main()
68 {
69  try {
70  // Create an image B&W container
72 
73  // Create a firewire grabber based on libdc1394-2.x
75 
76  // Grab an image from the firewire camera
77  g.acquire(I);
78 
79  // Create an image viewer for the image
80  vpDisplayX display(I);
81 
82  // Display the image
84  vpDisplay::flush(I) ;
85 
86  // Define a squared target
87  // The target is made of 4 planar points (square dim = 0.077m)
88  double sdim = 0.077; // square width and height
89  vpPoint target[4] ;
90  // Set the point world coordinates (x,y,z) in the object frame
91  // o ----> x
92  // |
93  // |
94  // \/
95  // y
96  target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
97  target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
98  target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
99  target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
100 
101  // Image processing to extract the 2D coordinates in sub-pixels of the 4
102  // points from the image acquired by the camera
103  // Creation of 4 trackers
104  vpDot dot[4];
105  vpImagePoint cog;
106  for (int i=0; i < 4; i ++) {
107  dot[i].setGraphics(true); // to display the tracking results
108  std::cout << "Click on dot " << i << std::endl;
109  dot[i].initTracking( I );
110  // The tracker computes the sub-pixels coordinates in the image
111  // i ----> u
112  // |
113  // |
114  // \/
115  // v
116  std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
117  // Flush the tracking results in the viewer
118  vpDisplay::flush(I) ;
119  }
120 
121  // Create an intrinsic camera parameters structure
122  vpCameraParameters cam;
123 
124  // Create a robot access
125  vpRobotAfma6 robot;
126 
127  // Load the end-effector to camera frame transformation obtained
128  // using a camera intrinsic model with distortion
131 
132  // Get the intrinsic camera parameters associated to the image
133  robot.getCameraParameters(cam, I);
134 
135  // Using the camera parameters, compute the perspective projection (transform
136  // the dot sub-pixel coordinates into coordinates in the camera frame in
137  // meter)
138  for (int i=0; i < 4; i ++) {
139  double x=0, y=0 ; // coordinates of the dots in the camera frame
140  // c ----> x
141  // |
142  // |
143  // \/
144  // y
145  //pixel to meter conversion
146  cog = dot[i].getCog();
147  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
148  target[i].set_x(x) ;
149  target[i].set_y(y) ;
150  }
151 
152  // From now, in target[i], we have the 3D coordinates of a point in the
153  // object frame, and their correspondances in the camera frame. We can now
154  // compute the pose cMo between the camera and the object.
155  vpPose pose;
156  // Add the 4 points to compute the pose
157  for (int i=0; i < 4; i ++) {
158  pose.addPoint(target[i]) ;
159  }
160  // Create an homogeneous matrix for the camera to object transformation
161  // computed just bellow
164  vpRxyzVector r;
165  // Compute the pose: initialisation is done by Lagrange method, and the final
166  // pose is computed by the more accurate Virtual Visual Servoing method.
168 
169 
170  std::cout << "Pose cMo: " << std::endl << cMo;
171  cMo.extract(R);
172  r.buildFrom(R);
173  std::cout << " rotation: "
174  << vpMath::deg(r[0]) << " "
175  << vpMath::deg(r[1]) << " "
176  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
177 
178  // Get the robot position in the reference frame
180  vpColVector p; // position x,y,z,rx,ry,rz
182  std::cout << "Robot pose in reference frame: " << p << std::endl;
184  t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
185  r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
186  R.buildFrom(r);
187  rMc.buildFrom(t, R);
188  std::cout << "Pose rMc: " << std::endl << rMc;
189  rMc.extract(R);
190  r.buildFrom(R);
191  std::cout << " rotation: "
192  << vpMath::deg(r[0]) << " "
193  << vpMath::deg(r[1]) << " "
194  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
195 
197  std::cout << "Robot pose in articular: " << p << std::endl;
198 
199  robot.get_fMc(p, rMc);
200  std::cout << "Pose rMc from MGD: " << std::endl << rMc;
201  rMc.extract(R);
202  r.buildFrom(R);
203  std::cout << " rotation: "
204  << vpMath::deg(r[0]) << " "
205  << vpMath::deg(r[1]) << " "
206  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
207 
209  rMo = rMc * cMo;
210  std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
211  rMo.extract(R);
212  r.buildFrom(R);
213  std::cout << " rotation: "
214  << vpMath::deg(r[0]) << " "
215  << vpMath::deg(r[1]) << " "
216  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
217 
218  return 0;
219  }
220  catch(...) {
221  std::cout << "Test failed" << std::endl;
222  return 0;
223  }
224 }
225 #else
226 int main()
227 {
228  std::cout << "Sorry, test not valid. You should have an Afma6 robot..."
229  << std::endl;
230  return 0;
231 }
232 
233 #endif
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Definition: vpAfma6.cpp:729
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
Definition: vpAfma6.cpp:1226
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.h:183
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Control of Irisa's gantry robot named Afma6.
Definition: vpRobotAfma6.h:214
Class that defines what is a point.
Definition: vpPoint.h:65
The vpRotationMatrix considers the particular case of a rotation matrix.
void init(void)
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
vpImagePoint getCog() const
Definition: vpDot.h:249
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:80
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.h:185
void extract(vpRotationMatrix &R) const
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void setGraphics(const bool activate)
Definition: vpDot.h:225
This tracker is meant to track a dot (connex pixels with same gray level) on a vpImage.
Definition: vpDot.h:80
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:298
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:148
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:188
void initTracking(const vpImage< unsigned char > &I)
Definition: vpDot.cpp:638
Class that consider the case of a translation vector.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74