Visual Servoing Platform version 3.5.0
photometricVisualServoingWithoutVpServo.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 * Authors:
32 * Eric Marchand
33 * Christophe Collewet
34 *
35 *****************************************************************************/
36
43#include <visp3/core/vpDebug.h>
44
45#include <visp3/core/vpImage.h>
46#include <visp3/core/vpImageTools.h>
47#include <visp3/io/vpImageIo.h>
48
49#include <visp3/core/vpCameraParameters.h>
50#include <visp3/core/vpTime.h>
51#include <visp3/robot/vpSimulatorCamera.h>
52
53#include <visp3/core/vpHomogeneousMatrix.h>
54#include <visp3/core/vpMath.h>
55#include <visp3/gui/vpDisplayD3D.h>
56#include <visp3/gui/vpDisplayGDI.h>
57#include <visp3/gui/vpDisplayGTK.h>
58#include <visp3/gui/vpDisplayOpenCV.h>
59#include <visp3/gui/vpDisplayX.h>
60
61#include <visp3/io/vpParseArgv.h>
62#include <visp3/visual_features/vpFeatureLuminance.h>
63
64#include <stdlib.h>
65#include <visp3/robot/vpImageSimulator.h>
66#define Z 1
67
68#include <visp3/core/vpIoTools.h>
69#include <visp3/io/vpParseArgv.h>
70
71// List of allowed command line options
72#define GETOPTARGS "cdi:n:h"
73
74void usage(const char *name, const char *badparam, std::string ipath, int niter);
75bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter);
76
87void usage(const char *name, const char *badparam, std::string ipath, int niter)
88{
89 fprintf(stdout, "\n\
90Tracking of Surf key-points.\n\
91\n\
92SYNOPSIS\n\
93 %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n",
94 name);
95
96 fprintf(stdout, "\n\
97OPTIONS: Default\n\
98 -i <input image path> %s\n\
99 Set image input path.\n\
100 From this path read \"doisneau/doisneau.jpg\"\n\
101 images. \n\
102 Setting the VISP_INPUT_IMAGE_PATH environment\n\
103 variable produces the same behaviour than using\n\
104 this option.\n\
105\n\
106 -c\n\
107 Disable the mouse click. Useful to automaze the \n\
108 execution of this program without humain intervention.\n\
109\n\
110 -d \n\
111 Turn off the display.\n\
112\n\
113 -n %%d %d\n\
114 Number of iterations.\n\
115\n\
116 -h\n\
117 Print the help.\n",
118 ipath.c_str(), niter);
119
120 if (badparam)
121 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
122}
137bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter)
138{
139 const char *optarg_;
140 int c;
141 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
142
143 switch (c) {
144 case 'c':
145 click_allowed = false;
146 break;
147 case 'd':
148 display = false;
149 break;
150 case 'i':
151 ipath = optarg_;
152 break;
153 case 'n':
154 niter = atoi(optarg_);
155 break;
156 case 'h':
157 usage(argv[0], NULL, ipath, niter);
158 return false;
159
160 default:
161 usage(argv[0], optarg_, ipath, niter);
162 return false;
163 }
164 }
165
166 if ((c == 1) || (c == -1)) {
167 // standalone param or error
168 usage(argv[0], NULL, ipath, niter);
169 std::cerr << "ERROR: " << std::endl;
170 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
171 return false;
172 }
173
174 return true;
175}
176
177int main(int argc, const char **argv)
178{
179#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
180 try {
181 std::string env_ipath;
182 std::string opt_ipath;
183 std::string ipath;
184 std::string filename;
185 bool opt_click_allowed = true;
186 bool opt_display = true;
187 int opt_niter = 400;
188
189 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
190 // environment variable value
192
193 // Set the default input path
194 if (!env_ipath.empty())
195 ipath = env_ipath;
196
197 // Read the command line options
198 if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) == false) {
199 return (-1);
200 }
201
202 // Get the option values
203 if (!opt_ipath.empty())
204 ipath = opt_ipath;
205
206 // Compare ipath and env_ipath. If they differ, we take into account
207 // the input path comming from the command line option
208 if (!opt_ipath.empty() && !env_ipath.empty()) {
209 if (ipath != env_ipath) {
210 std::cout << std::endl << "WARNING: " << std::endl;
211 std::cout << " Since -i <visp image path=" << ipath << "> "
212 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
213 << " we skip the environment variable." << std::endl;
214 }
215 }
216
217 // Test if an input path is set
218 if (opt_ipath.empty() && env_ipath.empty()) {
219 usage(argv[0], NULL, ipath, opt_niter);
220 std::cerr << std::endl << "ERROR:" << std::endl;
221 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
222 << " environment variable to specify the location of the " << std::endl
223 << " image path where test images are located." << std::endl
224 << std::endl;
225 exit(-1);
226 }
227
228 vpImage<unsigned char> Itexture;
229 filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
230 vpImageIo::read(Itexture, filename);
231
232 vpColVector X[4];
233 for (int i = 0; i < 4; i++)
234 X[i].resize(3);
235 // Top left corner
236 X[0][0] = -0.3;
237 X[0][1] = -0.215;
238 X[0][2] = 0;
239
240 // Top right corner
241 X[1][0] = 0.3;
242 X[1][1] = -0.215;
243 X[1][2] = 0;
244
245 // Bottom right corner
246 X[2][0] = 0.3;
247 X[2][1] = 0.215;
248 X[2][2] = 0;
249
250 // Bottom left corner
251 X[3][0] = -0.3;
252 X[3][1] = 0.215;
253 X[3][2] = 0;
254
256
258 sim.init(Itexture, X);
259
260 vpCameraParameters cam(870, 870, 160, 120);
261
262 // ----------------------------------------------------------
263 // Create the framegraber (here a simulated image)
264 vpImage<unsigned char> I(240, 320, 0);
266
267 // camera desired position
269 cdMo[2][3] = 1;
270
271 // set the robot at the desired position
272 sim.setCameraPosition(cdMo);
273 sim.getImage(I, cam); // and aquire the image Id
274 Id = I;
275
276// display the image
277#if defined VISP_HAVE_X11
278 vpDisplayX d;
279#elif defined VISP_HAVE_GDI
280 vpDisplayGDI d;
281#elif defined VISP_HAVE_GTK
282 vpDisplayGTK d;
283#elif defined VISP_HAVE_OPENCV
285#endif
286
287#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
288 if (opt_display) {
289 d.init(I, 20, 10, "Photometric visual servoing : s");
292 }
293 if (opt_display && opt_click_allowed) {
294 std::cout << "Click in the image to continue..." << std::endl;
296 }
297#endif
298
299 // ----------------------------------------------------------
300 // position the robot at the initial position
301 // ----------------------------------------------------------
302
303 // camera desired position
305 cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20));
306 vpHomogeneousMatrix wMo; // Set to identity
307 vpHomogeneousMatrix wMc; // Camera position in the world frame
308
309 // set the robot at the desired position
310 sim.setCameraPosition(cMo);
311 I = 0;
312 sim.getImage(I, cam); // and aquire the image Id
313
314#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
315 if (opt_display) {
318 }
319 if (opt_display && opt_click_allowed) {
320 std::cout << "Click in the image to continue..." << std::endl;
322 }
323#endif
324
326 Idiff = I;
327
328 vpImageTools::imageDifference(I, Id, Idiff);
329
330// Affiche de l'image de difference
331#if defined VISP_HAVE_X11
332 vpDisplayX d1;
333#elif defined VISP_HAVE_GDI
334 vpDisplayGDI d1;
335#elif defined VISP_HAVE_GTK
336 vpDisplayGTK d1;
337#endif
338#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
339 if (opt_display) {
340 d1.init(Idiff, 40 + static_cast<int>(I.getWidth()), 10, "photometric visual servoing : s-s* ");
341 vpDisplay::display(Idiff);
342 vpDisplay::flush(Idiff);
343 }
344#endif
345 // create the robot (here a simulated free flying camera)
346 vpSimulatorCamera robot;
347 robot.setSamplingTime(0.04);
348 wMc = wMo * cMo.inverse();
349 robot.setPosition(wMc);
350
351 // ------------------------------------------------------
352 // Visual feature, interaction matrix, error
353 // s, Ls, Lsd, Lt, Lp, etc
354 // ------------------------------------------------------
355
356 // current visual feature built from the image
357 // (actually, this is the image...)
359 sI.init(I.getHeight(), I.getWidth(), Z);
360 sI.setCameraParameters(cam);
361 sI.buildFrom(I);
362
363 // desired visual feature built from the image
365 sId.init(I.getHeight(), I.getWidth(), Z);
366 sId.setCameraParameters(cam);
367 sId.buildFrom(Id);
368
369 // Matrice d'interaction, Hessien, erreur,...
370 vpMatrix Lsd; // matrice d'interaction a la position desiree
371 vpMatrix Hsd; // hessien a la position desiree
372 vpMatrix H; // Hessien utilise pour le levenberg-Marquartd
373 vpColVector error; // Erreur I-I*
374
375 // Compute the interaction matrix
376 // link the variation of image intensity to camera motion
377
378 // here it is computed at the desired position
379 sId.interaction(Lsd);
380
381 // Compute the Hessian H = L^TL
382 Hsd = Lsd.AtA();
383
384 // Compute the Hessian diagonal for the Levenberg-Marquartd
385 // optimization process
386 unsigned int n = 6;
387 vpMatrix diagHsd(n, n);
388 diagHsd.eye(n);
389 for (unsigned int i = 0; i < n; i++)
390 diagHsd[i][i] = Hsd[i][i];
391
392 // ------------------------------------------------------
393 // Control law
394 double lambda; // gain
395 vpColVector e;
396 vpColVector v; // camera velocity send to the robot
397
398 // ----------------------------------------------------------
399 // Minimisation
400
401 double mu; // mu = 0 : Gauss Newton ; mu != 0 : LM
402 double lambdaGN;
403
404 mu = 0.01;
405 lambda = 30;
406 lambdaGN = 30;
407
408 // set a velocity control mode
410
411 // ----------------------------------------------------------
412 int iter = 1;
413 int iterGN = 90; // swicth to Gauss Newton after iterGN iterations
414
415 double normeError = 0;
416
417 vpChrono chrono;
418 chrono.start();
419 do {
420 std::cout << "--------------------------------------------" << iter++ << std::endl;
421
422 // Acquire the new image
423 sim.setCameraPosition(cMo);
424 sim.getImage(I, cam);
425#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
426 if (opt_display) {
429 }
430#endif
431 vpImageTools::imageDifference(I, Id, Idiff);
432#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
433 if (opt_display) {
434 vpDisplay::display(Idiff);
435 vpDisplay::flush(Idiff);
436 }
437#endif
438 // Compute current visual feature
439 sI.buildFrom(I);
440
441 // compute current error
442 sI.error(sId, error);
443
444 normeError = (error.sumSquare());
445 std::cout << "|e| " << normeError << std::endl;
446
447 // double t = vpTime::measureTimeMs() ;
448
449 // ---------- Levenberg Marquardt method --------------
450 {
451 if (iter > iterGN) {
452 mu = 0.0001;
453 lambda = lambdaGN;
454 }
455
456 // Compute the levenberg Marquartd term
457 {
458 H = ((mu * diagHsd) + Hsd).inverseByLU();
459 }
460 // compute the control law
461 e = H * Lsd.t() * error;
462
463 v = -lambda * e;
464 }
465
466 std::cout << "lambda = " << lambda << " mu = " << mu;
467 std::cout << " |Tc| = " << sqrt(v.sumSquare()) << std::endl;
468
469 // send the robot velocity
471 wMc = robot.getPosition();
472 cMo = wMc.inverse() * wMo;
473 } while (normeError > 10000 && iter < opt_niter);
474
475 chrono.stop();
476 std::cout << "Time to convergence: " << chrono.getDurationMs() << " ms" << std::endl;
477
478 v = 0;
480
481 return EXIT_SUCCESS;
482 } catch (const vpException &e) {
483 std::cout << "Catch an exception: " << e << std::endl;
484 return EXIT_FAILURE;
485 }
486#else
487 (void)argc;
488 (void)argv;
489 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
490 return EXIT_SUCCESS;
491#endif
492}
Generic class defining intrinsic camera parameters.
void start(bool reset=true)
Definition: vpTime.cpp:409
void stop()
Definition: vpTime.cpp:424
double getDurationMs()
Definition: vpTime.cpp:392
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
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
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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)
error that can be emited by ViSP classes.
Definition: vpException.h:72
Class that defines the image luminance visual feature.
void setCameraParameters(vpCameraParameters &_cam)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void buildFrom(vpImage< unsigned char > &I)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
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
vpMatrix inverseByLU() const
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Class that defines the simplest robot: a free flying camera.