Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
servoSimu4Points.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  * Demonstration of the wireframe simulator with a simple visual servoing
33  *
34 *****************************************************************************/
35 
42 #include <stdlib.h>
43 
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpImage.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpTime.h>
50 #include <visp3/core/vpVelocityTwistMatrix.h>
51 #include <visp3/gui/vpDisplayD3D.h>
52 #include <visp3/gui/vpDisplayGDI.h>
53 #include <visp3/gui/vpDisplayGTK.h>
54 #include <visp3/gui/vpDisplayOpenCV.h>
55 #include <visp3/gui/vpDisplayX.h>
56 #include <visp3/gui/vpPlot.h>
57 #include <visp3/io/vpImageIo.h>
58 #include <visp3/io/vpParseArgv.h>
59 #include <visp3/robot/vpSimulatorCamera.h>
60 #include <visp3/robot/vpWireFrameSimulator.h>
61 #include <visp3/visual_features/vpFeatureBuilder.h>
62 #include <visp3/visual_features/vpFeaturePoint.h>
63 #include <visp3/vs/vpServo.h>
64 
65 #define GETOPTARGS "dhp"
66 
67 #if defined(VISP_HAVE_DISPLAY) && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
68 
77 void usage(const char *name, const char *badparam)
78 {
79  fprintf(stdout, "\n\
80 Demonstration of the wireframe simulator with a simple visual servoing.\n\
81  \n\
82 The visual servoing consists in bringing the camera at a desired \n\
83 position from the object.\n\
84  \n\
85 The visual features used to compute the pose of the camera and \n\
86 thus the control law are four points.\n\
87  \n\
88 This demonstration explains also how to move the object around a world\n\
89 reference frame. Here, the movement is a rotation around the x and y axis\n\
90 at a given distance from the world frame. In fact the object trajectory\n\
91 is on a sphere whose center is the origin of the world frame.\n\
92  \n\
93 SYNOPSIS\n\
94  %s [-d] [-p] [-h]\n",
95  name);
96 
97  fprintf(stdout, "\n\
98 OPTIONS: Default\n\
99  -d \n\
100  Turn off the display.\n\
101  \n\
102  -p \n\
103  Turn off the plotter.\n\
104  \n\
105  -h\n\
106  Print the help.\n");
107 
108  if (badparam)
109  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110 }
111 
124 bool getOptions(int argc, const char **argv, bool &display, bool &plot)
125 {
126  const char *optarg_;
127  int c;
128  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
129 
130  switch (c) {
131  case 'd':
132  display = false;
133  break;
134  case 'p':
135  plot = false;
136  break;
137  case 'h':
138  usage(argv[0], nullptr);
139  return false;
140 
141  default:
142  usage(argv[0], optarg_);
143  return false;
144  }
145  }
146 
147  if ((c == 1) || (c == -1)) {
148  // standalone param or error
149  usage(argv[0], nullptr);
150  std::cerr << "ERROR: " << std::endl;
151  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
152  return false;
153  }
154 
155  return true;
156 }
157 
158 int main(int argc, const char **argv)
159 {
160  try {
161  bool opt_display = true;
162  bool opt_plot = true;
163  std::string filename = vpIoTools::getParent(argv[0]) + "/mire.png";
164  std::cout << "Read " << filename << std::endl;
165 
166  // Read the command line options
167  if (getOptions(argc, argv, opt_display, opt_plot) == false) {
168  return EXIT_FAILURE;
169  }
170 
171  vpImage<vpRGBa> Iint(480, 640, 255);
172  vpImage<vpRGBa> Iext1(480, 640, 255);
173  vpImage<vpRGBa> Iext2(480, 640, 255);
174 
175 #if defined(VISP_HAVE_X11)
176  vpDisplayX display[3];
177 #elif defined(HAVE_OPENCV_HIGHGUI)
179 #elif defined(VISP_HAVE_GDI)
181 #elif defined(VISP_HAVE_D3D9)
183 #elif defined(VISP_HAVE_GTK)
185 #endif
186 
187  if (opt_display) {
188  // Display size is automatically defined by the image (I) size
189  display[0].init(Iint, 100, 100, "The internal view");
190  display[1].init(Iext1, 100, 100, "The first external view");
191  display[2].init(Iext2, 100, 100, "The second external view");
192  vpDisplay::setWindowPosition(Iint, 0, 0);
193  vpDisplay::setWindowPosition(Iext1, 750, 0);
194  vpDisplay::setWindowPosition(Iext2, 0, 550);
195  vpDisplay::display(Iint);
196  vpDisplay::flush(Iint);
197  vpDisplay::display(Iext1);
198  vpDisplay::flush(Iext1);
199  vpDisplay::display(Iext2);
200  vpDisplay::flush(Iext2);
201  }
202 
203  vpPlot *plotter = nullptr;
204 
205  if (opt_plot) {
206  plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
207  plotter->setTitle(0, "Visual features error");
208  plotter->setTitle(1, "Camera velocities");
209  plotter->initGraph(0, 8);
210  plotter->initGraph(1, 6);
211  plotter->setLegend(0, 0, "error_feat_p1_x");
212  plotter->setLegend(0, 1, "error_feat_p1_y");
213  plotter->setLegend(0, 2, "error_feat_p2_x");
214  plotter->setLegend(0, 3, "error_feat_p2_y");
215  plotter->setLegend(0, 4, "error_feat_p3_x");
216  plotter->setLegend(0, 5, "error_feat_p3_y");
217  plotter->setLegend(0, 6, "error_feat_p4_x");
218  plotter->setLegend(0, 7, "error_feat_p4_y");
219  plotter->setLegend(1, 0, "vc_x");
220  plotter->setLegend(1, 1, "vc_y");
221  plotter->setLegend(1, 2, "vc_z");
222  plotter->setLegend(1, 3, "wc_x");
223  plotter->setLegend(1, 4, "wc_y");
224  plotter->setLegend(1, 5, "wc_z");
225  }
226 
227  vpServo task;
228  vpSimulatorCamera robot;
229  float sampling_time = 0.020f; // Sampling period in second
230  robot.setSamplingTime(sampling_time);
231 
232  // Since the task gain lambda is very high, we need to increase default
233  // max velocities
234  robot.setMaxTranslationVelocity(10);
236 
237  // Set initial position of the object in the camera frame
238  vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
239  // Set desired position of the object in the camera frame
240  vpHomogeneousMatrix cdMo(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
241  // Set initial position of the object in the world frame
242  vpHomogeneousMatrix wMo(0.0, 0.0, 0.2, 0, 0, 0);
243  // Position of the camera in the world frame
245  wMc = wMo * cMo.inverse();
246 
247  // The four point used as visual features
248  vpPoint point[4];
249  point[0].setWorldCoordinates(-0.1, -0.1, 0);
250  point[3].setWorldCoordinates(-0.1, 0.1, 0);
251  point[2].setWorldCoordinates(0.1, 0.1, 0);
252  point[1].setWorldCoordinates(0.1, -0.1, 0);
253 
254  // Projection of the points
255  for (int i = 0; i < 4; i++)
256  point[i].track(cMo);
257 
258  // Set the current visual feature
259  vpFeaturePoint p[4];
260  for (int i = 0; i < 4; i++)
261  vpFeatureBuilder::create(p[i], point[i]);
262 
263  // Projection of the points
264  for (int i = 0; i < 4; i++)
265  point[i].track(cdMo);
266 
267  vpFeaturePoint pd[4];
268  for (int i = 0; i < 4; i++)
269  vpFeatureBuilder::create(pd[i], point[i]);
270 
273 
274  vpHomogeneousMatrix cMe; // Identity
275  vpVelocityTwistMatrix cVe(cMe);
276  task.set_cVe(cVe);
277 
278  vpMatrix eJe;
279  robot.get_eJe(eJe);
280  task.set_eJe(eJe);
281 
282  for (int i = 0; i < 4; i++)
283  task.addFeature(p[i], pd[i]);
284 
285  task.setLambda(10);
286 
287  std::list<vpImageSimulator> list;
288  vpImageSimulator imsim;
289 
290  vpColVector X[4];
291  for (int i = 0; i < 4; i++)
292  X[i].resize(3);
293  X[0][0] = -0.2;
294  X[0][1] = -0.2;
295  X[0][2] = 0;
296 
297  X[1][0] = 0.2;
298  X[1][1] = -0.2;
299  X[1][2] = 0;
300 
301  X[2][0] = 0.2;
302  X[2][1] = 0.2;
303  X[2][2] = 0;
304 
305  X[3][0] = -0.2;
306  X[3][1] = 0.2;
307  X[3][2] = 0;
308 
309  imsim.init(filename.c_str(), X);
310 
311  list.push_back(imsim);
312 
314 
315  // Set the scene
317 
318  // Initialize simulator frames
319  sim.set_fMo(wMo); // Position of the object in the world reference frame
320  sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
321  sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
322 
323  // Set the External camera position
324  vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
325  sim.setExternalCameraPosition(camMf);
326 
327  // Computes the position of a camera which is fixed in the object frame
328  vpHomogeneousMatrix camoMf(0, 0.0, 1.5, 0, vpMath::rad(140), 0);
329  camoMf = camoMf * (sim.get_fMo().inverse());
330 
331  // Set the parameters of the cameras (internal and external)
332  vpCameraParameters camera(1000, 1000, 320, 240);
333  sim.setInternalCameraParameters(camera);
334  sim.setExternalCameraParameters(camera);
335 
336  int max_iter = 10;
337 
338  if (opt_display) {
339  max_iter = 2500;
340 
341  // Get the internal and external views
342  sim.getInternalImage(Iint);
343  sim.getExternalImage(Iext1);
344  sim.getExternalImage(Iext2, camoMf);
345 
346  // Display the object frame (current and desired position)
347  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
348  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
349 
350  // Display the object frame the world reference frame and the camera
351  // frame
352  vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
353  vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
354  vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
355 
356  // Display the world reference frame and the object frame
357  vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
358  vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
359 
360  vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
361 
362  vpDisplay::flush(Iint);
363  vpDisplay::flush(Iext1);
364  vpDisplay::flush(Iext2);
365 
366  std::cout << "Click on a display" << std::endl;
367 
368  while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
369  !vpDisplay::getClick(Iext2, false)) {
370  };
371  }
372 
373  robot.setPosition(wMc);
374  // Print the task
375  task.print();
376 
377  int iter = 0;
378  bool stop = false;
379  vpColVector v;
380 
381  double t_prev, t = vpTime::measureTimeMs();
382 
383  while (iter++ < max_iter && !stop) {
384  t_prev = t;
385  t = vpTime::measureTimeMs();
386 
387  if (opt_display) {
388  vpDisplay::display(Iint);
389  vpDisplay::display(Iext1);
390  vpDisplay::display(Iext2);
391  }
392 
393  robot.get_eJe(eJe);
394  task.set_eJe(eJe);
395 
396  wMc = robot.getPosition();
397  cMo = wMc.inverse() * wMo;
398  for (int i = 0; i < 4; i++) {
399  point[i].track(cMo);
400  vpFeatureBuilder::create(p[i], point[i]);
401  }
402 
403  v = task.computeControlLaw();
405 
406  // Compute the movement of the object around the world reference frame.
407  vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
408  vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5 * iter), 0, 0);
409  vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5 * iter), 0);
410 
411  // Move the object in the world frame
412  wMo = b * c * a;
413 
414  sim.set_fMo(wMo); // Move the object in the simulator
415  sim.setCameraPositionRelObj(cMo);
416 
417  // Compute the position of the external view which is fixed in the
418  // object frame
419  camoMf.buildFrom(0, 0.0, 1.5, 0, vpMath::rad(150), 0);
420  camoMf = camoMf * (sim.get_fMo().inverse());
421 
422  if (opt_plot) {
423  plotter->plot(0, iter, task.getError());
424  plotter->plot(1, iter, v);
425  }
426 
427  if (opt_display) {
428  // Get the internal and external views
429  sim.getInternalImage(Iint);
430  sim.getExternalImage(Iext1);
431  sim.getExternalImage(Iext2, camoMf);
432 
433  // Display the object frame (current and desired position)
434  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
435  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
436 
437  // Display the camera frame, the object frame the world reference
438  // frame
439  vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
440  vpColor::none);
441  vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo(), camera, 0.2, vpColor::none);
443 
444  // Display the world reference frame and the object frame
445  vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
446  vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
447 
448  vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
449 
450  std::stringstream ss;
451  ss << "Loop time: " << t - t_prev << " ms";
452  vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
453 
454  if (vpDisplay::getClick(Iint, false)) {
455  stop = true;
456  }
457  vpDisplay::flush(Iint);
458  vpDisplay::flush(Iext1);
459  vpDisplay::flush(Iext2);
460 
461  vpTime::wait(t, sampling_time * 1000); // Wait ms
462  }
463 
464  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
465  }
466 
467  if (opt_plot && plotter != nullptr) {
468  vpDisplay::display(Iint);
469  sim.getInternalImage(Iint);
470  vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
471  vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
472  vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
473  if (vpDisplay::getClick(Iint)) {
474  stop = true;
475  }
476  vpDisplay::flush(Iint);
477 
478  delete plotter;
479  }
480 
481  task.print();
482 
483  return EXIT_SUCCESS;
484  }
485  catch (const vpException &e) {
486  std::cout << "Catch an exception: " << e << std::endl;
487  return EXIT_FAILURE;
488  }
489 }
490 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
491 int main()
492 {
493  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
494  return EXIT_SUCCESS;
495 }
496 #else
497 int main()
498 {
499  std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..."
500  << std::endl;
501  std::cout << "Tip if you are on a unix-like system:" << std::endl;
502  std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
503  std::cout << "Tip if you are on a windows-like system:" << std::endl;
504  std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
505  return EXIT_SUCCESS;
506 }
507 
508 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:101
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void init(const vpImage< unsigned char > &I, vpColVector *X)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:2038
static double rad(double deg)
Definition: vpMath.h:127
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:109
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:202
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:269
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:503
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:110
void get_eJe(vpMatrix &eJe) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:257
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:236
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:378
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:169
void setLambda(double c)
Definition: vpServo.h:976
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
vpColVector getError() const
Definition: vpServo.h:504
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
@ DESIRED
Definition: vpServo.h:202
Class that defines the simplest robot: a free flying camera.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
void getExternalImage(vpImage< unsigned char > &I)
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()