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