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