ViSP
manSimu4Dots.cpp
1 /****************************************************************************
2  *
3  * $Id: manSimu4Dots.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Simulation of a visual servoing with visualization and image generation.
35  *
36  * Authors:
37  * Eric Marchand
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
54 #include <visp/vpConfig.h>
55 #include <visp/vpDebug.h>
56 
57 
58 #if (defined(VISP_HAVE_COIN_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
59 
60 #include <visp/vpImage.h>
61 #include <visp/vpCameraParameters.h>
62 #include <visp/vpTime.h>
63 #include <visp/vpImage.h>
64 #include <visp/vpImageConvert.h>
65 #include <visp/vpSimulator.h>
66 
67 #if defined(VISP_HAVE_X11)
68 # include <visp/vpDisplayX.h>
69 #elif defined(VISP_HAVE_GDI)
70 # include <visp/vpDisplayGDI.h>
71 #elif defined(VISP_HAVE_GTK)
72 # include <visp/vpDisplayGTK.h>
73 #endif
74 // You may have strange compiler issues using the simulator based on SoQt
75 // and the vpDisplayGTK. In that case prefer to use another display like
76 // vpDisplayX under linux or vpDisplayGDI under Windows
77 #include <visp/vpMath.h>
78 #include <visp/vpHomogeneousMatrix.h>
79 #include <visp/vpPose.h>
80 #include <visp/vpFeaturePoint.h>
81 #include <visp/vpDot2.h>
82 #include <visp/vpServo.h>
83 #include <visp/vpServoDisplay.h>
84 #include <visp/vpRobotCamera.h>
85 #include <visp/vpFeatureBuilder.h>
86 #include <visp/vpIoTools.h>
87 
88 static
89 void *mainLoop (void *_simu)
90 {
91  // pointer copy of the vpSimulator instance
92  vpSimulator *simu = (vpSimulator *)_simu ;
93 
94  // Simulation initialization
95  simu->initMainApplication() ;
96 
98  // Set the initial camera location
99  vpHomogeneousMatrix cMo(0.3,0.2,3,
101 
103  // Initialize the robot
104  vpRobotCamera robot ;
105  robot.setSamplingTime(0.04); // 40ms
106  robot.setPosition(cMo) ;
107  // Send the robot position to the visualizator
108  simu->setCameraPosition(cMo) ;
109  // Initialize the camera parameters
110  vpCameraParameters cam ;
111  simu->getCameraParameters(cam);
112 
114  // Desired visual features initialization
115 
116  // sets the points coordinates in the object frame (in meter)
117  vpPoint point[4] ;
118  point[0].setWorldCoordinates(-0.1,-0.1,0) ;
119  point[1].setWorldCoordinates(0.1,-0.1,0) ;
120  point[2].setWorldCoordinates(0.1,0.1,0) ;
121  point[3].setWorldCoordinates(-0.1,0.1,0) ;
122 
123  // sets the desired camera location
124  vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
125 
126  // computes the 3D point coordinates in the camera frame and its 2D coordinates
127  for (int i = 0 ; i < 4 ; i++)
128  point[i].project(cMo_d) ;
129 
130  // creates the associated features
131  vpFeaturePoint pd[4] ;
132  for (int i = 0 ; i < 4 ; i++)
133  vpFeatureBuilder::create(pd[i],point[i]) ;
134 
135 
136 
138  // Current visual features initialization
139  unsigned int height = simu->getInternalHeight();
140  unsigned int width = simu->getInternalWidth();
141 
142  // Create a greyscale image
143  vpImage<unsigned char> I(height,width);
144 
145  //Display initialization
146 #if defined(VISP_HAVE_X11)
147  vpDisplayX disp;
148 #elif defined(VISP_HAVE_GDI)
149  vpDisplayGDI disp;
150 #elif defined(VISP_HAVE_GTK)
151  vpDisplayGTK disp;
152 #endif
153  disp.init(I,100,100,"Simulation display");
154  // disp(I);
155  // Get the current image
156  vpTime::wait(500); // wait to be sure the image is generated
157  simu->getInternalImage(I);
158 
159  // Display the current image
161  vpDisplay::flush(I);
162 
163  // Initialize the four dots tracker
164  std::cout << "A click in the four dots clockwise. " << std::endl;
165  vpDot2 dot[4];
166  vpFeaturePoint p[4];
167  for (int i = 0 ; i < 4 ; i++)
168  {
169  dot[i].setGraphics(true);
170  // Call for a click
171  std::cout << "A click in the dot " << i << std::endl;
172  dot[i].initTracking(I);
173  // Create the associated feature
174  vpFeatureBuilder::create(p[i],cam,dot[i]);
175  // flush the display
176  vpDisplay::flush(I);
177  }
178 
179 
181  // Task defintion
182  vpServo task ;
183  // we want an eye-in-hand control law ;
186 
187  // Set the position of the camera in the end-effector frame
188  vpHomogeneousMatrix cMe ;
189  vpVelocityTwistMatrix cVe(cMe) ;
190  task.set_cVe(cVe) ;
191  // Set the Jacobian (expressed in the end-effector frame)
192  vpMatrix eJe ;
193  robot.get_eJe(eJe) ;
194  task.set_eJe(eJe) ;
195 
196  // we want to see a point on a point
197  for (int i = 0 ; i < 4 ; i++)
198  task.addFeature(p[i],pd[i]) ;
199  // Set the gain
200  task.setLambda(1.0) ;
201  // Print the current information about the task
202  task.print();
203 
204  vpTime::wait(500);
205 
207  // The control loop
208  int k = 0;
209  while(k++ < 200){
210  double t = vpTime::measureTimeMs();
211 
212  // Get the current internal camera view and display it
213  simu->getInternalImage(I);
215 
216  // Track the four dots and update the associated visual features
217  for (int i = 0 ; i < 4 ; i++)
218  {
219  dot[i].track(I) ;
220  vpFeatureBuilder::create(p[i],cam,dot[i]) ;
221  }
222 
223  // Display the desired and current visual features
224  vpServoDisplay::display(task,cam,I) ;
225  vpDisplay::flush(I);
226 
227  // Update the robot Jacobian
228  robot.get_eJe(eJe) ;
229  task.set_eJe(eJe) ;
230 
231  // Compute the control law
232  vpColVector v = task.computeControlLaw() ;
233 
234  // Send the computed velocity to the robot and compute the new robot position
236  robot.getPosition(cMo) ;
237 
238  // Send the robot position to the visualizator
239  simu->setCameraPosition(cMo) ;
240 
241 
242  // Wait 40 ms
243  vpTime::wait(t,40);
244  }
245  // Print information about the task
246  task.print();
247  task.kill();
248  simu->closeMainApplication() ;
249 
250  void *a=NULL ;
251  return a ;
252 }
253 
254 
255 int
256 main()
257 {
258  try {
259  vpSimulator simu ;
260 
261  // Internal view initialization : view from the robot camera
262  simu.initInternalViewer(480, 360) ;
263  // External view initialization : view from an external camera
264  simu.initExternalViewer(300, 300) ;
265 
266  // Inernal camera paramters initialization
267  vpCameraParameters cam(800,800,240,180) ;
268  simu.setInternalCameraParameters(cam) ;
269 
270  vpTime::wait(500) ;
271  // Load the scene
272  std::cout << "Load : ./4Points.iv" << std::endl
273  << "This file should be in the working directory" << std::endl;
274 
275  simu.load("./4points.iv") ;
276 
277  // Run the main loop
278  simu.initApplication(&mainLoop) ;
279  // Run the simulator
280  simu.mainLoop() ;
281  return 0;
282  }
283  catch(vpException e) {
284  std::cout << "Catch an exception: " << e << std::endl;
285  return 1;
286  }
287 }
288 
289 #else
290 int
291 main()
292 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
293 
294 }
295 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:418
void setPosition(const vpHomogeneousMatrix &cMw)
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:102
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:439
unsigned int getInternalHeight() const
Definition: vpSimulator.h:186
void closeMainApplication()
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:449
error that can be emited by ViSP classes.
Definition: vpException.h:76
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void getCameraParameters(vpCameraParameters &cam)
get the intrinsic parameters of the camera
Definition: vpSimulator.h:299
static double measureTimeMs()
Definition: vpTime.cpp:86
virtual void mainLoop()
activate the mainloop
static int wait(double t0, double t)
Definition: vpTime.cpp:149
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:127
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:465
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
Class that defines what is a point.
Definition: vpPoint.h:65
void getInternalImage(vpImage< unsigned char > &I)
get an Image of the internal view
virtual void setSamplingTime(const double &delta_t)
void kill()
Definition: vpServo.cpp:189
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:902
Class that defines the simplest robot: a free flying camera.
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:370
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void load(const char *file_name)
load an iv file
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:522
static double rad(double deg)
Definition: vpMath.h:100
void getPosition(vpHomogeneousMatrix &cMw) const
void initMainApplication()
perform some initialization in the main program thread
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void get_eJe(vpMatrix &eJe)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:414
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:266
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:251
unsigned int getInternalWidth() const
Definition: vpSimulator.h:180
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:220
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
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)
void setGraphics(const bool activate)
Definition: vpDot2.h:312
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74