ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
manSimu4Dots.cpp
1 /****************************************************************************
2  *
3  * $Id: manSimu4Dots.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  vpSimulator simu ;
259 
260  // Internal view initialization : view from the robot camera
261  simu.initInternalViewer(480, 360) ;
262  // External view initialization : view from an external camera
263  simu.initExternalViewer(300, 300) ;
264 
265  // Inernal camera paramters initialization
266  vpCameraParameters cam(800,800,240,180) ;
267  simu.setInternalCameraParameters(cam) ;
268 
269  vpTime::wait(500) ;
270  // Load the scene
271  std::cout << "Load : ./4Points.iv" << std::endl
272  << "This file should be in the working directory" << std::endl;
273 
274  simu.load("./4points.iv") ;
275 
276  // Run the main loop
277  simu.initApplication(&mainLoop) ;
278  // Run the simulator
279  simu.mainLoop() ;
280 }
281 
282 #else
283 int
284 main()
285 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
286 
287 }
288 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
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:401
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:133
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)
create a new ste of two visual features
Definition: vpServo.cpp:444
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
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:131
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:444
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:230
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1991
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()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
Class that defines the simplest robot: a free flying camera.
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:238
Generic class defining intrinsic camera parameters.
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)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
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 initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:245
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
unsigned int getInternalWidth() const
Definition: vpSimulator.h:180
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
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