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