Visual Servoing Platform version 3.5.0
servoSimuSphere.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 <cmath> // std::fabs
46#include <limits> // numeric_limits
47#include <stdlib.h>
48
49#include <visp3/core/vpCameraParameters.h>
50#include <visp3/core/vpHomogeneousMatrix.h>
51#include <visp3/core/vpImage.h>
52#include <visp3/core/vpIoTools.h>
53#include <visp3/core/vpMath.h>
54#include <visp3/core/vpSphere.h>
55#include <visp3/core/vpTime.h>
56#include <visp3/core/vpVelocityTwistMatrix.h>
57#include <visp3/gui/vpDisplayD3D.h>
58#include <visp3/gui/vpDisplayGDI.h>
59#include <visp3/gui/vpDisplayGTK.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayX.h>
62#include <visp3/gui/vpPlot.h>
63#include <visp3/io/vpImageIo.h>
64#include <visp3/io/vpParseArgv.h>
65#include <visp3/robot/vpSimulatorCamera.h>
66#include <visp3/robot/vpWireFrameSimulator.h>
67#include <visp3/visual_features/vpFeatureBuilder.h>
68#include <visp3/visual_features/vpGenericFeature.h>
69#include <visp3/vs/vpServo.h>
70
71#define GETOPTARGS "dhp"
72
73#if defined(VISP_HAVE_DISPLAY) \
74 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
75
84void usage(const char *name, const char *badparam)
85{
86 fprintf(stdout, "\n\
87Demonstration of the wireframe simulator with a simple visual servoing.\n\
88 \n\
89The visual servoing consists in bringing the camera at a desired position from the object.\n\
90 \n\
91The visual features used to compute the pose of the camera and thus the control law are special moments computed with the sphere's parameters.\n\
92 \n\
93SYNOPSIS\n\
94 %s [-d] [-p] [-h]\n", name);
95
96 fprintf(stdout, "\n\
97OPTIONS: Default\n\
98 -d \n\
99 Turn off the display.\n\
100 \n\
101 -p \n\
102 Turn off the plotter.\n\
103 \n\
104 -h\n\
105 Print the help.\n");
106
107 if (badparam)
108 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
109}
110
123bool getOptions(int argc, const char **argv, bool &display, bool &plot)
124{
125 const char *optarg_;
126 int c;
127 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
128
129 switch (c) {
130 case 'd':
131 display = false;
132 break;
133 case 'p':
134 plot = false;
135 break;
136 case 'h':
137 usage(argv[0], NULL);
138 return false;
139
140 default:
141 usage(argv[0], optarg_);
142 return false;
143 }
144 }
145
146 if ((c == 1) || (c == -1)) {
147 // standalone param or error
148 usage(argv[0], NULL);
149 std::cerr << "ERROR: " << std::endl;
150 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
151 return false;
152 }
153
154 return true;
155}
156
157/*
158 Computes the virtual visual features corresponding to the sphere and stores
159 it in the generic feature.
160
161 The visual feature vector is computed thanks to the following formula : s =
162 {sx, sy, sz} sx = gx*h2/(sqrt(h2+1) sx = gy*h2/(sqrt(h2+1) sz = sqrt(h2+1)
163
164 with gx and gy the center of gravity of the ellipse,
165 with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
166 with n20,n02,n11 the second order centered moments of the sphere normalized by its area
167 (i.e., such that \f$n_{ij} = \mu_{ij}/a\f$ where \f$\mu_{ij}\f$ are the centered moments and a the area)
168*/
169void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s)
170{
171 double gx = sphere.get_x();
172 double gy = sphere.get_y();
173 double n02 = sphere.get_n02();
174 double n20 = sphere.get_n20();
175 double n11 = sphere.get_n11();
176 double h2;
177 // if (gx != 0 || gy != 0)
178 if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
179 h2 = (vpMath::sqr(gx) + vpMath::sqr(gy)) /
180 (4 * n20 * vpMath::sqr(gy) + 4 * n02 * vpMath::sqr(gx) - 8 * n11 * gx * gy);
181 else
182 h2 = 1 / (4 * n20);
183
184 double sx = gx * h2 / (sqrt(h2 + 1));
185 double sy = gy * h2 / (sqrt(h2 + 1));
186 double sz = sqrt(h2 + 1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2));
187
188 s.set_s(sx, sy, sz);
189}
190
191/*
192 Computes the interaction matrix such as L = [-1/R*I3 [s]x]
193
194 with R the radius of the sphere
195 with I3 the 3x3 identity matrix
196 with [s]x the skew matrix related to s
197*/
198void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L)
199{
200 L = 0;
201 L[0][0] = -1 / sphere.getR();
202 L[1][1] = -1 / sphere.getR();
203 L[2][2] = -1 / sphere.getR();
204
205 double s0, s1, s2;
206 s.get_s(s0, s1, s2);
207
208 vpTranslationVector c(s0, s1, s2);
209 vpMatrix sk;
210 sk = c.skew();
211
212 for (unsigned int i = 0; i < 3; i++)
213 for (unsigned int j = 0; j < 3; j++)
214 L[i][j + 3] = sk[i][j];
215}
216
217int main(int argc, const char **argv)
218{
219 try {
220 bool opt_display = true;
221 bool opt_plot = true;
222
223 // Read the command line options
224 if (getOptions(argc, argv, opt_display, opt_plot) == false) {
225 exit(-1);
226 }
227
228 vpImage<vpRGBa> Iint(480, 640, 255);
229 vpImage<vpRGBa> Iext1(480, 640, 255);
230 vpImage<vpRGBa> Iext2(480, 640, 255);
231
232#if defined VISP_HAVE_X11
233 vpDisplayX display[3];
234#elif defined VISP_HAVE_OPENCV
235 vpDisplayOpenCV display[3];
236#elif defined VISP_HAVE_GDI
237 vpDisplayGDI display[3];
238#elif defined VISP_HAVE_D3D9
239 vpDisplayD3D display[3];
240#elif defined VISP_HAVE_GTK
241 vpDisplayGTK display[3];
242#endif
243
244 if (opt_display) {
245 // Display size is automatically defined by the image (I) size
246 display[0].init(Iint, 100, 100, "The internal view");
247 display[1].init(Iext1, 100, 100, "The first external view");
248 display[2].init(Iext2, 100, 100, "The second external view");
250 vpDisplay::setWindowPosition(Iext1, 750, 0);
251 vpDisplay::setWindowPosition(Iext2, 0, 550);
252 vpDisplay::display(Iint);
253 vpDisplay::flush(Iint);
254 vpDisplay::display(Iext1);
255 vpDisplay::flush(Iext1);
256 vpDisplay::display(Iext2);
257 vpDisplay::flush(Iext2);
258 }
259
260 vpPlot *plotter = NULL;
261
262 vpServo task;
263 vpSimulatorCamera robot;
264 float sampling_time = 0.020f; // Sampling period in second
265 robot.setSamplingTime(sampling_time);
266
267 // Since the task gain lambda is very high, we need to increase default
268 // max velocities
271
272 // Set initial position of the object in the camera frame
273 vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
274 // Set desired position of the object in the camera frame
275 vpHomogeneousMatrix cdMo(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
276 // Set initial position of the object in the world frame
277 vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
278 // Position of the camera in the world frame
280 wMc = wMo * cMo.inverse();
281
282 robot.setPosition(wMc);
283
284 // The sphere
285 vpSphere sphere(0, 0, 0, 0.15);
286
287 // Projection of the sphere
288 sphere.track(cMo);
289
290 // Set the current visual feature
291 vpGenericFeature s(3);
292 computeVisualFeatures(sphere, s);
293
294 // Projection of the points
295 sphere.track(cdMo);
296
297 vpGenericFeature sd(3);
298 computeVisualFeatures(sphere, sd);
299
300 vpMatrix L(3, 6);
301 computeInteractionMatrix(sd, sphere, L);
302 sd.setInteractionMatrix(L);
303
306
308 vpVelocityTwistMatrix cVe(cMe);
309 task.set_cVe(cVe);
310
311 vpMatrix eJe;
312 robot.get_eJe(eJe);
313 task.set_eJe(eJe);
314
315 task.addFeature(s, sd);
316
317 task.setLambda(7);
318
319 if (opt_plot) {
320 plotter = new vpPlot(2, 480, 640, 750, 550, "Real time curves plotter");
321 plotter->setTitle(0, "Visual features error");
322 plotter->setTitle(1, "Camera velocities");
323 plotter->initGraph(0, task.getDimension());
324 plotter->initGraph(1, 6);
325 plotter->setLegend(0, 0, "error_feat_sx");
326 plotter->setLegend(0, 1, "error_feat_sy");
327 plotter->setLegend(0, 2, "error_feat_sz");
328 plotter->setLegend(1, 0, "vc_x");
329 plotter->setLegend(1, 1, "vc_y");
330 plotter->setLegend(1, 2, "vc_z");
331 plotter->setLegend(1, 3, "wc_x");
332 plotter->setLegend(1, 4, "wc_y");
333 plotter->setLegend(1, 5, "wc_z");
334 }
335
337
338 // Set the scene
340
341 // Initialize simulator frames
342 sim.set_fMo(wMo); // Position of the object in the world reference frame
343 sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
344 sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
345
346 // Set the External camera position
347 vpHomogeneousMatrix camMf(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0);
348 sim.setExternalCameraPosition(camMf);
349
350 // Computes the position of a camera which is fixed in the object frame
351 vpHomogeneousMatrix camoMf(0, 0.0, 2.5, 0, vpMath::rad(140), 0);
352 camoMf = camoMf * (sim.get_fMo().inverse());
353
354 // Set the parameters of the cameras (internal and external)
355 vpCameraParameters camera(1000, 1000, 320, 240);
356 sim.setInternalCameraParameters(camera);
357 sim.setExternalCameraParameters(camera);
358
359 int max_iter = 10;
360
361 if (opt_display) {
362 max_iter = 1000;
363 // Get the internal and external views
364 sim.getInternalImage(Iint);
365 sim.getExternalImage(Iext1);
366 sim.getExternalImage(Iext2, camoMf);
367
368 // Display the object frame (current and desired position)
369 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
370 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
371
372 // Display the object frame the world reference frame and the camera
373 // frame
374 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
375 vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
376 vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
377
378 // Display the world reference frame and the object frame
379 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
380 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
381
382 vpDisplay::displayText(Iint, 20, 20, "Click to start visual servo", vpColor::red);
383
384 vpDisplay::flush(Iint);
385 vpDisplay::flush(Iext1);
386 vpDisplay::flush(Iext2);
387
388 std::cout << "Click on a display" << std::endl;
389 while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
390 !vpDisplay::getClick(Iext2, false)) {
391 };
392 }
393
394 // Print the task
395 task.print();
396
397 int iter = 0;
398 bool stop = false;
399 vpColVector v;
400
401 double t_prev, t = vpTime::measureTimeMs();
402
403 while (iter++ < max_iter && !stop) {
404 t_prev = t;
406
407 if (opt_display) {
408 vpDisplay::display(Iint);
409 vpDisplay::display(Iext1);
410 vpDisplay::display(Iext2);
411 }
412
413 robot.get_eJe(eJe);
414 task.set_eJe(eJe);
415
416 wMc = robot.getPosition();
417 cMo = wMc.inverse() * wMo;
418
419 sphere.track(cMo);
420
421 // Set the current visual feature
422 computeVisualFeatures(sphere, s);
423
424 v = task.computeControlLaw();
427
428 // Compute the position of the external view which is fixed in the
429 // object frame
430 camoMf.buildFrom(0, 0.0, 2.5, 0, vpMath::rad(150), 0);
431 camoMf = camoMf * (sim.get_fMo().inverse());
432
433 if (opt_plot) {
434 plotter->plot(0, iter, task.getError());
435 plotter->plot(1, iter, v);
436 }
437
438 if (opt_display) {
439 // Get the internal and external views
440 sim.getInternalImage(Iint);
441 sim.getExternalImage(Iext1);
442 sim.getExternalImage(Iext2, camoMf);
443
444 // Display the object frame (current and desired position)
445 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
446 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
447
448 // Display the camera frame, the object frame the world reference
449 // frame
450 vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
454
455 // Display the world reference frame and the object frame
456 vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
457 vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
458
459 vpDisplay::displayText(Iint, 20, 20, "Click to stop visual servo", vpColor::red);
460
461 std::stringstream ss;
462 ss << "Loop time: " << t - t_prev << " ms";
463 vpDisplay::displayText(Iint, 40, 20, ss.str(), vpColor::red);
464
465 if (vpDisplay::getClick(Iint, false)) {
466 stop = true;
467 }
468
469 vpDisplay::flush(Iint);
470 vpDisplay::flush(Iext1);
471 vpDisplay::flush(Iext2);
472
473 vpTime::wait(t, sampling_time * 1000); // Wait ms
474 }
475
476 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
477 }
478
479 if (opt_plot && plotter != NULL) {
480 vpDisplay::display(Iint);
481 sim.getInternalImage(Iint);
482 vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
483 vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
484 vpDisplay::displayText(Iint, 20, 20, "Click to quit", vpColor::red);
485 if (vpDisplay::getClick(Iint)) {
486 stop = true;
487 }
488 vpDisplay::flush(Iint);
489
490 delete plotter;
491 }
492
493 task.print();
494 return EXIT_SUCCESS;
495 } catch (const vpException &e) {
496 std::cout << "Catch an exception: " << e << std::endl;
497 return EXIT_FAILURE;
498 }
499}
500#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
501int main()
502{
503 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
504 return EXIT_SUCCESS;
505}
506#else
507int main()
508{
509 std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
510 std::cout << "Tip if you are on a unix-like system:" << std::endl;
511 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
512 std::cout << "Tip if you are on a windows-like system:" << std::endl;
513 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
514 return EXIT_SUCCESS;
515}
516#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
void track(const vpHomogeneousMatrix &cMo)
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
void get_s(vpColVector &s) const
get the value of all the features.
void set_s(const vpColVector &s)
set the value of all the features.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition: vpMath.h:110
static double sqr(double x)
Definition: vpMath.h:116
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
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
unsigned int getDimension() const
Return the task dimension.
Definition: vpServo.cpp:553
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.
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:83
double get_n02() const
Definition: vpSphere.h:109
double get_y() const
Definition: vpSphere.h:105
double get_n11() const
Definition: vpSphere.h:108
double get_x() const
Definition: vpSphere.h:104
double getR() const
Definition: vpSphere.h:114
double get_n20() const
Definition: vpSphere.h:107
Class that consider the case of a translation vector.
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)
@ SPHERE
A 15cm radius sphere.
void getExternalImage(vpImage< unsigned char > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()