Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
servoSimuSquareLine2DCamVelocityDisplay.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Simulation of a 2D visual servoing on a line.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
48 #include <visp3/core/vpConfig.h>
49 #include <visp3/core/vpDebug.h>
50 
51 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
52 
53 #include <stdio.h>
54 #include <stdlib.h>
55 
56 #include <visp3/core/vpCameraParameters.h>
57 #include <visp3/core/vpHomogeneousMatrix.h>
58 #include <visp3/core/vpImage.h>
59 #include <visp3/core/vpLine.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/gui/vpDisplayGDI.h>
62 #include <visp3/gui/vpDisplayGTK.h>
63 #include <visp3/gui/vpDisplayOpenCV.h>
64 #include <visp3/gui/vpDisplayX.h>
65 #include <visp3/io/vpParseArgv.h>
66 #include <visp3/robot/vpSimulatorCamera.h>
67 #include <visp3/visual_features/vpFeatureBuilder.h>
68 #include <visp3/visual_features/vpFeatureLine.h>
69 #include <visp3/vs/vpServo.h>
70 #include <visp3/vs/vpServoDisplay.h>
71 
72 // List of allowed command line options
73 #define GETOPTARGS "cdh"
74 
75 void usage(const char *name, const char *badparam);
76 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
77 
86 void usage(const char *name, const char *badparam)
87 {
88  fprintf(stdout, "\n\
89 Simulation of 2D a visual servoing on a line:\n\
90 - eye-in-hand control law,\n\
91 - velocity computed in the camera frame,\n\
92 - display the camera view.\n\
93  \n\
94 SYNOPSIS\n\
95  %s [-c] [-d] [-h]\n", name);
96 
97  fprintf(stdout, "\n\
98 OPTIONS: Default\n\
99  \n\
100  -c\n\
101  Disable the mouse click. Useful to automaze the \n\
102  execution of this program without humain intervention.\n\
103  \n\
104  -d \n\
105  Turn off the display.\n\
106  \n\
107  -h\n\
108  Print the help.\n");
109 
110  if (badparam)
111  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
112 }
113 
126 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
127 {
128  const char *optarg_;
129  int c;
130  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
131 
132  switch (c) {
133  case 'c':
134  click_allowed = false;
135  break;
136  case 'd':
137  display = false;
138  break;
139  case 'h':
140  usage(argv[0], NULL);
141  return false;
142  break;
143 
144  default:
145  usage(argv[0], optarg_);
146  return false;
147  break;
148  }
149  }
150 
151  if ((c == 1) || (c == -1)) {
152  // standalone param or error
153  usage(argv[0], NULL);
154  std::cerr << "ERROR: " << std::endl;
155  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
156  return false;
157  }
158 
159  return true;
160 }
161 
162 int main(int argc, const char **argv)
163 {
164  try {
165  bool opt_display = true;
166  bool opt_click_allowed = true;
167 
168  // Read the command line options
169  if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
170  exit(-1);
171  }
172 
173  vpImage<unsigned char> I(512, 512, 0);
174 
175 // We open a window using either X11, GTK or GDI.
176 #if defined VISP_HAVE_X11
177  vpDisplayX display;
178 #elif defined VISP_HAVE_GTK
179  vpDisplayGTK display;
180 #elif defined VISP_HAVE_GDI
181  vpDisplayGDI display;
182 #elif defined VISP_HAVE_OPENCV
183  vpDisplayOpenCV display;
184 #endif
185 
186  if (opt_display) {
187  try {
188  // Display size is automatically defined by the image (I) size
189  display.init(I, 100, 100, "Camera view...");
190  // Display the image
191  // The image class has a member that specify a pointer toward
192  // the display that has been initialized in the display declaration
193  // therefore is is no longuer necessary to make a reference to the
194  // display variable.
196  vpDisplay::flush(I);
197  } catch (...) {
198  vpERROR_TRACE("Error while displaying the image");
199  exit(-1);
200  }
201  }
202 
203  // Set the camera parameters
204  double px, py;
205  px = py = 600;
206  double u0, v0;
207  u0 = v0 = 256;
208 
209  vpCameraParameters cam(px, py, u0, v0);
210 
211  vpServo task;
212  vpSimulatorCamera robot;
213 
214  // sets the initial camera location
215  vpHomogeneousMatrix cMo(0.2, 0.2, 1, vpMath::rad(45), vpMath::rad(45), vpMath::rad(125));
216 
217  // Compute the position of the object in the world frame
218  vpHomogeneousMatrix wMc, wMo;
219  robot.getPosition(wMc);
220  wMo = wMc * cMo;
221 
222  // sets the final camera location (for simulation purpose)
223  vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
224 
225  int nbline = 4;
226 
227  // sets the line coordinates (2 planes) in the world frame
228  vpLine line[4];
229  line[0].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
230  line[1].setWorldCoordinates(0, 1, 0, 0.05, 0, 0, 1, 0);
231  line[2].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
232  line[3].setWorldCoordinates(0, 1, 0, -0.05, 0, 0, 1, 0);
233 
234  vpFeatureLine ld[4];
235  vpFeatureLine l[4];
236 
237  // sets the desired position of the visual feature
238  for (int i = 0; i < nbline; i++) {
239  line[i].track(cMod);
240  line[i].print();
241 
242  vpFeatureBuilder::create(ld[i], line[i]);
243  }
244 
245  // computes the line coordinates in the camera frame and its 2D
246  // coordinates sets the current position of the visual feature
247  for (int i = 0; i < nbline; i++) {
248  line[i].track(cMo);
249  line[i].print();
250 
251  vpFeatureBuilder::create(l[i], line[i]);
252  l[i].print();
253  }
254 
255  // define the task
256  // - we want an eye-in-hand control law
257  // - robot is controlled in the camera frame
260  // It could be also interesting to test the following tasks
261  // task.setInteractionMatrixType(vpServo::DESIRED,
262  // vpServo::PSEUDO_INVERSE); task.setInteractionMatrixType(vpServo::MEAN,
263  // vpServo::PSEUDO_INVERSE);
264 
265  // we want to see a four lines on four lines
266  for (int i = 0; i < nbline; i++)
267  task.addFeature(l[i], ld[i]);
268 
270  vpServoDisplay::display(task, cam, I);
271  vpDisplay::flush(I);
272 
273  // set the gain
274  task.setLambda(1);
275 
276  // Display task information
277  task.print();
278 
279  if (opt_display && opt_click_allowed) {
280  std::cout << "\n\nClick in the camera view window to start..." << std::endl;
282  }
283 
284  unsigned int iter = 0;
285  // loop
286  while (iter++ < 200) {
287  std::cout << "---------------------------------------------" << iter << std::endl;
288  vpColVector v;
289 
290  // get the robot position
291  robot.getPosition(wMc);
292  // Compute the position of the object frame in the camera frame
293  cMo = wMc.inverse() * wMo;
294 
295  // new line position: retrieve x,y and Z of the vpLine structure
296  for (int i = 0; i < nbline; i++) {
297  line[i].track(cMo);
298  vpFeatureBuilder::create(l[i], line[i]);
299  }
300 
301  if (opt_display) {
303  vpServoDisplay::display(task, cam, I);
304  vpDisplay::flush(I);
305  }
306 
307  // compute the control law
308  v = task.computeControlLaw();
309 
310  // send the camera velocity to the controller
312 
313  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
314  ;
315  }
316 
317  if (opt_display && opt_click_allowed) {
318  std::cout << "\nClick in the camera view window to end..." << std::endl;
320  }
321 
322  // Display task information
323  task.print();
324  task.kill();
325  return EXIT_SUCCESS;
326  } catch (const vpException &e) {
327  std::cout << "Catch a ViSP exception: " << e << std::endl;
328  return EXIT_FAILURE;
329  }
330 }
331 
332 #else
333 int main()
334 {
335  std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
336  std::cout << "Tip if you are on a unix-like system:" << std::endl;
337  std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
338  std::cout << "Tip if you are on a windows-like system:" << std::endl;
339  std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
340  return EXIT_SUCCESS;
341 }
342 #endif
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Definition: vpLine.cpp:85
#define vpERROR_TRACE
Definition: vpDebug.h:393
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
error that can be emited by ViSP classes.
Definition: vpException.h:71
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix getPosition() const
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
virtual void print() const
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:105
void kill()
Definition: vpServo.cpp:192
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
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:406
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:137
void print(unsigned int select=FEATURE_ALL) const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
static double rad(double deg)
Definition: vpMath.h:108
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
vpColVector getError() const
Definition: vpServo.h:282
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)