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
void setPosition(const vpHomogeneousMatrix &wMc)
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:191
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
Class that defines the simplest robot: a free flying camera.
#define vpTRACE
Definition: vpDebug.h:401
A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:133
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
static const vpColor none
Definition: vpColor.h:179
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
static std::string path(const char *pathname)
Definition: vpIoTools.cpp:715
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static double measureTimeMs()
Definition: vpTime.cpp:86
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo)
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:230
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1991
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf)
Class that defines what is a point.
Definition: vpPoint.h:65
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
vpColVector getError() const
Definition: vpServo.h:301
vpHomogeneousMatrix get_fMo() const
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
virtual void setWindowPosition(int winx, int winy)=0
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:238
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
void init(const vpImage< unsigned char > &I, vpColVector *_X)
void getPosition(vpHomogeneousMatrix &wMc) const
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void getExternalImage(vpImage< unsigned char > &I)
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:368
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
void setExternalCameraParameters(const vpCameraParameters &cam)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:215
void getInternalImage(vpImage< unsigned char > &I)
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 setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo)
vpHomogeneousMatrix inverse() const
void setInternalCameraParameters(const vpCameraParameters &cam)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
virtual bool getClick(bool blocking=true)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
void get_eJe(vpMatrix &eJe)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
void set_fMo(const vpHomogeneousMatrix &fMo)
vpHomogeneousMatrix getExternalCameraPosition() const
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