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