ViSP  3.0.0
manSimu4Dots.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Simulation of a visual servoing with visualization and image generation.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
51 #include <visp3/core/vpConfig.h>
52 #include <visp3/core/vpDebug.h>
53 
54 
55 #if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
56 
57 #include <visp3/core/vpImage.h>
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpTime.h>
60 #include <visp3/core/vpImage.h>
61 #include <visp3/core/vpImageConvert.h>
62 #include <visp3/ar/vpSimulator.h>
63 
64 #if defined(VISP_HAVE_X11)
65 # include <visp3/gui/vpDisplayX.h>
66 #elif defined(VISP_HAVE_GDI)
67 # include <visp3/gui/vpDisplayGDI.h>
68 #elif defined(VISP_HAVE_GTK)
69 # include <visp3/gui/vpDisplayGTK.h>
70 #endif
71 // You may have strange compiler issues using the simulator based on SoQt
72 // and the vpDisplayGTK. In that case prefer to use another display like
73 // vpDisplayX under linux or vpDisplayGDI under Windows
74 #include <visp3/core/vpMath.h>
75 #include <visp3/core/vpHomogeneousMatrix.h>
76 #include <visp3/vision/vpPose.h>
77 #include <visp3/visual_features/vpFeaturePoint.h>
78 #include <visp3/blob/vpDot2.h>
79 #include <visp3/vs/vpServo.h>
80 #include <visp3/vs/vpServoDisplay.h>
81 #include <visp3/robot/vpSimulatorCamera.h>
82 #include <visp3/visual_features/vpFeatureBuilder.h>
83 #include <visp3/core/vpIoTools.h>
84 
85 static
86 void *mainLoop (void *_simu)
87 {
88  // pointer copy of the vpSimulator instance
89  vpSimulator *simu = (vpSimulator *)_simu ;
90 
91  // Simulation initialization
92  simu->initMainApplication() ;
93 
95  // Set the initial camera location
96  vpHomogeneousMatrix cMo(0.3,0.2,3,
98  vpHomogeneousMatrix wMo; // Set to identity
99  vpHomogeneousMatrix wMc; // Camera position in the world frame
100 
102  // Initialize the robot
103  vpSimulatorCamera robot ;
104  robot.setSamplingTime(0.04); // 40ms
105  wMc = wMo * cMo.inverse();
106  robot.setPosition(wMc) ;
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 
136  // Current visual features initialization
137  unsigned int height = simu->getInternalHeight();
138  unsigned int width = simu->getInternalWidth();
139 
140  // Create a greyscale image
141  vpImage<unsigned char> I(height,width);
142 
143  //Display initialization
144 #if defined(VISP_HAVE_X11)
145  vpDisplayX disp;
146 #elif defined(VISP_HAVE_GDI)
147  vpDisplayGDI disp;
148 #elif defined(VISP_HAVE_GTK)
149  vpDisplayGTK disp;
150 #endif
151  disp.init(I,100,100,"Simulation display");
152  // disp(I);
153  // Get the current image
154  vpTime::wait(500); // wait to be sure the image is generated
155  simu->getInternalImage(I);
156 
157  // Display the current image
159  vpDisplay::flush(I);
160 
161  // Initialize the four dots tracker
162  std::cout << "A click in the four dots clockwise. " << std::endl;
163  vpDot2 dot[4];
164  vpFeaturePoint p[4];
165  for (int i = 0 ; i < 4 ; i++)
166  {
167  dot[i].setGraphics(true);
168  // Call for a click
169  std::cout << "A click in the dot " << i << std::endl;
170  dot[i].initTracking(I);
171  // Create the associated feature
172  vpFeatureBuilder::create(p[i],cam,dot[i]);
173  // flush the display
174  vpDisplay::flush(I);
175  }
176 
177 
179  // Task defintion
180  vpServo task ;
181  // we want an eye-in-hand control law ;
184 
185  // Set the position of the camera in the end-effector frame
186  vpHomogeneousMatrix cMe ;
187  vpVelocityTwistMatrix cVe(cMe) ;
188  task.set_cVe(cVe) ;
189  // Set the Jacobian (expressed in the end-effector frame)
190  vpMatrix eJe ;
191  robot.get_eJe(eJe) ;
192  task.set_eJe(eJe) ;
193 
194  // we want to see a point on a point
195  for (int i = 0 ; i < 4 ; i++)
196  task.addFeature(p[i],pd[i]) ;
197  // Set the gain
198  task.setLambda(1.0) ;
199  // Print the current information about the task
200  task.print();
201 
202  vpTime::wait(500);
203 
205  // The control loop
206  int k = 0;
207  while(k++ < 200){
208  double t = vpTime::measureTimeMs();
209 
210  // Get the current internal camera view and display it
211  simu->getInternalImage(I);
213 
214  // Track the four dots and update the associated visual features
215  for (int i = 0 ; i < 4 ; i++)
216  {
217  dot[i].track(I) ;
218  vpFeatureBuilder::create(p[i],cam,dot[i]) ;
219  }
220 
221  // Display the desired and current visual features
222  vpServoDisplay::display(task,cam,I) ;
223  vpDisplay::flush(I);
224 
225  // Update the robot Jacobian
226  robot.get_eJe(eJe) ;
227  task.set_eJe(eJe) ;
228 
229  // Compute the control law
230  vpColVector v = task.computeControlLaw() ;
231 
232  // Send the computed velocity to the robot and compute the new robot position
234  wMc = robot.getPosition();
235  cMo = wMc.inverse() * wMo;
236 
237  // Send the robot position to the visualizator
238  simu->setCameraPosition(cMo) ;
239 
240  // Wait 40 ms
241  vpTime::wait(t,40);
242  }
243  // Print information about the task
244  task.print();
245  task.kill();
246  simu->closeMainApplication() ;
247 
248  void *a=NULL ;
249  return a ;
250 }
251 
252 
253 int
254 main()
255 {
256  try {
257  vpSimulator simu ;
258 
259  // Internal view initialization : view from the robot camera
260  simu.initInternalViewer(480, 360) ;
261  // External view initialization : view from an external camera
262  simu.initExternalViewer(300, 300) ;
263 
264  // Inernal camera paramters initialization
265  vpCameraParameters cam(800,800,240,180) ;
266  simu.setInternalCameraParameters(cam) ;
267 
268  vpTime::wait(500) ;
269  // Load the scene
270 
271  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
272  std::string ipath = vpIoTools::getViSPImagesDataPath();
273  std::string filename = "./4points.iv";
274 
275  // Set the default input path
276  if (! ipath.empty())
277  filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
278 
279  std::cout << "Load : " << filename << std::endl
280  << "This file should be in the working directory" << std::endl;
281 
282  simu.load(filename.c_str());
283 
284  // Run the main loop
285  simu.initApplication(&mainLoop) ;
286  // Run the simulator
287  simu.mainLoop() ;
288  return 0;
289  }
290  catch(vpException e) {
291  std::cout << "Catch an exception: " << e << std::endl;
292  return 1;
293  }
294 }
295 
296 #else
297 int
298 main()
299 { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
300 
301 }
302 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1091
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:98
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:459
unsigned int getInternalHeight() const
Definition: vpSimulator.h:182
void closeMainApplication()
Define the X11 console to display images.
Definition: vpDisplayX.h:148
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:446
error that can be emited by ViSP classes.
Definition: vpException.h:73
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:297
virtual void mainLoop()
activate the mainloop
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:124
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:461
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
Class that defines what is a point.
Definition: vpPoint.h:59
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:186
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Definition: vpServo.cpp:899
#define vpTRACE
Definition: vpDebug.h:414
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1265
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:390
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:141
void load(const char *file_name)
load an iv file
vpHomogeneousMatrix getPosition() const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:519
static double rad(double deg)
Definition: vpMath.h:104
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void initMainApplication()
perform some initialization in the main program thread
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:434
vpHomogeneousMatrix inverse() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:262
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:248
unsigned int getInternalWidth() const
Definition: vpSimulator.h:176
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:217
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:309