Visual Servoing Platform version 3.5.0
servoSimu4Points.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 * Demonstration of the wireframe simulator with a simple visual servoing
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
45#include <stdlib.h>
46
47#include <visp3/core/vpCameraParameters.h>
48#include <visp3/core/vpHomogeneousMatrix.h>
49#include <visp3/core/vpImage.h>
50#include <visp3/core/vpIoTools.h>
51#include <visp3/core/vpMath.h>
52#include <visp3/core/vpTime.h>
53#include <visp3/core/vpVelocityTwistMatrix.h>
54#include <visp3/gui/vpDisplayD3D.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayGTK.h>
57#include <visp3/gui/vpDisplayOpenCV.h>
58#include <visp3/gui/vpDisplayX.h>
59#include <visp3/gui/vpPlot.h>
60#include <visp3/io/vpImageIo.h>
61#include <visp3/io/vpParseArgv.h>
62#include <visp3/robot/vpSimulatorCamera.h>
63#include <visp3/robot/vpWireFrameSimulator.h>
64#include <visp3/visual_features/vpFeatureBuilder.h>
65#include <visp3/visual_features/vpFeaturePoint.h>
66#include <visp3/vs/vpServo.h>
67
68#define GETOPTARGS "dhp"
69
70#if defined(VISP_HAVE_DISPLAY) \
71 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
72
81void usage(const char *name, const char *badparam)
82{
83 fprintf(stdout, "\n\
84Demonstration of the wireframe simulator with a simple visual servoing.\n\
85 \n\
86The visual servoing consists in bringing the camera at a desired \n\
87position from the object.\n\
88 \n\
89The visual features used to compute the pose of the camera and \n\
90thus the control law are four points.\n\
91 \n\
92This demonstration explains also how to move the object around a world\n\
93reference frame. Here, the movement is a rotation around the x and y axis\n\
94at a given distance from the world frame. In fact the object trajectory\n\
95is on a sphere whose center is the origin of the world frame.\n\
96 \n\
97SYNOPSIS\n\
98 %s [-d] [-p] [-h]\n", name);
99
100 fprintf(stdout, "\n\
101OPTIONS: Default\n\
102 -d \n\
103 Turn off the display.\n\
104 \n\
105 -p \n\
106 Turn off the plotter.\n\
107 \n\
108 -h\n\
109 Print the help.\n");
110
111 if (badparam)
112 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
113}
114
127bool getOptions(int argc, const char **argv, bool &display, bool &plot)
128{
129 const char *optarg_;
130 int c;
131 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
132
133 switch (c) {
134 case 'd':
135 display = false;
136 break;
137 case 'p':
138 plot = false;
139 break;
140 case 'h':
141 usage(argv[0], NULL);
142 return false;
143
144 default:
145 usage(argv[0], optarg_);
146 return false;
147 }
148 }
149
150 if ((c == 1) || (c == -1)) {
151 // standalone param or error
152 usage(argv[0], NULL);
153 std::cerr << "ERROR: " << std::endl;
154 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
155 return false;
156 }
157
158 return true;
159}
160
161int main(int argc, const char **argv)
162{
163 try {
164 bool opt_display = true;
165 bool opt_plot = true;
166 std::string filename = vpIoTools::getParent(argv[0]) + "/mire.png";
167 std::cout << "Read " << filename << std::endl;
168
169 // Read the command line options
170 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
171 exit(-1);
172 }
173
174 vpImage<vpRGBa> Iint(480, 640, 255);
175 vpImage<vpRGBa> Iext1(480, 640, 255);
176 vpImage<vpRGBa> Iext2(480, 640, 255);
177
178#if defined VISP_HAVE_X11
179 vpDisplayX display[3];
180#elif defined VISP_HAVE_OPENCV
181 vpDisplayOpenCV display[3];
182#elif defined VISP_HAVE_GDI
183 vpDisplayGDI display[3];
184#elif defined VISP_HAVE_D3D9
185 vpDisplayD3D display[3];
186#elif defined VISP_HAVE_GTK
187 vpDisplayGTK display[3];
188#endif
189
190 if (opt_display) {
191 // Display size is automatically defined by the image (I) size
192 display[0].init(Iint, 100, 100, "The internal view");
193 display[1].init(Iext1, 100, 100, "The first external view");
194 display[2].init(Iext2, 100, 100, "The second external view");
196 vpDisplay::setWindowPosition(Iext1, 750, 0);
197 vpDisplay::setWindowPosition(Iext2, 0, 550);
198 vpDisplay::display(Iint);
199 vpDisplay::flush(Iint);
200 vpDisplay::display(Iext1);
201 vpDisplay::flush(Iext1);
202 vpDisplay::display(Iext2);
203 vpDisplay::flush(Iext2);
204 }
205
206 vpPlot *plotter = NULL;
207
208 if (opt_plot) {
209 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
210 plotter->setTitle(0, "Visual features error");
211 plotter->setTitle(1, "Camera velocities");
212 plotter->initGraph(0, 8);
213 plotter->initGraph(1, 6);
214 plotter->setLegend(0, 0, "error_feat_p1_x");
215 plotter->setLegend(0, 1, "error_feat_p1_y");
216 plotter->setLegend(0, 2, "error_feat_p2_x");
217 plotter->setLegend(0, 3, "error_feat_p2_y");
218 plotter->setLegend(0, 4, "error_feat_p3_x");
219 plotter->setLegend(0, 5, "error_feat_p3_y");
220 plotter->setLegend(0, 6, "error_feat_p4_x");
221 plotter->setLegend(0, 7, "error_feat_p4_y");
222 plotter->setLegend(1, 0, "vc_x");
223 plotter->setLegend(1, 1, "vc_y");
224 plotter->setLegend(1, 2, "vc_z");
225 plotter->setLegend(1, 3, "wc_x");
226 plotter->setLegend(1, 4, "wc_y");
227 plotter->setLegend(1, 5, "wc_z");
228 }
229
230 vpServo task;
231 vpSimulatorCamera robot;
232 float sampling_time = 0.020f; // Sampling period in second
233 robot.setSamplingTime(sampling_time);
234
235 // Since the task gain lambda is very high, we need to increase default
236 // max velocities
239
240 // Set initial position of the object in the camera frame
241 vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
242 // Set desired position of the object in the camera frame
243 vpHomogeneousMatrix cdMo(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
244 // Set initial position of the object in the world frame
245 vpHomogeneousMatrix wMo(0.0, 0.0, 0.2, 0, 0, 0);
246 // Position of the camera in the world frame
248 wMc = wMo * cMo.inverse();
249
250 // The four point used as visual features
251 vpPoint point[4];
252 point[0].setWorldCoordinates(-0.1, -0.1, 0);
253 point[3].setWorldCoordinates(-0.1, 0.1, 0);
254 point[2].setWorldCoordinates(0.1, 0.1, 0);
255 point[1].setWorldCoordinates(0.1, -0.1, 0);
256
257 // Projection of the points
258 for (int i = 0; i < 4; i++)
259 point[i].track(cMo);
260
261 // Set the current visual feature
262 vpFeaturePoint p[4];
263 for (int i = 0; i < 4; i++)
264 vpFeatureBuilder::create(p[i], point[i]);
265
266 // Projection of the points
267 for (int i = 0; i < 4; i++)
268 point[i].track(cdMo);
269
270 vpFeaturePoint pd[4];
271 for (int i = 0; i < 4; i++)
272 vpFeatureBuilder::create(pd[i], point[i]);
273
276
277 vpHomogeneousMatrix cMe; // Identity
278 vpVelocityTwistMatrix cVe(cMe);
279 task.set_cVe(cVe);
280
281 vpMatrix eJe;
282 robot.get_eJe(eJe);
283 task.set_eJe(eJe);
284
285 for (int i = 0; i < 4; i++)
286 task.addFeature(p[i], pd[i]);
287
288 task.setLambda(10);
289
290 std::list<vpImageSimulator> list;
291 vpImageSimulator imsim;
292
293 vpColVector X[4];
294 for (int i = 0; i < 4; i++)
295 X[i].resize(3);
296 X[0][0] = -0.2;
297 X[0][1] = -0.2;
298 X[0][2] = 0;
299
300 X[1][0] = 0.2;
301 X[1][1] = -0.2;
302 X[1][2] = 0;
303
304 X[2][0] = 0.2;
305 X[2][1] = 0.2;
306 X[2][2] = 0;
307
308 X[3][0] = -0.2;
309 X[3][1] = 0.2;
310 X[3][2] = 0;
311
312 imsim.init(filename.c_str(), X);
313
314 list.push_back(imsim);
315
317
318 // Set the scene
320
321 // Initialize simulator frames
322 sim.set_fMo(wMo); // Position of the object in the world reference frame
323 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
324 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
325
326 // Set the External camera position
327 vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0));
328 sim.setExternalCameraPosition(camMf);
329
330 // Computes the position of a camera which is fixed in the object frame
331 vpHomogeneousMatrix camoMf(0, 0.0, 1.5, 0, vpMath::rad(140), 0);
332 camoMf = camoMf * (sim.get_fMo().inverse());
333
334 // Set the parameters of the cameras (internal and external)
335 vpCameraParameters camera(1000, 1000, 320, 240);
336 sim.setInternalCameraParameters(camera);
337 sim.setExternalCameraParameters(camera);
338
339 int max_iter = 10;
340
341 if (opt_display) {
342 max_iter = 2500;
343
344 // Get the internal and external views
345 sim.getInternalImage(Iint);
346 sim.getExternalImage(Iext1);
347 sim.getExternalImage(Iext2, camoMf);
348
349 // Display the object frame (current and desired position)
350 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
351 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
352
353 // Display the object frame the world reference frame and the camera
354 // frame
355 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
356 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
357 vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
358
359 // Display the world reference frame and the object frame
360 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
361 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
362
363 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
364
365 vpDisplay::flush(Iint);
366 vpDisplay::flush(Iext1);
367 vpDisplay::flush(Iext2);
368
369 std::cout << "Click on a display" << std::endl;
370
371 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
372 !vpDisplay::getClick(Iext2, false)) {
373 };
374 }
375
376 robot.setPosition(wMc);
377 // Print the task
378 task.print();
379
380 int iter = 0;
381 bool stop = false;
382 vpColVector v;
383
384 double t_prev, t = vpTime::measureTimeMs();
385
386 while (iter++ < max_iter && !stop) {
387 t_prev = t;
389
390 if (opt_display) {
391 vpDisplay::display(Iint);
392 vpDisplay::display(Iext1);
393 vpDisplay::display(Iext2);
394 }
395
396 robot.get_eJe(eJe);
397 task.set_eJe(eJe);
398
399 wMc = robot.getPosition();
400 cMo = wMc.inverse() * wMo;
401 for (int i = 0; i < 4; i++) {
402 point[i].track(cMo);
403 vpFeatureBuilder::create(p[i], point[i]);
404 }
405
406 v = task.computeControlLaw();
408
409 // Compute the movement of the object around the world reference frame.
410 vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
411 vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5 * iter), 0, 0);
412 vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5 * iter), 0);
413
414 // Move the object in the world frame
415 wMo = b * c * a;
416
417 sim.set_fMo(wMo); // Move the object in the simulator
419
420 // Compute the position of the external view which is fixed in the
421 // object frame
422 camoMf.buildFrom(0, 0.0, 1.5, 0, vpMath::rad(150), 0);
423 camoMf = camoMf * (sim.get_fMo().inverse());
424
425 if (opt_plot) {
426 plotter->plot(0, iter, task.getError());
427 plotter->plot(1, iter, v);
428 }
429
430 if (opt_display) {
431 // Get the internal and external views
432 sim.getInternalImage(Iint);
433 sim.getExternalImage(Iext1);
434 sim.getExternalImage(Iext2, camoMf);
435
436 // Display the object frame (current and desired position)
437 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
438 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
439
440 // Display the camera frame, the object frame the world reference
441 // frame
442 vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
446
447 // Display the world reference frame and the object frame
448 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
449 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
450
451 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
452
453 std::stringstream ss;
454 ss << "Loop time: " << t - t_prev << " ms";
455 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
456
457 if (vpDisplay::getClick(Iint, false)) {
458 stop = true;
459 }
460 vpDisplay::flush(Iint);
461 vpDisplay::flush(Iext1);
462 vpDisplay::flush(Iext2);
463
464 vpTime::wait(t, sampling_time * 1000); // Wait ms
465 }
466
467 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
468 }
469
470 if (opt_plot && plotter != NULL) {
471 vpDisplay::display(Iint);
472 sim.getInternalImage(Iint);
473 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
474 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
475 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
476 if (vpDisplay::getClick(Iint)) {
477 stop = true;
478 }
479 vpDisplay::flush(Iint);
480
481 delete plotter;
482 }
483
484 task.print();
485
486 return EXIT_SUCCESS;
487 } catch (const vpException &e) {
488 std::cout << "Catch an exception: " << e << std::endl;
489 return EXIT_FAILURE;
490 }
491}
492#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
493int main()
494{
495 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
496 return EXIT_SUCCESS;
497}
498#else
499int main()
500{
501 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
502 std::cout << "Tip if you are on a unix-like system:" << std::endl;
503 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
504 std::cout << "Tip if you are on a windows-like system:" << std::endl;
505 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
506 return EXIT_SUCCESS;
507}
508
509#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
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
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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...
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void init(const vpImage< unsigned char > &I, vpColVector *X)
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1606
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
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)
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
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 getError() const
Definition: vpServo.h:278
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 wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()