ViSP  2.6.2
servoSimuSphere.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimuSphere.cpp 3625 2012-03-13 19:47:29Z 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  * Demonstration of the wireframe simulator with a simple visual servoing
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
55 #include <visp/vpImage.h>
56 #include <visp/vpImageIo.h>
57 #include <visp/vpDisplayOpenCV.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGTK.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpDisplayD3D.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpTime.h>
64 
65 #include <visp/vpMath.h>
66 #include <visp/vpHomogeneousMatrix.h>
67 #include <visp/vpSphere.h>
68 #include <visp/vpGenericFeature.h>
69 #include <visp/vpServo.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpFeatureBuilder.h>
72 #include <visp/vpParseArgv.h>
73 #include <visp/vpIoTools.h>
74 #include <visp/vpVelocityTwistMatrix.h>
75 #include <visp/vpTranslationVector.h>
76 #include <visp/vpWireFrameSimulator.h>
77 #include <stdlib.h>
78 #include <cmath> // std::fabs
79 #include <limits> // numeric_limits
80 #define GETOPTARGS "dh"
81 
82 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK))
83 
92 void usage(const char *name, const char *badparam)
93 {
94  fprintf(stdout, "\n\
95 Demonstration of the wireframe simulator with a simple visual servoing.\n\
96  \n\
97 The visual servoing consists in bringing the camera at a desired position from the object.\n\
98  \n\
99 The visual features used to compute the pose of the camera and thus the control law are special moments computed with the sphere's parameters.\n\
100  \n\
101 SYNOPSIS\n\
102  %s [-d] [-h]\n", name);
103 
104  fprintf(stdout, "\n\
105 OPTIONS: Default\n\
106  -d \n\
107  Turn off the display.\n\
108  \n\
109  -h\n\
110  Print the help.\n");
111 
112  if (badparam)
113  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
114 }
115 
127 bool getOptions(int argc, const char **argv, bool &display)
128 {
129  const char *optarg;
130  int c;
131  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
132 
133  switch (c) {
134  case 'd': display = false; break;
135  case 'h': usage(argv[0], NULL); return false; break;
136 
137  default:
138  usage(argv[0], optarg);
139  return false; break;
140  }
141  }
142 
143  if ((c == 1) || (c == -1)) {
144  // standalone param or error
145  usage(argv[0], NULL);
146  std::cerr << "ERROR: " << std::endl;
147  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
148  return false;
149  }
150 
151  return true;
152 }
153 
154 
155 /*
156  Computes the virtual visual features corresponding to the sphere and stores it in the generic feature.
157 
158  The visual feature vector is computed thanks to the following formula : s = {sx, sy, sz}
159  sx = gx*h2/(sqrt(h2+1)
160  sx = gy*h2/(sqrt(h2+1)
161  sz = sqrt(h2+1)
162 
163  with gx and gy the center of gravity of the ellipse,
164  with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
165  with n20,n02,n11 the second order moments of the sphere
166 */
167 void computeVisualFeatures(const vpSphere sphere, vpGenericFeature &s)
168 {
169  double gx = sphere.get_x();
170  double gy = sphere.get_y();
171  double m02 = sphere.get_mu02();
172  double m20 = sphere.get_mu20();
173  double m11 = sphere.get_mu11();
174  double h2;
175  //if (gx != 0 || gy != 0)
176  if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
177  h2 = (vpMath::sqr(gx)+vpMath::sqr(gy))/(4*m20*vpMath::sqr(gy)+4*m02*vpMath::sqr(gx)-8*m11*gx*gy);
178  else
179  h2 = 1/(4*m20);
180 
181  double sx = gx*h2/(sqrt(h2+1));
182  double sy = gy*h2/(sqrt(h2+1));
183  double sz = sqrt(h2+1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2));
184 
185 
186 
187  s.set_s(sx,sy,sz);
188 }
189 
190 /*
191  Computes the interaction matrix such as L = [-1/R*I3 [s]x]
192 
193  with R the radius of the sphere
194  with I3 the 3x3 identity matrix
195  with [s]x the skew matrix related to s
196 */
197 void computeInteractionMatrix(const vpGenericFeature s,const vpSphere sphere, vpMatrix &L)
198 {
199  L = 0;
200  L[0][0] = -1/sphere.getR();
201  L[1][1] = -1/sphere.getR();
202  L[2][2] = -1/sphere.getR();
203 
204  double s0,s1,s2;
205  s.get_s(s0,s1,s2);
206 
207  vpTranslationVector c(s0,s1,s2);
208  vpMatrix sk;
209  sk = c.skew();
210 
211  for(unsigned int i = 0; i < 3; i++)
212  for(unsigned int j = 0; j < 3; j++)
213  L[i][j+3] = sk[i][j];
214 }
215 
216 
217 int
218 main(int argc, const char ** argv)
219 {
220  bool opt_display = true;
221 
222  // Read the command line options
223  if (getOptions(argc, argv, opt_display) == false) {
224  exit (-1);
225  }
226 
227  vpImage<vpRGBa> Iint(480,640,255);
228  vpImage<vpRGBa> Iext1(480,640,255);
229  vpImage<vpRGBa> Iext2(480,640,255);
230 
231 #if defined VISP_HAVE_X11
232  vpDisplayX display[3];
233 #elif defined VISP_HAVE_OPENCV
234  vpDisplayOpenCV display[3];
235 #elif defined VISP_HAVE_GDI
236  vpDisplayGDI display[3];
237 #elif defined VISP_HAVE_D3D9
238  vpDisplayD3D display[3];
239 #elif defined VISP_HAVE_GTK
240  vpDisplayGTK display[3];
241 #endif
242 
243  if (opt_display)
244  {
245  try
246  {
247  // Display size is automatically defined by the image (I) size
248  display[0].init(Iint, 100, 100,"The internal view") ;
249  display[1].init(Iext1, 100, 100,"The first external view") ;
250  display[2].init(Iext2, 100, 100,"The second external view") ;
251  vpDisplay::setWindowPosition (Iint, 0, 0);
252  vpDisplay::setWindowPosition (Iext1, 700, 0);
253  vpDisplay::setWindowPosition (Iext2, 0, 550);
254  vpDisplay::display(Iint);
255  vpDisplay::flush(Iint);
256  vpDisplay::display(Iext1);
257  vpDisplay::flush(Iext1);
258  vpDisplay::display(Iext2);
259  vpDisplay::flush(Iext2);
260  }
261  catch(...)
262  {
263  vpERROR_TRACE("Error while displaying the image") ;
264  exit(-1);
265  }
266  }
267 
268  vpServo task;
269  vpRobotCamera robot ;
270  float sampling_time = 0.040f; // Sampling period in second
271  robot.setSamplingTime(sampling_time);
272 
273  //cMo initial
274  vpPoseVector cMoi(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
275 
276  vpHomogeneousMatrix cMo(cMoi);
277  robot.setPosition(cMo);
278 
279  //The sphere
280  vpSphere sphere(0,0,0,0.15);
281 
282  // Projection of the sphere
283  sphere.track(cMo);
284 
285  //Set the current visual feature
286  vpGenericFeature s(3);
287  computeVisualFeatures(sphere, s);
288 
289  //cMo desired
291 
292  // Projection of the points
293  sphere.track(cdMo);
294 
295  vpGenericFeature sd(3);
296  computeVisualFeatures(sphere, sd);
297 
298  vpMatrix L(3,6);
299  computeInteractionMatrix(sd,sphere,L);
300  sd.setInteractionMatrix(L);
301 
304 
306  vpVelocityTwistMatrix cVe(cMe);
307  task.set_cVe(cVe);
308 
309  vpMatrix eJe;
310  robot.get_eJe(eJe);
311  task.set_eJe(eJe);
312 
313  task.addFeature(s,sd) ;
314 
315  task.setLambda(10);
316 
317 
319 
320  //Set the scene
322 
323  //Set the initial and the desired position of the camera.
324  sim.setCameraPositionRelObj(cMoi) ;
325  sim.setDesiredCameraPosition(cdMo);
326 
327  //Set the External camera position
329  sim.setExternalCameraPosition(camMf);
330 
331  //Computes the position of a camera which is fixed in the object frame
332  vpHomogeneousMatrix camoMf;
333  vpHomogeneousMatrix temp(vpHomogeneousMatrix(0,0.0,2.5,0,vpMath::rad(140),0)*(sim.get_fMo().inverse()));
336  temp.extract(T);
337  temp.extract(R);
338  camoMf.buildFrom(T,R);
339 
340  //Set the parameters of the cameras (internal and external)
341  vpCameraParameters camera(1000,1000,320,240);
342  sim.setInternalCameraParameters(camera);
343  sim.setExternalCameraParameters(camera);
344 
345  int stop = 10;
346 
347  if (opt_display)
348  {
349  stop = 1000;
350  //Get the internal and external views
351  sim.getInternalImage(Iint);
352  sim.getExternalImage(Iext1);
353  sim.getExternalImage(Iext2,camoMf);
354 
355  //Display the object frame (current and desired position)
356  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
357  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
358 
359  //Display the object frame the world reference frame and the camera frame
360  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
361  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
362  vpDisplay::displayFrame(Iext1,camMf,camera,0.2,vpColor::none);
363 
364  //Display the world reference frame and the object frame
365  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
366  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
367 
368  vpDisplay::flush(Iint);
369  vpDisplay::flush(Iext1);
370  vpDisplay::flush(Iext2);
371 
372  std::cout << "Click on a display" << std::endl;
373  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext1,false) && !vpDisplay::getClick(Iext2,false)){};
374  }
375 
376  //Print the task
377  task.print() ;
378 
379  int iter = 0;
380  vpColVector v ;
381 
382  while(iter++ < stop)
383  {
384  if (opt_display)
385  {
386  vpDisplay::display(Iint) ;
387  vpDisplay::display(Iext1) ;
388  vpDisplay::display(Iext2) ;
389  }
390 
391  double t = vpTime::measureTimeMs();
392 
393  robot.get_eJe(eJe) ;
394  task.set_eJe(eJe) ;
395 
396  robot.getPosition(cMo) ;
397 
398  sphere.track(cMo);
399 
400  //Set the current visual feature
401  computeVisualFeatures(sphere, s);
402 
403  v = task.computeControlLaw() ;
405  sim.setCameraPositionRelObj(cMo);
406 
407  //Compute the position of the external view which is fixed in the object frame
408  vpHomogeneousMatrix temp(vpHomogeneousMatrix(0,0.0,2.5,0,vpMath::rad(150),0)*(sim.get_fMo().inverse()));
411  temp.extract(T);
412  temp.extract(R);
413  camoMf.buildFrom(T,R);
414 
415  if (opt_display)
416  {
417  //Get the internal and external views
418  sim.getInternalImage(Iint);
419  sim.getExternalImage(Iext1);
420  sim.getExternalImage(Iext2,camoMf);
421 
422  //Display the object frame (current and desired position)
423  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
424  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
425 
426  //Display the object frame the world reference frame and the camera frame
429 
431 
432  //Display the world reference frame and the object frame
433  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
434  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
435 
436  vpDisplay::flush(Iint);
437  vpDisplay::flush(Iext1);
438  vpDisplay::flush(Iext2);
439  }
440 
441  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
442 
443  vpTRACE("\t\t || s - s* || ") ;
444  std::cout << ( task.getError() ).sumSquare() <<std::endl ;
445  }
446 
447  task.print() ;
448  task.kill() ;
449 
450  return 0;
451 }
452 #else
453 int
454 main()
455 {
456  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
457 }
458 
459 #endif
double get_mu02() const
Definition: vpSphere.h:87
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
double getR() const
Definition: vpSphere.h:93
void set_s(const vpColVector &s)
set the value of all the features.
double get_x() const
Definition: vpSphere.h:83
void setSamplingTime(const double &delta_t)
Definition: vpRobotCamera.h:87
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
#define vpTRACE
Definition: vpDebug.h:401
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
double get_mu11() const
Definition: vpSphere.h:86
static const vpColor none
Definition: vpColor.h:177
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:250
void setCameraPositionRelObj(const vpHomogeneousMatrix cMo)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void track(const vpHomogeneousMatrix &cMo)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static double measureTimeMs()
Definition: vpTime.cpp:86
Class that defines what is a sphere.
Definition: vpSphere.h:64
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:227
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
void skew(const vpTranslationVector &t, vpMatrix &M)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
The vpRotationMatrix considers the particular case of a rotation matrix.
void get_s(vpColVector &s) const
get the value of all the features.
double get_mu20() const
Definition: vpSphere.h:85
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:108
void setExternalCameraPosition(const vpHomogeneousMatrix camMf)
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
vpColVector getError() const
Definition: vpServo.h:298
vpHomogeneousMatrix get_fMo() const
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
virtual void setWindowPosition(int winx, int winy)=0
static double sqr(double x)
Definition: vpMath.h:106
Class that defines the simplest robot: a free flying camera.
Definition: vpRobotCamera.h:65
double get_y() const
Definition: vpSphere.h:84
void getInternalImage(vpImage< vpRGBa > &I)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:235
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo)
void getPosition(vpColVector &q)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:346
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
static double rad(double deg)
Definition: vpMath.h:100
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject)
void get_eJe(vpMatrix &_eJe)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
void setInternalCameraParameters(const vpCameraParameters cam)
virtual bool getClick(bool blocking=true)=0
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
Class required to compute the visual servoing control law.
Definition: vpServo.h:150
void getExternalImage(vpImage< vpRGBa > &I)
void setExternalCameraParameters(const vpCameraParameters cam)
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
vpHomogeneousMatrix getExternalCameraPosition() const