ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoSimu4Points.cpp
1 /****************************************************************************
2  *
3  * $Id: servoSimu4Points.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 
50 #include <visp/vpCameraParameters.h>
51 #include <visp/vpDisplayOpenCV.h>
52 #include <visp/vpDisplayX.h>
53 #include <visp/vpDisplayGTK.h>
54 #include <visp/vpDisplayGDI.h>
55 #include <visp/vpDisplayD3D.h>
56 #include <visp/vpFeatureBuilder.h>
57 #include <visp/vpFeaturePoint.h>
58 #include <visp/vpHomogeneousMatrix.h>
59 #include <visp/vpImage.h>
60 #include <visp/vpImageIo.h>
61 #include <visp/vpIoTools.h>
62 #include <visp/vpMath.h>
63 #include <visp/vpParseArgv.h>
64 #include <visp/vpServo.h>
65 #include <visp/vpSimulatorCamera.h>
66 #include <visp/vpTime.h>
67 #include <visp/vpVelocityTwistMatrix.h>
68 #include <visp/vpWireFrameSimulator.h>
69 
70 #define GETOPTARGS "dh"
71 
72 #ifdef VISP_HAVE_DISPLAY
73 
83 void usage(const char *name, std::string ipath, const char *badparam)
84 {
85  fprintf(stdout, "\n\
86 Demonstration of the wireframe simulator with a simple visual servoing.\n\
87  \n\
88 The visual servoing consists in bringing the camera at a desired \n\
89 position from the object.\n\
90  \n\
91 The visual features used to compute the pose of the camera and \n\
92 thus the control law are four points.\n\
93  \n\
94 This demonstration explains also how to move the object around a world\n\
95 reference frame. Here, the movement is a rotation around the x and y axis\n\
96 at a given distance from the world frame. In fact the object trajectory\n\
97 is on a sphere whose center is the origin of the world frame.\n\
98  \n\
99 SYNOPSIS\n\
100  %s [-d] [-h]\n", name);
101 
102  fprintf(stdout, "\n\
103 OPTIONS: Default\n\
104  -i <input image path> %s\n\
105  Set mire.pgm image input path.\n\
106  From this path read \"ViSP-images/mire/mire.pgm\" video.\n\
107  Setting the VISP_INPUT_IMAGE_PATH environment variable \n\
108  produces the same behaviour than using this option.\n\
109  \n\
110  -d \n\
111  Turn off the display.\n\
112  \n\
113  -h\n\
114  Print the help.\n", ipath.c_str());
115 
116  if (badparam)
117  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
118 }
119 
120 
133 bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
134 {
135  const char *optarg;
136  int c;
137  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
138 
139  switch (c) {
140  case 'i': ipath = optarg; break;
141  case 'd': display = false; break;
142  case 'h': usage(argv[0],ipath, NULL); return false; break;
143 
144  default:
145  usage(argv[0],ipath, optarg);
146  return false; break;
147  }
148  }
149 
150  if ((c == 1) || (c == -1)) {
151  // standalone param or error
152  usage(argv[0], ipath, NULL);
153  std::cerr << "ERROR: " << std::endl;
154  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
155  return false;
156  }
157 
158  return true;
159 }
160 
161 
162 int
163 main(int argc, const char ** argv)
164 {
165  bool opt_display = true;
166  std::string opt_ipath;
167  std::string env_ipath;
168  std::string ipath ;
169  std::string filename;
170 
171  // Read the command line options
172  if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
173  exit (-1);
174  }
175 
176  vpImage<vpRGBa> Iint(480,640,255);
177  vpImage<vpRGBa> Iext1(480,640,255);
178  vpImage<vpRGBa> Iext2(480,640,255);
179 
180 #if defined VISP_HAVE_X11
181  vpDisplayX display[3];
182 #elif defined VISP_HAVE_OPENCV
183  vpDisplayOpenCV display[3];
184 #elif defined VISP_HAVE_GDI
185  vpDisplayGDI display[3];
186 #elif defined VISP_HAVE_D3D9
187  vpDisplayD3D display[3];
188 #elif defined VISP_HAVE_GTK
189  vpDisplayGTK display[3];
190 #endif
191 
192  if (opt_display)
193  {
194  try
195  {
196  // Display size is automatically defined by the image (I) size
197  display[0].init(Iint, 100, 100,"The internal view") ;
198  display[1].init(Iext1, 100, 100,"The first external view") ;
199  display[2].init(Iext2, 100, 100,"The second external view") ;
200  vpDisplay::setWindowPosition (Iint, 0, 0);
201  vpDisplay::setWindowPosition (Iext1, 700, 0);
202  vpDisplay::setWindowPosition (Iext2, 0, 550);
203  vpDisplay::display(Iint);
204  vpDisplay::flush(Iint);
205  vpDisplay::display(Iext1);
206  vpDisplay::flush(Iext1);
207  vpDisplay::display(Iext2);
208  vpDisplay::flush(Iext2);
209  }
210  catch(...)
211  {
212  vpERROR_TRACE("Error while displaying the image") ;
213  exit(-1);
214  }
215  }
216 
217  vpServo task;
218  vpSimulatorCamera robot ;
219  float sampling_time = 0.040f; // Sampling period in second
220  robot.setSamplingTime(sampling_time);
221 
222  // Since the task gain lambda is very high, we need to increase default max velocities
223  robot.setMaxTranslationVelocity(10);
225 
226  // Set initial position of the object in the camera frame
227  vpHomogeneousMatrix cMo(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
228  // Set desired position of the object in the camera frame
229  vpHomogeneousMatrix cdMo(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
230  // Set initial position of the object in the world frame
231  vpHomogeneousMatrix wMo(0.0,0.0,0.2,0,0,0);
232  // Position of the camera in the world frame
233  vpHomogeneousMatrix wMc, cMw;
234  wMc = wMo * cMo.inverse();
235  cMw = wMc.inverse();
236 
237  //The four point used as visual features
238  vpPoint point[4] ;
239  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
240  point[3].setWorldCoordinates(-0.1,0.1,0) ;
241  point[2].setWorldCoordinates(0.1,0.1,0) ;
242  point[1].setWorldCoordinates(0.1,-0.1,0) ;
243 
244  // Projection of the points
245  for (int i = 0 ; i < 4 ; i++)
246  point[i].track(cMo);
247 
248  //Set the current visual feature
249  vpFeaturePoint p[4];
250  for (int i = 0 ; i < 4 ; i++)
251  vpFeatureBuilder::create(p[i], point[i]);
252 
253  // Projection of the points
254  for (int i = 0 ; i < 4 ; i++)
255  point[i].track(cdMo);
256 
257  vpFeaturePoint pd[4];
258  for (int i = 0 ; i < 4 ; i++)
259  vpFeatureBuilder::create(pd[i], point[i]);
260 
263 
264  vpHomogeneousMatrix cMe; // Identity
265  vpVelocityTwistMatrix cVe(cMe);
266  task.set_cVe(cVe);
267 
268  vpMatrix eJe;
269  robot.get_eJe(eJe);
270  task.set_eJe(eJe);
271 
272  for (int i = 0 ; i < 4 ; i++)
273  task.addFeature(p[i],pd[i]) ;
274 
275  task.setLambda(10);
276 
277  std::list<vpImageSimulator> list;
278  vpImageSimulator imsim;
279 
280  vpColVector X[4];
281  for (int i = 0; i < 4; i++) X[i].resize(3);
282  X[0][0] = -0.2;
283  X[0][1] = -0.2;
284  X[0][2] = 0;
285 
286  X[1][0] = 0.2;
287  X[1][1] = -0.2;
288  X[1][2] = 0;
289 
290  X[2][0] = 0.2;
291  X[2][1] = 0.2;
292  X[2][2] = 0;
293 
294  X[3][0] = -0.2;
295  X[3][1] = 0.2;
296  X[3][2] = 0;
297 
298  char *ptenv = getenv("VISP_INPUT_IMAGE_PATH");
299  if (ptenv != NULL)
300  env_ipath = ptenv;
301 
302  if (! env_ipath.empty())
303  ipath = env_ipath;
304 
305  if (!opt_ipath.empty())
306  ipath = opt_ipath;
307 
308  filename = ipath + vpIoTools::path("/ViSP-images/mire/mire.pgm");
309 
310  try
311  {
312  imsim.init(filename.c_str(), X);
313  }
314  catch(...)
315  {
316  vpTRACE("You need the ViSP data ");
317  task.kill();
318  return 1;
319  }
320 
321  list.push_back(imsim);
322 
324 
325  // Set the scene
327 
328  // Initialize simulator frames
329  sim.set_fMo( wMo ); // Position of the object in the world reference frame
330  sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
331  sim.setDesiredCameraPosition(cdMo); // desired position of the camera
332 
333  // Set the External camera position
335  sim.setExternalCameraPosition(camMf);
336 
337  //Computes the position of a camera which is fixed in the object frame
338  vpHomogeneousMatrix camoMf(0,0.0,1.5,0,vpMath::rad(140),0);
339  camoMf = camoMf*(sim.get_fMo().inverse());
340 
341  //Set the parameters of the cameras (internal and external)
342  vpCameraParameters camera(1000,1000,320,240);
343  sim.setInternalCameraParameters(camera);
344  sim.setExternalCameraParameters(camera);
345 
346  int stop = 10;
347 
348  if (opt_display)
349  {
350  stop = 2500;
351 
352  //Get the internal and external views
353  sim.getInternalImage(Iint);
354  sim.getExternalImage(Iext1);
355  sim.getExternalImage(Iext2, camoMf);
356 
357  //Display the object frame (current and desired position)
358  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
359  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
360 
361  //Display the object frame the world reference frame and the camera frame
362  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
363  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
364  vpDisplay::displayFrame(Iext1,camMf,camera,0.2,vpColor::none);
365 
366  //Display the world reference frame and the object frame
367  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
368  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
369 
370  vpDisplay::flush(Iint);
371  vpDisplay::flush(Iext1);
372  vpDisplay::flush(Iext2);
373 
374  std::cout << "Click on a display" << std::endl;
375  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext1,false) && !vpDisplay::getClick(Iext2,false)){};
376  }
377 
378  robot.setPosition( wMc );
379  //Print the task
380  task.print() ;
381 
382  int iter = 0;
383  vpColVector v ;
384 
385  while(iter++ < stop)
386  {
387  if (opt_display)
388  {
389  vpDisplay::display(Iint) ;
390  vpDisplay::display(Iext1) ;
391  vpDisplay::display(Iext2) ;
392  }
393 
394  double t = vpTime::measureTimeMs();
395 
396  robot.get_eJe(eJe) ;
397  task.set_eJe(eJe) ;
398 
399  robot.getPosition(wMc) ;
400  cMo = wMc.inverse() * wMo;
401  for (int i = 0 ; i < 4 ; i++)
402  {
403  point[i].track(cMo) ;
404  vpFeatureBuilder::create(p[i],point[i]) ;
405  }
406 
407  v = task.computeControlLaw() ;
409 
410  //Compute the movement of the object around the world reference frame.
411  vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
412  vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5*iter), 0, 0);
413  vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5*iter), 0);
414 
415  // Move the object in the world frame
416  wMo = b*c*a;
417 
418  sim.set_fMo( wMo ); //Move the object in the simulator
419  sim.setCameraPositionRelObj(cMo);
420 
421  //Compute the position of the external view which is fixed in the object frame
422  camoMf.buildFrom(0,0.0,1.5,0,vpMath::rad(150),0);
423  camoMf = camoMf*(sim.get_fMo().inverse());
424 
425  if (opt_display)
426  {
427  //Get the internal and external views
428  sim.getInternalImage(Iint);
429  sim.getExternalImage(Iext1);
430  sim.getExternalImage(Iext2,camoMf);
431 
432  //Display the object frame (current and desired position)
433  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
434  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
435 
436  //Display the camera frame, the object frame the world reference frame
440 
441  //Display the world reference frame and the object frame
442  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
443  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
444 
445  vpDisplay::flush(Iint);
446  vpDisplay::flush(Iext1);
447  vpDisplay::flush(Iext2);
448  }
449 
450  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
451 
452  std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
453  }
454 
455  task.print() ;
456  task.kill() ;
457 
458  return 0;
459 }
460 #else
461 int
462 main()
463 {
464  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
465 }
466 
467 #endif