ViSP
photometricVisualServoing.cpp
1 /****************************************************************************
2  *
3  * $Id: photometricVisualServoing.cpp 5108 2015-01-05 07:48:58Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  *
35  * Authors:
36  * Eric Marchand
37  * Christophe Collewet
38  *
39  *****************************************************************************/
40 
48 #include <visp/vpDebug.h>
49 
50 #include <visp/vpImage.h>
51 #include <visp/vpImageIo.h>
52 #include <visp/vpImageTools.h>
53 
54 #include <visp/vpCameraParameters.h>
55 #include <visp/vpTime.h>
56 #include <visp/vpRobotCamera.h>
57 
58 #include <visp/vpMath.h>
59 #include <visp/vpHomogeneousMatrix.h>
60 #include <visp/vpDisplayGTK.h>
61 #include <visp/vpDisplayGDI.h>
62 #include <visp/vpDisplayOpenCV.h>
63 #include <visp/vpDisplayD3D.h>
64 #include <visp/vpDisplayX.h>
65 
66 #include <visp/vpFeatureLuminance.h>
67 #include <visp/vpParseArgv.h>
68 
69 #include <visp/vpImageSimulator.h>
70 #include <stdlib.h>
71 #define Z 1
72 
73 #include <visp/vpParseArgv.h>
74 #include <visp/vpIoTools.h>
75 
76 // List of allowed command line options
77 #define GETOPTARGS "cdi:n:h"
78 
79 void usage(const char *name, const char *badparam, std::string ipath, int niter);
80 bool getOptions(int argc, const char **argv, std::string &ipath,
81  bool &click_allowed, bool &display, int &niter);
82 
93 void usage(const char *name, const char *badparam, std::string ipath, int niter)
94 {
95  fprintf(stdout, "\n\
96 Tracking of Surf key-points.\n\
97 \n\
98 SYNOPSIS\n\
99  %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n", name);
100 
101  fprintf(stdout, "\n\
102 OPTIONS: Default\n\
103  -i <input image path> %s\n\
104  Set image input path.\n\
105  From this path read \"ViSP-images/doisneau/doisneau.jpg\"\n\
106  images. \n\
107  Setting the VISP_INPUT_IMAGE_PATH environment\n\
108  variable produces the same behaviour than using\n\
109  this option.\n\
110 \n\
111  -c\n\
112  Disable the mouse click. Useful to automaze the \n\
113  execution of this program without humain intervention.\n\
114 \n\
115  -d \n\
116  Turn off the display.\n\
117 \n\
118  -n %%d %d\n\
119  Number of iterations.\n\
120 \n\
121  -h\n\
122  Print the help.\n",
123  ipath.c_str(), niter);
124 
125  if (badparam)
126  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
127 }
142 bool getOptions(int argc, const char **argv, std::string &ipath,
143  bool &click_allowed, bool &display, int &niter)
144 {
145  const char *optarg_;
146  int c;
147  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
148 
149  switch (c) {
150  case 'c': click_allowed = false; break;
151  case 'd': display = false; break;
152  case 'i': ipath = optarg_; break;
153  case 'n': niter = atoi(optarg_); break;
154  case 'h': usage(argv[0], NULL, ipath, niter); return false; break;
155 
156  default:
157  usage(argv[0], optarg_, ipath, niter);
158  return false; break;
159  }
160  }
161 
162  if ((c == 1) || (c == -1)) {
163  // standalone param or error
164  usage(argv[0], NULL, ipath, niter);
165  std::cerr << "ERROR: " << std::endl;
166  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
167  return false;
168  }
169 
170  return true;
171 }
172 
173 
174 
175 int
176 main(int argc, const char ** argv)
177 {
178  try {
179  std::string env_ipath;
180  std::string opt_ipath;
181  std::string ipath;
182  std::string filename;
183  bool opt_click_allowed = true;
184  bool opt_display = true;
185  int opt_niter = 400;
186 
187  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
188  env_ipath = vpIoTools::getViSPImagesDataPath();
189 
190  // Set the default input path
191  if (! env_ipath.empty())
192  ipath = env_ipath;
193 
194  // Read the command line options
195  if (getOptions(argc, argv, opt_ipath, opt_click_allowed,
196  opt_display, opt_niter) == false) {
197  return (-1);
198  }
199 
200  // Get the option values
201  if (!opt_ipath.empty())
202  ipath = opt_ipath;
203 
204  // Compare ipath and env_ipath. If they differ, we take into account
205  // the input path comming from the command line option
206  if (!opt_ipath.empty() && !env_ipath.empty()) {
207  if (ipath != env_ipath) {
208  std::cout << std::endl
209  << "WARNING: " << std::endl;
210  std::cout << " Since -i <visp image path=" << ipath << "> "
211  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
212  << " we skip the environment variable." << std::endl;
213  }
214  }
215 
216  // Test if an input path is set
217  if (opt_ipath.empty() && env_ipath.empty()){
218  usage(argv[0], NULL, ipath, opt_niter);
219  std::cerr << std::endl
220  << "ERROR:" << std::endl;
221  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
222  << std::endl
223  << " environment variable to specify the location of the " << std::endl
224  << " image path where test images are located." << std::endl << std::endl;
225  exit(-1);
226  }
227 
228  vpImage<unsigned char> Itexture ;
229  filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
230  vpImageIo::read(Itexture,filename) ;
231 
232  vpColVector X[4];
233  for (int i = 0; i < 4; i++) X[i].resize(3);
234  // Top left corner
235  X[0][0] = -0.3;
236  X[0][1] = -0.215;
237  X[0][2] = 0;
238 
239  // Top right corner
240  X[1][0] = 0.3;
241  X[1][1] = -0.215;
242  X[1][2] = 0;
243 
244  // Bottom right corner
245  X[2][0] = 0.3;
246  X[2][1] = 0.215;
247  X[2][2] = 0;
248 
249  //Bottom left corner
250  X[3][0] = -0.3;
251  X[3][1] = 0.215;
252  X[3][2] = 0;
253 
254  vpImageSimulator sim;
255 
257  sim.init(Itexture, X);
258 
259  vpCameraParameters cam(870, 870, 160, 120);
260 
261  // ----------------------------------------------------------
262  // Create the framegraber (here a simulated image)
263  vpImage<unsigned char> I(240,320,0) ;
265 
266  //camera desired position
267  vpHomogeneousMatrix cdMo ;
268  cdMo[2][3] = 1 ;
269 
270  //set the robot at the desired position
271  sim.setCameraPosition(cdMo) ;
272  sim.getImage(I,cam); // and aquire the image Id
273  Id = I ;
274 
275  // display the image
276 #if defined VISP_HAVE_X11
277  vpDisplayX d;
278 #elif defined VISP_HAVE_GDI
279  vpDisplayGDI d;
280 #elif defined VISP_HAVE_GTK
281  vpDisplayGTK d;
282 #elif defined VISP_HAVE_OPENCV
283  vpDisplayOpenCV d;
284 #endif
285 
286 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
287  if (opt_display) {
288  d.init(I, 20, 10, "Photometric visual servoing : s") ;
290  vpDisplay::flush(I);
291  }
292  if (opt_display && opt_click_allowed) {
293  std::cout << "Click in the image to continue..." << std::endl;
295  }
296 #endif
297 
298  // ----------------------------------------------------------
299  // position the robot at the initial position
300  // ----------------------------------------------------------
301 
302  //camera desired position
303  vpHomogeneousMatrix cMo ;
304  cMo.buildFrom(0,0,1.2,vpMath::rad(15),vpMath::rad(-5),vpMath::rad(20));
305 
306  //set the robot at the desired position
307  sim.setCameraPosition(cMo) ;
308  I =0 ;
309  sim.getImage(I,cam); // and aquire the image Id
310 
311 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
312  if (opt_display) {
313  vpDisplay::display(I) ;
314  vpDisplay::flush(I) ;
315  }
316  if (opt_display && opt_click_allowed) {
317  std::cout << "Click in the image to continue..." << std::endl;
319  }
320 #endif
321 
322  vpImage<unsigned char> Idiff ;
323  Idiff = I ;
324 
325  vpImageTools::imageDifference(I,Id,Idiff) ;
326 
327  // Affiche de l'image de difference
328 #if defined VISP_HAVE_X11
329  vpDisplayX d1;
330 #elif defined VISP_HAVE_GDI
331  vpDisplayGDI d1;
332 #elif defined VISP_HAVE_GTK
333  vpDisplayGTK d1;
334 #endif
335 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
336  if (opt_display) {
337  d1.init(Idiff, 40+(int)I.getWidth(), 10, "photometric visual servoing : s-s* ") ;
338  vpDisplay::display(Idiff) ;
339  vpDisplay::flush(Idiff) ;
340  }
341 #endif
342  // create the robot (here a simulated free flying camera)
343  vpRobotCamera robot ;
344  robot.setSamplingTime(0.04);
345  robot.setPosition(cMo) ;
346 
347  // ------------------------------------------------------
348  // Visual feature, interaction matrix, error
349  // s, Ls, Lsd, Lt, Lp, etc
350  // ------------------------------------------------------
351 
352  // current visual feature built from the image
353  // (actually, this is the image...)
354  vpFeatureLuminance sI ;
355  sI.init( I.getHeight(), I.getWidth(), Z) ;
356  sI.setCameraParameters(cam) ;
357  sI.buildFrom(I) ;
358 
359  // desired visual feature built from the image
360  vpFeatureLuminance sId ;
361  sId.init(I.getHeight(), I.getWidth(), Z) ;
362  sId.setCameraParameters(cam) ;
363  sId.buildFrom(Id) ;
364 
365  // Matrice d'interaction, Hessien, erreur,...
366  vpMatrix Lsd; // matrice d'interaction a la position desiree
367  vpMatrix Hsd; // hessien a la position desiree
368  vpMatrix H ; // Hessien utilise pour le levenberg-Marquartd
369  vpColVector error ; // Erreur I-I*
370 
371  // Compute the interaction matrix
372  // link the variation of image intensity to camera motion
373 
374  // here it is computed at the desired position
375  sId.interaction(Lsd) ;
376 
377  // Compute the Hessian H = L^TL
378  Hsd = Lsd.AtA() ;
379 
380  // Compute the Hessian diagonal for the Levenberg-Marquartd
381  // optimization process
382  unsigned int n = 6 ;
383  vpMatrix diagHsd(n,n) ;
384  diagHsd.eye(n);
385  for(unsigned int i = 0 ; i < n ; i++) diagHsd[i][i] = Hsd[i][i];
386 
387  // ------------------------------------------------------
388  // Control law
389  double lambda ; //gain
390  vpColVector e ;
391  vpColVector v ; // camera velocity send to the robot
392 
393  // ----------------------------------------------------------
394  // Minimisation
395 
396  double mu ; // mu = 0 : Gauss Newton ; mu != 0 : LM
397  double lambdaGN;
398 
399  mu = 0.01;
400  lambda = 30 ;
401  lambdaGN = 30;
402 
403  // set a velocity control mode
405 
406  // ----------------------------------------------------------
407  int iter = 1;
408  int iterGN = 90 ; // swicth to Gauss Newton after iterGN iterations
409 
410  double normeError = 0;
411  do {
412  std::cout << "--------------------------------------------" << iter++ << std::endl ;
413 
414  // Acquire the new image
415  sim.setCameraPosition(cMo) ;
416  sim.getImage(I,cam) ;
417 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
418  if (opt_display) {
419  vpDisplay::display(I) ;
420  vpDisplay::flush(I) ;
421  }
422 #endif
423  vpImageTools::imageDifference(I,Id,Idiff) ;
424 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
425  if (opt_display) {
426  vpDisplay::display(Idiff) ;
427  vpDisplay::flush(Idiff) ;
428  }
429 #endif
430  // Compute current visual feature
431  sI.buildFrom(I) ;
432 
433  // compute current error
434  sI.error(sId,error) ;
435 
436  normeError = (error.sumSquare());
437  std::cout << "|e| "<<normeError <<std::endl ;
438 
439  // double t = vpTime::measureTimeMs() ;
440 
441  // ---------- Levenberg Marquardt method --------------
442  {
443  if (iter > iterGN)
444  {
445  mu = 0.0001 ;
446  lambda = lambdaGN;
447  }
448 
449  // Compute the levenberg Marquartd term
450  {
451  H = ((mu * diagHsd) + Hsd).inverseByLU();
452  }
453  // compute the control law
454  e = H * Lsd.t() *error ;
455 
456  v = - lambda*e;
457  }
458 
459  std::cout << "lambda = " << lambda << " mu = " << mu ;
460  std::cout << " |Tc| = " << sqrt(v.sumSquare()) << std::endl;
461 
462  // send the robot velocity
464  robot.getPosition(cMo) ;
465 
466  }
467  while(normeError > 10000 && iter < opt_niter);
468 
469  v = 0 ;
471 
472  return 0;
473  }
474  catch(vpException e) {
475  std::cout << "Catch an exception: " << e << std::endl;
476  return 1;
477  }
478 }
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void init(const vpImage< unsigned char > &I, vpColVector *X)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1071
unsigned int getWidth() const
Definition: vpImage.h:161
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void buildFrom(vpImage< unsigned char > &I)
void setPosition(const vpHomogeneousMatrix &cMw)
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void setCameraParameters(vpCameraParameters &_cam)
Define the X11 console to display images.
Definition: vpDisplayX.h:152
error that can be emited by ViSP classes.
Definition: vpException.h:76
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
double sumSquare() const
Definition: vpMatrix.cpp:868
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:80
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
virtual void setSamplingTime(const double &delta_t)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
Initialize the velocity controller.
Definition: vpRobot.h:70
void setInterpolationType(const vpInterpolationType interplt)
vpMatrix AtA() const
Definition: vpMatrix.cpp:1479
Class that defines the image luminance visual feature.
Class that defines the simplest robot: a free flying camera.
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1245
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
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 buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
Definition: vpMath.h:100
void getPosition(vpHomogeneousMatrix &cMw) const
vpMatrix t() const
Definition: vpMatrix.cpp:1296
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
unsigned int getHeight() const
Definition: vpImage.h:152
virtual bool getClick(bool blocking=true)=0
static void read(vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:278
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)