Visual Servoing Platform version 3.5.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
84static 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
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
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
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);
219
220 // Update the robot Jacobian
221 robot.get_eJe(eJe);
222 task.set_eJe(eJe);
223
224 // Compute the control law
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 simu->closeMainApplication();
242
243 void *a = NULL;
244 return a;
245}
246
247int main()
248{
249 try {
250 vpSimulator simu;
251
252 // Internal view initialization : view from the robot camera
253 simu.initInternalViewer(480, 360);
254 // External view initialization : view from an external camera
255 simu.initExternalViewer(300, 300);
256
257 // Inernal camera paramters initialization
258 vpCameraParameters cam(800, 800, 240, 180);
260
261 vpTime::wait(500);
262 // Load the scene
263
264 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
265 // environment variable value
266 std::string ipath = vpIoTools::getViSPImagesDataPath();
267 std::string filename = "./4points.iv";
268
269 // Set the default input path
270 if (!ipath.empty())
271 filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
272
273 std::cout << "Load : " << filename << std::endl << "This file should be in the working directory" << std::endl;
274
275 simu.load(filename.c_str());
276
277 // Run the main loop
278 simu.initApplication(&mainLoop);
279 // Run the simulator
280 simu.mainLoop();
281 return EXIT_SUCCESS;
282 } catch (const vpException &e) {
283 std::cout << "Catch an exception: " << e << std::endl;
284 return EXIT_FAILURE;
285 }
286}
287
288#else
289int main()
290{
291 std::cout << "You do not have X11, GTK, or OpenCV, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
292 std::cout << "Tip if you are on a unix-like system:" << std::endl;
293 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
294 std::cout << "Tip if you are on a windows-like system:" << std::endl;
295 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
296 return EXIT_SUCCESS;
297}
298#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
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, bool canMakeTheWindowGrow=true)
Definition: vpDot2.cpp:441
void setGraphics(bool activate)
Definition: vpDot2.h:314
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:253
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
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 setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ DESIRED
Definition: vpServo.h:186
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:100
unsigned int getInternalWidth() const
Definition: vpSimulator.h:174
void load(const char *file_name)
load an iv file
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
virtual void mainLoop()
activate the mainloop
void getCameraParameters(vpCameraParameters &cam)
get the intrinsic parameters of the camera
Definition: vpSimulator.h:293
void getInternalImage(vpImage< unsigned char > &I)
get an Image of the internal view
void initMainApplication()
perform some initialization in the main program thread
void initApplication(void *(*start_routine)(void *))
begin the main program
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
unsigned int getInternalHeight() const
Definition: vpSimulator.h:180
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void closeMainApplication()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()