Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
wireframeSimulator.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  * Demonstration of the wireframe simulator
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
45 #include <stdlib.h>
46 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpHomogeneousMatrix.h>
49 #include <visp3/core/vpImage.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/gui/vpDisplayD3D.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayGTK.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/io/vpImageIo.h>
58 #include <visp3/io/vpParseArgv.h>
59 #include <visp3/robot/vpWireFrameSimulator.h>
60 
61 #define GETOPTARGS "cdh"
62 
63 #ifdef VISP_HAVE_DISPLAY
64 
65 void usage(const char *name, const char *badparam);
66 bool getOptions(int argc, const char **argv, bool &display, bool &click);
67 
76 void usage(const char *name, const char *badparam)
77 {
78  fprintf(stdout, "\n\
79 Demonstration of the wireframe simulator.\n\
80 \n\
81 The goal of this example is to present the basic functionalities of the wire frame simulator.\n\
82 \n\
83 SYNOPSIS\n\
84  %s [-c] [-d] [-h]\n", name);
85 
86  fprintf(stdout, "\n\
87 OPTIONS: Default\n\
88  -c \n\
89  Disable mouse click.\n\
90 \n\
91  -d \n\
92  Turn off the display.\n\
93 \n\
94  -h\n\
95  Print the help.\n");
96 
97  if (badparam)
98  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
99 }
100 
113 bool getOptions(int argc, const char **argv, bool &display, bool &click)
114 {
115  const char *optarg_;
116  int c;
117  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
118 
119  switch (c) {
120  case 'c':
121  click = false;
122  break;
123  case 'd':
124  display = false;
125  break;
126  case 'h':
127  usage(argv[0], NULL);
128  return false;
129  break;
130 
131  default:
132  usage(argv[0], optarg_);
133  return false;
134  break;
135  }
136  }
137 
138  if ((c == 1) || (c == -1)) {
139  // standalone param or error
140  usage(argv[0], NULL);
141  std::cerr << "ERROR: " << std::endl;
142  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
143  return false;
144  }
145 
146  return true;
147 }
148 
149 int main(int argc, const char **argv)
150 {
151  try {
152  bool opt_display = true;
153  bool opt_click = true;
154 
155  // Read the command line options
156  if (getOptions(argc, argv, opt_display, opt_click) == false) {
157  exit(-1);
158  }
159 
160  /*
161  Three vpImage are created : one for the main camera and the others
162  for two external cameras
163  */
164  vpImage<vpRGBa> Iint(480, 640, 255);
165  vpImage<vpRGBa> Iext1(480, 640, 255);
166  vpImage<vpRGBa> Iext2(480, 640, 255);
167 
168 /*
169 Create a display for each different cameras.
170 */
171 #if defined VISP_HAVE_X11
172  vpDisplayX display[3];
173 #elif defined VISP_HAVE_OPENCV
174  vpDisplayOpenCV display[3];
175 #elif defined VISP_HAVE_GDI
176  vpDisplayGDI display[3];
177 #elif defined VISP_HAVE_D3D9
178  vpDisplayD3D display[3];
179 #elif defined VISP_HAVE_GTK
180  vpDisplayGTK display[3];
181 #endif
182 
183  if (opt_display) {
184  // Display size is automatically defined by the image (I) size
185  display[0].init(Iint, 100, 100, "The internal view");
186  display[1].init(Iext1, 100, 100, "The first external view");
187  display[2].init(Iext2, 100, 100, "The second external view");
188  vpDisplay::setWindowPosition(Iint, 0, 0);
189  vpDisplay::setWindowPosition(Iext1, 700, 0);
190  vpDisplay::setWindowPosition(Iext2, 0, 550);
191  vpDisplay::display(Iint);
192  vpDisplay::flush(Iint);
193  vpDisplay::display(Iext1);
194  vpDisplay::flush(Iext1);
195  vpDisplay::display(Iext2);
196  vpDisplay::flush(Iext2);
197  }
198 
199  // The homogeneous matrix which gives the current position of the main
200  // camera relative to the object
201  vpHomogeneousMatrix cMo(0, 0.05, 1.3, vpMath::rad(15), vpMath::rad(25), 0);
202 
203  // The homogeneous matrix which gives the desired position of the main
204  // camera relative to the object
206 
207  // Declaration of the simulator
209  /*
210  Set the scene. It enables to choose the shape of the object and the shape
211  of the desired object which is displayed in the main camera view. It
212  exists several objects in ViSP. See the html documentation of the
213  simulator class to have the complete list.
214 
215  Note : if you don't want to have a desired object displayed in the main
216  camera view you can use the initObject Method.
217 
218  Here the object is a plate with 4 points and it is the same object which
219  is used to display the object at the desired position.
220  */
222 
223  /*
224  The object at the current position will be displayed in blue
225  The object at the desired position will be displayed in red
226  The camera will be display in green
227  */
231  /*
232  Set the position of the object frame in the current and desired camera frame
233  */
234  sim.setCameraPositionRelObj(cMo);
235  sim.setDesiredCameraPosition(cdMo);
236  /*
237  Set the main external camera's position relative to the world reference
238  frame. More information about the different frames are given in the html
239  documentation.
240  */
241  vpHomogeneousMatrix camMw(vpHomogeneousMatrix(0.0, 0, 4.5, vpMath::rad(0), vpMath::rad(-30), 0));
242  sim.setExternalCameraPosition(camMw);
243 
244  /*
245  Set the parameters of the cameras (internal and external)
246  */
247  vpCameraParameters camera(1000, 1000, 320, 240);
248  sim.setInternalCameraParameters(camera);
249  sim.setExternalCameraParameters(camera);
250 
251  vpHomogeneousMatrix camoMw(vpHomogeneousMatrix(-0.3, 0.2, 2.5, vpMath::rad(0), vpMath::rad(10), 0));
252 
253  if (opt_display) {
254  // Get the view of the internal camera
255  sim.getInternalImage(Iint);
256  // Get the view of the main external camera
257  sim.getExternalImage(Iext1);
258  // Get the view of an external camera that you can positionned thanks
259  // to a vpHomogeneousMatrix which describes the position of the world reference frame in the camera frame
260  sim.getExternalImage(Iext2, camoMw);
261  // Display the views.
262 
263  vpDisplay::flush(Iint);
264  vpDisplay::flush(Iext1);
265  vpDisplay::flush(Iext2);
266  }
267 
268  std::cout << std::endl;
269  std::cout << "Here are presented the effect of the basic functions of "
270  "the simulator"
271  << std::endl;
272  std::cout << std::endl;
273 
274  if (opt_display) {
275  if (opt_click) {
276  std::cout << "Click on the internal view window to continue. the "
277  "object will move. The external cameras are fixed. The "
278  "main camera moves too because the homogeneous matrix "
279  "cMo didn't change."
280  << std::endl;
281  vpDisplay::getClick(Iint);
282  }
283  vpDisplay::display(Iint);
284  vpDisplay::display(Iext1);
285  vpDisplay::display(Iext2);
286  }
287  /*
288  To move the object you have to define a vpHomogeneousMatrix which gives
289  the position of the object relative to the world refrenece frame.
290  */
291  vpHomogeneousMatrix mov(0.05, 0.05, 0.2, vpMath::rad(10), 0, 0);
292  sim.set_fMo(mov);
293 
294  if (opt_display) {
295  // Get the view of the internal camera
296  sim.getInternalImage(Iint);
297  // Get the view of the main external camera
298  sim.getExternalImage(Iext1);
299  // Get the view of an external camera that you can positionned thanks
300  // to a vpHomogeneousMatrix which describes the position of the world reference frame in the camera frame
301  sim.getExternalImage(Iext2, camoMw);
302  // Display the views.
303 
304  vpDisplay::flush(Iint);
305  vpDisplay::flush(Iext1);
306  vpDisplay::flush(Iext2);
307  }
308 
309  std::cout << std::endl;
310  if (opt_display) {
311  if (opt_click) {
312  std::cout << "Click on the internal view window to continue" << std::endl;
313  vpDisplay::getClick(Iint);
314  }
315  }
316  std::cout << std::endl;
317  std::cout << "Now you can move the main external camera. Click inside "
318  "the corresponding window with one of the three buttons of "
319  "your mouse and move the pointer."
320  << std::endl;
321  std::cout << std::endl;
322  std::cout << "Click on the internal view window when you are finished" << std::endl;
323 
324  /*
325  To move the main external camera you need a loop containing the
326  getExternalImage method. This functionnality is only available for the
327  main external camera.
328  */
329  if (opt_display && opt_click) {
330  while (!vpDisplay::getClick(Iint, false)) {
331  vpDisplay::display(Iext1);
332  sim.getExternalImage(Iext1);
333  vpDisplay::flush(Iext1);
334  }
335  }
336 
337  std::cout << std::endl;
338  std::cout << "You have seen the main capabilities of the simulator. "
339  "Other specific functionalities are available. Please "
340  "refers to the html documentation to access the list of all "
341  "functions"
342  << std::endl;
343  return EXIT_SUCCESS;
344  } catch (const vpException &e) {
345  std::cout << "Catch an exception: " << e << std::endl;
346  return EXIT_SUCCESS;
347  }
348 }
349 #else
350 int main()
351 {
352  std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
353  std::cout << "Tip if you are on a unix-like system:" << std::endl;
354  std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
355  std::cout << "Tip if you are on a windows-like system:" << std::endl;
356  std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
357  return EXIT_SUCCESS;
358 }
359 
360 #endif
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setCameraColor(const vpColor &col)
void setDesiredViewColor(const vpColor &col)
void set_fMo(const vpHomogeneousMatrix &fMo_)
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:134
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setCurrentViewColor(const vpColor &col)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:217
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:106
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 init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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:134
void getExternalImage(vpImage< unsigned char > &I)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
static double rad(double deg)
Definition: vpMath.h:110
void setExternalCameraParameters(const vpCameraParameters &cam)
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
void setInternalCameraParameters(const vpCameraParameters &cam)
static const vpColor blue
Definition: vpColor.h:223