ViSP  2.6.2
testRobotViper850Pose.cpp
1 /****************************************************************************
2  *
3  * $Id: testRobotViper850Pose.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 
55 #include <visp/vpConfig.h>
56 #include <visp/vpImage.h>
57 #include <visp/vpDisplayX.h>
58 #include <visp/vpRobotViper850.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>
66 #include <iostream>
67 #if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2)
68 
69 int main()
70 {
71  try {
72  // Create an image B&W container
74 
75  // Create a firewire grabber based on libdc1394-2.x
76  bool reset = false;
77  vp1394TwoGrabber g(reset);
78 
79  // Grab an image from the firewire camera
80  g.acquire(I);
81 
82  // Create an image viewer for the image
83  vpDisplayX display(I);
84 
85  // Display the image
87  vpDisplay::flush(I) ;
88 
89  // Define a squared target
90  // The target is made of 4 planar points (square dim = 0.077m)
91  double sdim = 0.077; // square width and height
92  vpPoint target[4] ;
93  // Set the point world coordinates (x,y,z) in the object frame
94  // o ----> x
95  // |
96  // |
97  // \/
98  // y
99  target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
100  target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
101  target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
102  target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
103 
104  // Image processing to extract the 2D coordinates in sub-pixels of the 4
105  // points from the image acquired by the camera
106  // Creation of 4 trackers
107  vpDot dot[4];
108  vpImagePoint cog;
109  for (int i=0; i < 4; i ++) {
110  dot[i].setGraphics(true); // to display the tracking results
111  std::cout << "Click on dot " << i << std::endl;
112  dot[i].initTracking( I );
113  // The tracker computes the sub-pixels coordinates in the image
114  // i ----> u
115  // |
116  // |
117  // \/
118  // v
119  std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
120  // Flush the tracking results in the viewer
121  vpDisplay::flush(I) ;
122  }
123 
124  // Create an intrinsic camera parameters structure
125  vpCameraParameters cam;
126 
127  // Create a robot access
128  vpRobotViper850 robot;
129 
130  // Load the end-effector to camera frame transformation obtained
131  // using a camera intrinsic model with distortion
134 
135  // Get the intrinsic camera parameters associated to the image
136  robot.getCameraParameters(cam, I);
137 
138  // Using the camera parameters, compute the perspective projection (transform
139  // the dot sub-pixel coordinates into coordinates in the camera frame in
140  // meter)
141  for (int i=0; i < 4; i ++) {
142  double x=0, y=0 ; // coordinates of the dots in the camera frame
143  // c ----> x
144  // |
145  // |
146  // \/
147  // y
148  //pixel to meter conversion
149  cog = dot[i].getCog();
150  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
151  target[i].set_x(x) ;
152  target[i].set_y(y) ;
153  }
154 
155  // From now, in target[i], we have the 3D coordinates of a point in the
156  // object frame, and their correspondances in the camera frame. We can now
157  // compute the pose cMo between the camera and the object.
158  vpPose pose;
159  // Add the 4 points to compute the pose
160  for (int i=0; i < 4; i ++) {
161  pose.addPoint(target[i]) ;
162  }
163  // Create an homogeneous matrix for the camera to object transformation
164  // computed just bellow
167  vpRxyzVector r;
168  // Compute the pose: initialisation is done by Lagrange method, and the final
169  // pose is computed by the more accurate Virtual Visual Servoing method.
171 
172 
173  std::cout << "Pose cMo: " << std::endl << cMo;
174  cMo.extract(R);
175  r.buildFrom(R);
176  std::cout << " rotation: "
177  << vpMath::deg(r[0]) << " "
178  << vpMath::deg(r[1]) << " "
179  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
180 
181  // Get the robot position in the reference frame
183  vpColVector p; // position x,y,z,rx,ry,rz
185  std::cout << "Robot pose in reference frame: " << p << std::endl;
187  t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
188  r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
189  R.buildFrom(r);
190  rMc.buildFrom(t, R);
191  std::cout << "Pose rMc: " << std::endl << rMc;
192  rMc.extract(R);
193  r.buildFrom(R);
194  std::cout << " rotation: "
195  << vpMath::deg(r[0]) << " "
196  << vpMath::deg(r[1]) << " "
197  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
198 
200  std::cout << "Robot pose in articular: " << p << std::endl;
201 
202  robot.get_fMc(p, rMc);
203  std::cout << "Pose rMc from MGD: " << std::endl << rMc;
204  rMc.extract(R);
205  r.buildFrom(R);
206  std::cout << " rotation: "
207  << vpMath::deg(r[0]) << " "
208  << vpMath::deg(r[1]) << " "
209  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
210 
212  rMo = rMc * cMo;
213  std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
214  rMo.extract(R);
215  r.buildFrom(R);
216  std::cout << " rotation: "
217  << vpMath::deg(r[0]) << " "
218  << vpMath::deg(r[1]) << " "
219  << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
220 
221  return 0;
222  }
223  catch(...) {
224  std::cout << "Test failed" << std::endl;
225  return 0;
226  }
227 }
228 #else
229 int main()
230 {
231  std::cout << "Sorry, test not valid. You should have an Viper850 robot..."
232  << std::endl;
233  return 0;
234 }
235 
236 #endif
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:99
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Control of Irisa's Viper S850 robot named Viper850.
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...
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
Class that defines what is a point.
Definition: vpPoint.h:65
The vpRotationMatrix considers the particular case of a rotation matrix.
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 getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
Definition: vpViper850.cpp:576
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.
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Definition: vpViper.cpp:597
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