ViSP  3.0.0
simulateFourPoints2DPolarCamVelocity.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Simulation of a visual servoing with visualization.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
49 #include <visp3/core/vpConfig.h>
50 #include <visp3/core/vpDebug.h>
51 
52 
53 #ifdef VISP_HAVE_COIN3D_AND_GUI
54 
55 #include <visp3/core/vpImage.h>
56 #include <visp3/core/vpCameraParameters.h>
57 #include <visp3/core/vpTime.h>
58 #include <visp3/ar/vpSimulator.h>
59 #include <visp3/core/vpMath.h>
60 #include <visp3/core/vpHomogeneousMatrix.h>
61 #include <visp3/visual_features/vpFeaturePointPolar.h>
62 #include <visp3/vs/vpServo.h>
63 #include <visp3/robot/vpSimulatorCamera.h>
64 #include <visp3/visual_features/vpFeatureBuilder.h>
65 #include <visp3/io/vpParseArgv.h>
66 #include <visp3/core/vpIoTools.h>
67 
68 #define GETOPTARGS "di:h"
69 #define SAVE 0
70 
80 void usage(const char *name, const char *badparam, std::string ipath)
81 {
82  fprintf(stdout, "\n\
83 Simulation Servo 4points.\n\
84  \n\
85 SYNOPSIS\n\
86  %s [-i <input image path>] [-d] [-h]\n", name);
87 
88 fprintf(stdout, "\n\
89 OPTIONS: Default\n\
90  -i <input image path> %s\n\
91  Set image input path.\n\
92  From this path read \"ViSP-images/iv/4points.iv\"\n\
93  cad model.\n\
94  Setting the VISP_INPUT_IMAGE_PATH environment\n\
95  variable produces the same behaviour than using\n\
96  this option.\n\
97  \n\
98  -d \n\
99  Disable the image display. This can be useful \n\
100  for automatic tests using crontab under Unix or \n\
101  using the task manager under Windows.\n\
102  \n\
103  -h\n\
104  Print the help.\n\n", ipath.c_str());
105 
106  if (badparam)
107  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
108 }
109 
125 bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
126 {
127  const char *optarg;
128  int c;
129  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
130 
131  switch (c) {
132  case 'i': ipath = optarg; break;
133  case 'd': display = false; break;
134  case 'h': usage(argv[0], NULL, ipath); return false; break;
135 
136  default:
137  usage(argv[0], optarg, ipath); return false; break;
138  }
139  }
140 
141  if ((c == 1) || (c == -1)) {
142  // standalone param or error
143  usage(argv[0], NULL, ipath);
144  std::cerr << "ERROR: " << std::endl;
145  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
146  return false;
147  }
148 
149  return true;
150 }
151 
152 static
153 void *mainLoop (void *_simu)
154 {
155  vpSimulator *simu = (vpSimulator *)_simu ;
156  simu->initMainApplication() ;
157 
158  vpServo task ;
159  vpSimulatorCamera robot ;
160 
161  float sampling_time = 0.040f; // Sampling period in second
162  robot.setSamplingTime(sampling_time);
163  robot.setMaxTranslationVelocity(4.);
164 
165  // Sets the initial camera location
166  vpPoseVector vcMo ;
167 
168  vcMo[0] = 0. ;
169  vcMo[1] = 0. ;
170  vcMo[2] = 3 ;
171  vcMo[3] = 0 ;
172  vcMo[4] = vpMath::rad(0) ;
173  vcMo[5] = vpMath::rad(90) ;
174 
175  vpHomogeneousMatrix cMo(vcMo);
176  vpHomogeneousMatrix wMo; // Set to identity
177  vpHomogeneousMatrix wMc; // Camera location in world frame
178  wMc = wMo * cMo.inverse();
179  robot.setPosition(wMc) ;
180  simu->setCameraPosition(cMo) ;
181 
182  simu->getCameraPosition(cMo) ;
183  wMc = wMo * cMo.inverse();
184  robot.setPosition(wMc) ;
185 
186  vpCameraParameters cam ;
187 
188  // Sets the point coordinates in the world frame
189  vpPoint point[4] ;
190  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
191  point[1].setWorldCoordinates(0.1,-0.1,0) ;
192  point[2].setWorldCoordinates(0.1,0.1,0) ;
193  point[3].setWorldCoordinates(-0.1,0.1,0) ;
194 
195  // Project : computes the point coordinates in the camera frame and its 2D coordinates
196  for (int i = 0 ; i < 4 ; i++) {
197  point[i].changeFrame(cMo); // Compute point coordinates in the camera frame
198  point[i].project(); // Compute desired point doordinates in the camera frame
199  }
200 
201  // Sets the desired position of the point
202  vpFeaturePointPolar p[4] ;
203  for (int i = 0 ; i < 4 ; i++)
204  vpFeatureBuilder::create(p[i], point[i]) ; //retrieve x,y and Z of the vpPoint structure to build the polar coordinates
205 
206  std::cout << "s: \n";
207  for (int i=0; i < 4; i ++) {
208  printf("[%d] rho %f theta %f Z %f\n",
209  i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z());
210  }
211 
212  // Sets the desired position of the point
213  vcMo[0] = 0 ;
214  vcMo[1] = 0 ;
215  vcMo[2] = 1 ;
216  vcMo[3] = vpMath::rad(0);
217  vcMo[4] = vpMath::rad(0);
218  vcMo[5] = vpMath::rad(0);
219 
220  vpHomogeneousMatrix cMod(vcMo);
221 
222  vpFeaturePointPolar pd[4] ;
223  vpPoint pointd[4]; // Desired position of the points
224  pointd[0].setWorldCoordinates(-0.1,-0.1,0) ;
225  pointd[1].setWorldCoordinates(0.1,-0.1,0) ;
226  pointd[2].setWorldCoordinates(0.1,0.1,0) ;
227  pointd[3].setWorldCoordinates(-0.1,0.1,0) ;
228  for (int i=0; i < 4; i ++) {
229  pointd[i].changeFrame(cMod); // Compute desired point doordinates in the camera frame
230  pointd[i].project(); // Compute desired point doordinates in the camera frame
231 
232  vpFeatureBuilder::create(pd[i], pointd[i]) ; //retrieve x,y and Z of the vpPoint structure to build the polar coordinates
233  }
234  std::cout << "s*: \n";
235  for (int i=0; i < 4; i ++) {
236  printf("[%d] rho %f theta %f Z %f\n",
237  i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
238  }
239 
240  // Define the task
241  // We want an eye-in-hand control law
242  // Articular velocity are computed
245 
246  // Set the position of the camera in the end-effector frame
247  vpHomogeneousMatrix cMe ;
248  vpVelocityTwistMatrix cVe(cMe) ;
249  task.set_cVe(cVe) ;
250 
251  // Set the Jacobian (expressed in the end-effector frame)
252  vpMatrix eJe ;
253  robot.get_eJe(eJe) ;
254  task.set_eJe(eJe) ;
255 
256  // We want to see a point on a point
257  for (int i = 0 ; i < 4 ; i++)
258  task.addFeature(p[i],pd[i]) ;
259 
260  // Set the gain
261  task.setLambda(1.0) ;
262 
263  // Display task information
264  task.print() ;
265 
266  vpTime::wait(1000); // Sleep 1s
267 
268  unsigned int iter=0 ;
269  // Visual servo loop
270  while(iter++ < 200) {
271  double t = vpTime::measureTimeMs();
272 
273  robot.get_eJe(eJe) ;
274  task.set_eJe(eJe) ;
275 
276  wMc = robot.getPosition();
277  cMo = wMc.inverse() * wMo;
278  for (int i = 0 ; i < 4 ; i++)
279  {
280  point[i].track(cMo) ;
281  vpFeatureBuilder::create(p[i],point[i]) ;
282  }
283 
284  vpColVector v = task.computeControlLaw() ;
286 
287  simu->setCameraPosition(cMo) ;
288 
289  if(SAVE==1)
290  {
291  char name[FILENAME_MAX];
292  sprintf(name,"/tmp/image.%04d.external.png",iter) ;
293  std::cout << name << std::endl ;
294  simu->write(name) ;
295  sprintf(name,"/tmp/image.%04d.internal.png",iter) ;
296  simu->write(name) ;
297  }
298 
299  vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
300 
301  }
302  // Display task information
303  task.print() ;
304  task.kill() ;
305 
306  std::cout << "cMo:\n" << cMo << std::endl;
307  vpPoseVector pose(cMo);
308  std::cout << "final pose:\n" << pose.t() << std::endl;
309 
310  simu->closeMainApplication() ;
311 
312  void *a=NULL ;
313  return a ;
314 }
315 
316 int main(int argc, const char ** argv)
317 {
318  try {
319  std::string env_ipath;
320  std::string opt_ipath;
321  std::string ipath;
322  std::string filename;
323  std::string username;
324  bool opt_display = true;
325 
326  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
327  env_ipath = vpIoTools::getViSPImagesDataPath();
328 
329  // Set the default input path
330  if (! env_ipath.empty())
331  ipath = env_ipath;
332 
333  // Read the command line options
334  if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
335  exit (-1);
336  }
337 
338  // Get the option values
339  if (!opt_ipath.empty())
340  ipath = opt_ipath;
341 
342  // Compare ipath and env_ipath. If they differ, we take into account
343  // the input path comming from the command line option
344  if (!opt_ipath.empty() && !env_ipath.empty()) {
345  if (ipath != env_ipath) {
346  std::cout << std::endl
347  << "WARNING: " << std::endl;
348  std::cout << " Since -i <visp image path=" << ipath << "> "
349  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
350  << " we skip the environment variable." << std::endl;
351  }
352  }
353 
354  // Test if an input path is set
355  if (opt_ipath.empty() && env_ipath.empty()){
356  usage(argv[0], NULL, ipath);
357  std::cerr << std::endl
358  << "ERROR:" << std::endl;
359  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
360  << std::endl
361  << " environment variable to specify the location of the " << std::endl
362  << " image path where test images are located." << std::endl << std::endl;
363  exit(-1);
364  }
365 
366  vpCameraParameters cam ;
367  vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
368 
369 
370  if (opt_display) {
371  vpSimulator simu ;
372  simu.initInternalViewer(300, 300) ;
373  simu.initExternalViewer(300, 300) ;
374 
375  vpTime::wait(1000) ;
376  simu.setZoomFactor(1.0f) ;
377 
378  // Load the cad model
379  filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
380  simu.load(filename.c_str()) ;
381 
382  simu.setInternalCameraParameters(cam) ;
383  simu.setExternalCameraParameters(cam) ;
384  simu.initApplication(&mainLoop) ;
385 
386  simu.mainLoop() ;
387  }
388  return 0;
389  }
390  catch(vpException e) {
391  std::cout << "Catch an exception: " << e << std::endl;
392  return 1;
393  }
394 }
395 
396 #else
397 int
398 main()
399 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
400 
401 }
402 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1091
void write(const char *fileName)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:238
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:98
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
error that can be emited by ViSP classes.
Definition: vpException.h:73
void track(const vpHomogeneousMatrix &cMo)
virtual void mainLoop()
activate the mainloop
Class that defines 2D image point visual feature with polar coordinates described in ...
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)
void kill()
Definition: vpServo.cpp:186
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
Definition: vpSimulator.h:252
#define vpTRACE
Definition: vpDebug.h:414
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1265
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
void load(const char *file_name)
load an iv file
vpHomogeneousMatrix getPosition() const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
static double rad(double deg)
Definition: vpMath.h:104
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void initMainApplication()
perform some initialization in the main program thread
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:434
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:247
void get_eJe(vpMatrix &eJe)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
void setZoomFactor(const float zoom)
set the size of the camera/frame