ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoSimuSphere.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimuSphere.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 
48 #include <stdlib.h>
49 #include <cmath> // std::fabs
50 #include <limits> // numeric_limits
51 
52 #include <visp/vpCameraParameters.h>
53 #include <visp/vpDisplayOpenCV.h>
54 #include <visp/vpDisplayX.h>
55 #include <visp/vpDisplayGTK.h>
56 #include <visp/vpDisplayGDI.h>
57 #include <visp/vpDisplayD3D.h>
58 #include <visp/vpFeatureBuilder.h>
59 #include <visp/vpGenericFeature.h>
60 #include <visp/vpHomogeneousMatrix.h>
61 #include <visp/vpImage.h>
62 #include <visp/vpImageIo.h>
63 #include <visp/vpIoTools.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpParseArgv.h>
66 #include <visp/vpRobotCamera.h>
67 #include <visp/vpServo.h>
68 #include <visp/vpSphere.h>
69 #include <visp/vpTime.h>
70 #include <visp/vpVelocityTwistMatrix.h>
71 #include <visp/vpWireFrameSimulator.h>
72 
73 #define GETOPTARGS "dh"
74 
75 #ifdef VISP_HAVE_DISPLAY
76 
85 void usage(const char *name, const char *badparam)
86 {
87  fprintf(stdout, "\n\
88 Demonstration of the wireframe simulator with a simple visual servoing.\n\
89  \n\
90 The visual servoing consists in bringing the camera at a desired position from the object.\n\
91  \n\
92 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\
93  \n\
94 SYNOPSIS\n\
95  %s [-d] [-h]\n", name);
96 
97  fprintf(stdout, "\n\
98 OPTIONS: Default\n\
99  -d \n\
100  Turn off the display.\n\
101  \n\
102  -h\n\
103  Print the help.\n");
104 
105  if (badparam)
106  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
107 }
108 
120 bool getOptions(int argc, const char **argv, bool &display)
121 {
122  const char *optarg;
123  int c;
124  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
125 
126  switch (c) {
127  case 'd': display = false; break;
128  case 'h': usage(argv[0], NULL); return false; break;
129 
130  default:
131  usage(argv[0], optarg);
132  return false; break;
133  }
134  }
135 
136  if ((c == 1) || (c == -1)) {
137  // standalone param or error
138  usage(argv[0], NULL);
139  std::cerr << "ERROR: " << std::endl;
140  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
141  return false;
142  }
143 
144  return true;
145 }
146 
147 
148 /*
149  Computes the virtual visual features corresponding to the sphere and stores it in the generic feature.
150 
151  The visual feature vector is computed thanks to the following formula : s = {sx, sy, sz}
152  sx = gx*h2/(sqrt(h2+1)
153  sx = gy*h2/(sqrt(h2+1)
154  sz = sqrt(h2+1)
155 
156  with gx and gy the center of gravity of the ellipse,
157  with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
158  with n20,n02,n11 the second order moments of the sphere
159 */
160 void computeVisualFeatures(const vpSphere sphere, vpGenericFeature &s)
161 {
162  double gx = sphere.get_x();
163  double gy = sphere.get_y();
164  double m02 = sphere.get_mu02();
165  double m20 = sphere.get_mu20();
166  double m11 = sphere.get_mu11();
167  double h2;
168  //if (gx != 0 || gy != 0)
169  if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
170  h2 = (vpMath::sqr(gx)+vpMath::sqr(gy))/(4*m20*vpMath::sqr(gy)+4*m02*vpMath::sqr(gx)-8*m11*gx*gy);
171  else
172  h2 = 1/(4*m20);
173 
174  double sx = gx*h2/(sqrt(h2+1));
175  double sy = gy*h2/(sqrt(h2+1));
176  double sz = sqrt(h2+1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2));
177 
178  s.set_s(sx,sy,sz);
179 }
180 
181 /*
182  Computes the interaction matrix such as L = [-1/R*I3 [s]x]
183 
184  with R the radius of the sphere
185  with I3 the 3x3 identity matrix
186  with [s]x the skew matrix related to s
187 */
188 void computeInteractionMatrix(const vpGenericFeature s,const vpSphere sphere, vpMatrix &L)
189 {
190  L = 0;
191  L[0][0] = -1/sphere.getR();
192  L[1][1] = -1/sphere.getR();
193  L[2][2] = -1/sphere.getR();
194 
195  double s0,s1,s2;
196  s.get_s(s0,s1,s2);
197 
198  vpTranslationVector c(s0,s1,s2);
199  vpMatrix sk;
200  sk = c.skew();
201 
202  for(unsigned int i = 0; i < 3; i++)
203  for(unsigned int j = 0; j < 3; j++)
204  L[i][j+3] = sk[i][j];
205 }
206 
207 
208 int
209 main(int argc, const char ** argv)
210 {
211  bool opt_display = true;
212 
213  // Read the command line options
214  if (getOptions(argc, argv, opt_display) == false) {
215  exit (-1);
216  }
217 
218  vpImage<vpRGBa> Iint(480,640,255);
219  vpImage<vpRGBa> Iext1(480,640,255);
220  vpImage<vpRGBa> Iext2(480,640,255);
221 
222 #if defined VISP_HAVE_X11
223  vpDisplayX display[3];
224 #elif defined VISP_HAVE_OPENCV
225  vpDisplayOpenCV display[3];
226 #elif defined VISP_HAVE_GDI
227  vpDisplayGDI display[3];
228 #elif defined VISP_HAVE_D3D9
229  vpDisplayD3D display[3];
230 #elif defined VISP_HAVE_GTK
231  vpDisplayGTK display[3];
232 #endif
233 
234  if (opt_display)
235  {
236  try
237  {
238  // Display size is automatically defined by the image (I) size
239  display[0].init(Iint, 100, 100,"The internal view") ;
240  display[1].init(Iext1, 100, 100,"The first external view") ;
241  display[2].init(Iext2, 100, 100,"The second external view") ;
242  vpDisplay::setWindowPosition (Iint, 0, 0);
243  vpDisplay::setWindowPosition (Iext1, 700, 0);
244  vpDisplay::setWindowPosition (Iext2, 0, 550);
245  vpDisplay::display(Iint);
246  vpDisplay::flush(Iint);
247  vpDisplay::display(Iext1);
248  vpDisplay::flush(Iext1);
249  vpDisplay::display(Iext2);
250  vpDisplay::flush(Iext2);
251  }
252  catch(...)
253  {
254  vpERROR_TRACE("Error while displaying the image") ;
255  exit(-1);
256  }
257  }
258 
259  vpServo task;
260  vpRobotCamera robot ;
261  float sampling_time = 0.040f; // Sampling period in second
262  robot.setSamplingTime(sampling_time);
263 
264  // Since the task gain lambda is very high, we need to increase default max velocities
265  robot.setMaxTranslationVelocity(10);
267 
268  // Set initial position of the object in the camera frame
269  vpHomogeneousMatrix cMo(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
270  // Set desired position of the object in the camera frame
271  vpHomogeneousMatrix cdMo(0.0,0.0,0.8,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
272  // Set initial position of the object in the world frame
273  vpHomogeneousMatrix wMo(0.0,0.0,0,0,0,0);
274  // Position of the camera in the world frame
275  vpHomogeneousMatrix wMc, cMw;
276  wMc = wMo * cMo.inverse();
277  cMw = wMc.inverse();
278 
279  robot.setPosition( cMw );
280 
281  //The sphere
282  vpSphere sphere(0,0,0,0.15);
283 
284  // Projection of the sphere
285  sphere.track(cMo);
286 
287  //Set the current visual feature
288  vpGenericFeature s(3);
289  computeVisualFeatures(sphere, s);
290 
291  // Projection of the points
292  sphere.track(cdMo);
293 
294  vpGenericFeature sd(3);
295  computeVisualFeatures(sphere, sd);
296 
297  vpMatrix L(3,6);
298  computeInteractionMatrix(sd,sphere,L);
299  sd.setInteractionMatrix(L);
300 
303 
305  vpVelocityTwistMatrix cVe(cMe);
306  task.set_cVe(cVe);
307 
308  vpMatrix eJe;
309  robot.get_eJe(eJe);
310  task.set_eJe(eJe);
311 
312  task.addFeature(s,sd) ;
313 
314  task.setLambda(7);
315 
317 
318  // Set the scene
320 
321  // Initialize simulator frames
322  sim.set_fMo( wMo ); // Position of the object in the world reference frame
323  sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
324  sim.setDesiredCameraPosition(cdMo); // desired position of the camera
325 
326  // Set the External camera position
327  vpHomogeneousMatrix camMf(0.0,0,3.5,vpMath::rad(0),vpMath::rad(30),0);
328  sim.setExternalCameraPosition(camMf);
329 
330  // Computes the position of a camera which is fixed in the object frame
331  vpHomogeneousMatrix camoMf(0,0.0,2.5,0,vpMath::rad(140),0);
332  camoMf = camoMf*(sim.get_fMo().inverse());
333 
334  // Set the parameters of the cameras (internal and external)
335  vpCameraParameters camera(1000,1000,320,240);
336  sim.setInternalCameraParameters(camera);
337  sim.setExternalCameraParameters(camera);
338 
339  int stop = 10;
340 
341  if (opt_display)
342  {
343  stop = 1000;
344  //Get the internal and external views
345  sim.getInternalImage(Iint);
346  sim.getExternalImage(Iext1);
347  sim.getExternalImage(Iext2,camoMf);
348 
349  //Display the object frame (current and desired position)
350  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
351  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
352 
353  //Display the object frame the world reference frame and the camera frame
354  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
355  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
356  vpDisplay::displayFrame(Iext1,camMf,camera,0.2,vpColor::none);
357 
358  //Display the world reference frame and the object frame
359  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
360  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
361 
362  vpDisplay::flush(Iint);
363  vpDisplay::flush(Iext1);
364  vpDisplay::flush(Iext2);
365 
366  std::cout << "Click on a display" << std::endl;
367  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext1,false) && !vpDisplay::getClick(Iext2,false)){};
368  }
369 
370  //Print the task
371  task.print() ;
372 
373  int iter = 0;
374  vpColVector v ;
375 
376  while(iter++ < stop)
377  {
378  if (opt_display)
379  {
380  vpDisplay::display(Iint) ;
381  vpDisplay::display(Iext1) ;
382  vpDisplay::display(Iext2) ;
383  }
384 
385  double t = vpTime::measureTimeMs();
386 
387  robot.get_eJe(eJe) ;
388  task.set_eJe(eJe) ;
389 
390  robot.getPosition(cMw) ;
391  cMo = cMw * wMo;
392 
393  sphere.track(cMo);
394 
395  //Set the current visual feature
396  computeVisualFeatures(sphere, s);
397 
398  v = task.computeControlLaw() ;
400  sim.setCameraPositionRelObj(cMo);
401 
402  //Compute the position of the external view which is fixed in the object frame
403  camoMf.buildFrom(0,0.0,2.5,0,vpMath::rad(150),0);
404  camoMf = camoMf*(sim.get_fMo().inverse());
405 
406  if (opt_display)
407  {
408  //Get the internal and external views
409  sim.getInternalImage(Iint);
410  sim.getExternalImage(Iext1);
411  sim.getExternalImage(Iext2,camoMf);
412 
413  //Display the object frame (current and desired position)
414  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
415  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
416 
417  //Display the camera frame, the object frame the world reference frame
421 
422  //Display the world reference frame and the object frame
423  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
424  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
425 
426  vpDisplay::flush(Iint);
427  vpDisplay::flush(Iext1);
428  vpDisplay::flush(Iext2);
429  }
430 
431  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
432 
433  std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
434  }
435 
436  task.print() ;
437  task.kill() ;
438 
439  return 0;
440 }
441 #else
442 int
443 main()
444 {
445  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
446 }
447 
448 #endif