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