Visual Servoing Platform  version 3.0.1
servoSimu4Points.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Demonstration of the wireframe simulator with a simple visual servoing
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
44 #include <stdlib.h>
45 
46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/gui/vpDisplayOpenCV.h>
48 #include <visp3/gui/vpDisplayX.h>
49 #include <visp3/gui/vpDisplayGTK.h>
50 #include <visp3/gui/vpDisplayGDI.h>
51 #include <visp3/gui/vpDisplayD3D.h>
52 #include <visp3/visual_features/vpFeatureBuilder.h>
53 #include <visp3/visual_features/vpFeaturePoint.h>
54 #include <visp3/core/vpHomogeneousMatrix.h>
55 #include <visp3/core/vpImage.h>
56 #include <visp3/io/vpImageIo.h>
57 #include <visp3/core/vpIoTools.h>
58 #include <visp3/core/vpMath.h>
59 #include <visp3/io/vpParseArgv.h>
60 #include <visp3/vs/vpServo.h>
61 #include <visp3/robot/vpSimulatorCamera.h>
62 #include <visp3/core/vpTime.h>
63 #include <visp3/core/vpVelocityTwistMatrix.h>
64 #include <visp3/robot/vpWireFrameSimulator.h>
65 
66 #define GETOPTARGS "dh"
67 
68 #ifdef VISP_HAVE_DISPLAY
69 
70 void usage(const char *name, std::string ipath, const char *badparam);
71 bool getOptions(int argc, const char **argv, std::string &ipath, bool &display);
72 
82 void usage(const char *name, std::string ipath, const char *badparam)
83 {
84  fprintf(stdout, "\n\
85 Demonstration of the wireframe simulator with a simple visual servoing.\n\
86  \n\
87 The visual servoing consists in bringing the camera at a desired \n\
88 position from the object.\n\
89  \n\
90 The visual features used to compute the pose of the camera and \n\
91 thus the control law are four points.\n\
92  \n\
93 This demonstration explains also how to move the object around a world\n\
94 reference frame. Here, the movement is a rotation around the x and y axis\n\
95 at a given distance from the world frame. In fact the object trajectory\n\
96 is on a sphere whose center is the origin of the world frame.\n\
97  \n\
98 SYNOPSIS\n\
99  %s [-d] [-h]\n", name);
100 
101  fprintf(stdout, "\n\
102 OPTIONS: Default\n\
103  -i <input image path> %s\n\
104  Set mire.pgm image input path.\n\
105  From this path read \"ViSP-images/mire/mire.pgm\" video.\n\
106  Setting the VISP_INPUT_IMAGE_PATH environment variable \n\
107  produces the same behaviour than using this option.\n\
108  \n\
109  -d \n\
110  Turn off the display.\n\
111  \n\
112  -h\n\
113  Print the help.\n", ipath.c_str());
114 
115  if (badparam)
116  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
117 }
118 
119 
132 bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
133 {
134  const char *optarg_;
135  int c;
136  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
137 
138  switch (c) {
139  case 'i': ipath = optarg_; break;
140  case 'd': display = false; break;
141  case 'h': usage(argv[0],ipath, NULL); return false; break;
142 
143  default:
144  usage(argv[0],ipath, optarg_);
145  return false; break;
146  }
147  }
148 
149  if ((c == 1) || (c == -1)) {
150  // standalone param or error
151  usage(argv[0], ipath, NULL);
152  std::cerr << "ERROR: " << std::endl;
153  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
154  return false;
155  }
156 
157  return true;
158 }
159 
160 
161 int
162 main(int argc, const char ** argv)
163 {
164  try {
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  // Display size is automatically defined by the image (I) size
195  display[0].init(Iint, 100, 100,"The internal view") ;
196  display[1].init(Iext1, 100, 100,"The first external view") ;
197  display[2].init(Iext2, 100, 100,"The second external view") ;
198  vpDisplay::setWindowPosition (Iint, 0, 0);
199  vpDisplay::setWindowPosition (Iext1, 700, 0);
200  vpDisplay::setWindowPosition (Iext2, 0, 550);
201  vpDisplay::display(Iint);
202  vpDisplay::flush(Iint);
203  vpDisplay::display(Iext1);
204  vpDisplay::flush(Iext1);
205  vpDisplay::display(Iext2);
206  vpDisplay::flush(Iext2);
207  }
208 
209  vpServo task;
210  vpSimulatorCamera robot ;
211  float sampling_time = 0.040f; // Sampling period in second
212  robot.setSamplingTime(sampling_time);
213 
214  // Since the task gain lambda is very high, we need to increase default max velocities
215  robot.setMaxTranslationVelocity(10);
217 
218  // Set initial position of the object in the camera frame
219  vpHomogeneousMatrix cMo(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
220  // Set desired position of the object in the camera frame
221  vpHomogeneousMatrix cdMo(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0));
222  // Set initial position of the object in the world frame
223  vpHomogeneousMatrix wMo(0.0,0.0,0.2,0,0,0);
224  // Position of the camera in the world frame
226  wMc = wMo * cMo.inverse();
227 
228  //The four point used as visual features
229  vpPoint point[4] ;
230  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
231  point[3].setWorldCoordinates(-0.1,0.1,0) ;
232  point[2].setWorldCoordinates(0.1,0.1,0) ;
233  point[1].setWorldCoordinates(0.1,-0.1,0) ;
234 
235  // Projection of the points
236  for (int i = 0 ; i < 4 ; i++)
237  point[i].track(cMo);
238 
239  //Set the current visual feature
240  vpFeaturePoint p[4];
241  for (int i = 0 ; i < 4 ; i++)
242  vpFeatureBuilder::create(p[i], point[i]);
243 
244  // Projection of the points
245  for (int i = 0 ; i < 4 ; i++)
246  point[i].track(cdMo);
247 
248  vpFeaturePoint pd[4];
249  for (int i = 0 ; i < 4 ; i++)
250  vpFeatureBuilder::create(pd[i], point[i]);
251 
254 
255  vpHomogeneousMatrix cMe; // Identity
256  vpVelocityTwistMatrix cVe(cMe);
257  task.set_cVe(cVe);
258 
259  vpMatrix eJe;
260  robot.get_eJe(eJe);
261  task.set_eJe(eJe);
262 
263  for (int i = 0 ; i < 4 ; i++)
264  task.addFeature(p[i],pd[i]) ;
265 
266  task.setLambda(10);
267 
268  std::list<vpImageSimulator> list;
269  vpImageSimulator imsim;
270 
271  vpColVector X[4];
272  for (int i = 0; i < 4; i++) X[i].resize(3);
273  X[0][0] = -0.2;
274  X[0][1] = -0.2;
275  X[0][2] = 0;
276 
277  X[1][0] = 0.2;
278  X[1][1] = -0.2;
279  X[1][2] = 0;
280 
281  X[2][0] = 0.2;
282  X[2][1] = 0.2;
283  X[2][2] = 0;
284 
285  X[3][0] = -0.2;
286  X[3][1] = 0.2;
287  X[3][2] = 0;
288 
289  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
290  env_ipath = vpIoTools::getViSPImagesDataPath();
291 
292  if (! env_ipath.empty())
293  ipath = env_ipath;
294 
295  if (!opt_ipath.empty())
296  ipath = opt_ipath;
297 
298  filename = vpIoTools::createFilePath(ipath, "ViSP-images/mire/mire.pgm");
299 
300  imsim.init(filename.c_str(), X);
301 
302  list.push_back(imsim);
303 
305 
306  // Set the scene
308 
309  // Initialize simulator frames
310  sim.set_fMo( wMo ); // Position of the object in the world reference frame
311  sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
312  sim.setDesiredCameraPosition(cdMo); // desired position of the camera
313 
314  // Set the External camera position
316  sim.setExternalCameraPosition(camMf);
317 
318  //Computes the position of a camera which is fixed in the object frame
319  vpHomogeneousMatrix camoMf(0,0.0,1.5,0,vpMath::rad(140),0);
320  camoMf = camoMf*(sim.get_fMo().inverse());
321 
322  //Set the parameters of the cameras (internal and external)
323  vpCameraParameters camera(1000,1000,320,240);
324  sim.setInternalCameraParameters(camera);
325  sim.setExternalCameraParameters(camera);
326 
327  int stop = 10;
328 
329  if (opt_display)
330  {
331  stop = 2500;
332 
333  //Get the internal and external views
334  sim.getInternalImage(Iint);
335  sim.getExternalImage(Iext1);
336  sim.getExternalImage(Iext2, camoMf);
337 
338  //Display the object frame (current and desired position)
339  vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
340  vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
341 
342  //Display the object frame the world reference frame and the camera frame
343  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo()*cMo.inverse(),camera,0.2,vpColor::none);
344  vpDisplay::displayFrame(Iext1,camMf*sim.get_fMo(),camera,0.2,vpColor::none);
345  vpDisplay::displayFrame(Iext1,camMf,camera,0.2,vpColor::none);
346 
347  //Display the world reference frame and the object frame
348  vpDisplay::displayFrame(Iext2,camoMf,camera,0.2,vpColor::none);
349  vpDisplay::displayFrame(Iext2,camoMf*sim.get_fMo(),camera,0.05,vpColor::none);
350 
351  vpDisplay::flush(Iint);
352  vpDisplay::flush(Iext1);
353  vpDisplay::flush(Iext2);
354 
355  std::cout << "Click on a display" << std::endl;
356  while (!vpDisplay::getClick(Iint,false) && !vpDisplay::getClick(Iext1,false) && !vpDisplay::getClick(Iext2,false)){};
357  }
358 
359  robot.setPosition( wMc );
360  //Print the task
361  task.print() ;
362 
363  int iter = 0;
364  vpColVector v ;
365 
366  while(iter++ < stop)
367  {
368  if (opt_display)
369  {
370  vpDisplay::display(Iint) ;
371  vpDisplay::display(Iext1) ;
372  vpDisplay::display(Iext2) ;
373  }
374 
375  double t = vpTime::measureTimeMs();
376 
377  robot.get_eJe(eJe) ;
378  task.set_eJe(eJe) ;
379 
380  wMc = robot.getPosition() ;
381  cMo = wMc.inverse() * wMo;
382  for (int i = 0 ; i < 4 ; i++)
383  {
384  point[i].track(cMo) ;
385  vpFeatureBuilder::create(p[i],point[i]) ;
386  }
387 
388  v = task.computeControlLaw() ;
390 
391  //Compute the movement of the object around the world reference frame.
392  vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
393  vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5*iter), 0, 0);
394  vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5*iter), 0);
395 
396  // Move the object in the world frame
397  wMo = b*c*a;
398 
399  sim.set_fMo( wMo ); //Move the object in the simulator
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,1.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  catch(vpException &e) {
442  std::cout << "Catch an exception: " << e << std::endl;
443  return 1;
444  }
445 }
446 #else
447 int
448 main()
449 {
450  vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
451 }
452 
453 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
void init(const vpImage< unsigned char > &I, vpColVector *X)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1157
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:238
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
vpHomogeneousMatrix get_fMo() const
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.
void set_fMo(const vpHomogeneousMatrix &fMo_)
#define vpERROR_TRACE
Definition: vpDebug.h:391
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:460
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:512
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
vpHomogeneousMatrix getPosition() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
Class that defines what is a point.
Definition: vpPoint.h:59
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:107
void kill()
Definition: vpServo.cpp:191
vpColVector computeControlLaw()
Definition: vpServo.cpp:954
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1366
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:391
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 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
Implementation of a velocity twist matrix and operations on such kind of matrices.
void getExternalImage(vpImage< unsigned char > &I)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:585
static double rad(double deg)
Definition: vpMath.h:104
void setExternalCameraParameters(const vpCameraParameters &cam)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:262
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0, 0))
void getInternalImage(vpImage< unsigned char > &I)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:435
void setInternalCameraParameters(const vpCameraParameters &cam)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:314
vpColVector getError() const
Definition: vpServo.h:271
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpHomogeneousMatrix getExternalCameraPosition() const