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