Visual Servoing Platform version 3.5.0
photometricVisualServoing.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/vpImage.h>
44#include <visp3/core/vpImageTools.h>
45#include <visp3/io/vpImageIo.h>
46
47#include <visp3/core/vpCameraParameters.h>
48#include <visp3/core/vpTime.h>
49#include <visp3/robot/vpSimulatorCamera.h>
50
51#include <visp3/core/vpHomogeneousMatrix.h>
52#include <visp3/core/vpMath.h>
53#include <visp3/gui/vpDisplayD3D.h>
54#include <visp3/gui/vpDisplayGDI.h>
55#include <visp3/gui/vpDisplayGTK.h>
56#include <visp3/gui/vpDisplayOpenCV.h>
57#include <visp3/gui/vpDisplayX.h>
58
59#include <visp3/io/vpParseArgv.h>
60#include <visp3/visual_features/vpFeatureLuminance.h>
61#include <visp3/vs/vpServo.h>
62
63#include <stdlib.h>
64#include <visp3/robot/vpImageSimulator.h>
65#define Z 1
66
67#include <visp3/core/vpIoTools.h>
68#include <visp3/io/vpParseArgv.h>
69
70// List of allowed command line options
71#define GETOPTARGS "cdi:n:h"
72
73void usage(const char *name, const char *badparam, std::string ipath, int niter);
74bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter);
75
86void usage(const char *name, const char *badparam, std::string ipath, int niter)
87{
88 fprintf(stdout, "\n\
89Tracking of Surf key-points.\n\
90\n\
91SYNOPSIS\n\
92 %s [-i <input image path>] [-c] [-d] [-n <number of iterations>] [-h]\n",
93 name);
94
95 fprintf(stdout, "\n\
96OPTIONS: Default\n\
97 -i <input image path> %s\n\
98 Set image input path.\n\
99 From this path read \"doisneau/doisneau.jpg\"\n\
100 images. \n\
101 Setting the VISP_INPUT_IMAGE_PATH environment\n\
102 variable produces the same behaviour than using\n\
103 this option.\n\
104\n\
105 -c\n\
106 Disable the mouse click. Useful to automaze the \n\
107 execution of this program without humain intervention.\n\
108\n\
109 -d \n\
110 Turn off the display.\n\
111\n\
112 -n %%d %d\n\
113 Number of iterations.\n\
114\n\
115 -h\n\
116 Print the help.\n",
117 ipath.c_str(), niter);
118
119 if (badparam)
120 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
121}
136bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter)
137{
138 const char *optarg_;
139 int c;
140 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
141
142 switch (c) {
143 case 'c':
144 click_allowed = false;
145 break;
146 case 'd':
147 display = false;
148 break;
149 case 'i':
150 ipath = optarg_;
151 break;
152 case 'n':
153 niter = atoi(optarg_);
154 break;
155 case 'h':
156 usage(argv[0], NULL, ipath, niter);
157 return false;
158
159 default:
160 usage(argv[0], optarg_, ipath, niter);
161 return false;
162 }
163 }
164
165 if ((c == 1) || (c == -1)) {
166 // standalone param or error
167 usage(argv[0], NULL, ipath, niter);
168 std::cerr << "ERROR: " << std::endl;
169 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
170 return false;
171 }
172
173 return true;
174}
175
176int main(int argc, const char **argv)
177{
178#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
179 try {
180 std::string env_ipath;
181 std::string opt_ipath;
182 std::string ipath;
183 std::string filename;
184 bool opt_click_allowed = true;
185 bool opt_display = true;
186 int opt_niter = 400;
187
188 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
189 // environment variable value
191
192 // Set the default input path
193 if (!env_ipath.empty())
194 ipath = env_ipath;
195
196 // Read the command line options
197 if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) == false) {
198 return (-1);
199 }
200
201 // Get the option values
202 if (!opt_ipath.empty())
203 ipath = opt_ipath;
204
205 // Compare ipath and env_ipath. If they differ, we take into account
206 // the input path comming from the command line option
207 if (!opt_ipath.empty() && !env_ipath.empty()) {
208 if (ipath != env_ipath) {
209 std::cout << std::endl << "WARNING: " << std::endl;
210 std::cout << " Since -i <visp image path=" << ipath << "> "
211 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
212 << " we skip the environment variable." << std::endl;
213 }
214 }
215
216 // Test if an input path is set
217 if (opt_ipath.empty() && env_ipath.empty()) {
218 usage(argv[0], NULL, ipath, opt_niter);
219 std::cerr << std::endl << "ERROR:" << std::endl;
220 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
221 << " environment variable to specify the location of the " << std::endl
222 << " image path where test images are located." << std::endl
223 << std::endl;
224 exit(-1);
225 }
226
227 vpImage<unsigned char> Itexture;
228 filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
229 vpImageIo::read(Itexture, filename);
230
231 vpColVector X[4];
232 for (int i = 0; i < 4; i++)
233 X[i].resize(3);
234 // Top left corner
235 X[0][0] = -0.3;
236 X[0][1] = -0.215;
237 X[0][2] = 0;
238
239 // Top right corner
240 X[1][0] = 0.3;
241 X[1][1] = -0.215;
242 X[1][2] = 0;
243
244 // Bottom right corner
245 X[2][0] = 0.3;
246 X[2][1] = 0.215;
247 X[2][2] = 0;
248
249 // Bottom left corner
250 X[3][0] = -0.3;
251 X[3][1] = 0.215;
252 X[3][2] = 0;
253
255
257 sim.init(Itexture, X);
258
259 vpCameraParameters cam(870, 870, 160, 120);
260
261 // ----------------------------------------------------------
262 // Create the framegraber (here a simulated image)
263 vpImage<unsigned char> I(240, 320, 0);
265
266 // camera desired position
268 cdMo[2][3] = 1;
269
270 // set the robot at the desired position
271 sim.setCameraPosition(cdMo);
272 sim.getImage(I, cam); // and aquire the image Id
273 Id = I;
274
275// display the image
276#if defined VISP_HAVE_X11
277 vpDisplayX d;
278#elif defined VISP_HAVE_GDI
279 vpDisplayGDI d;
280#elif defined VISP_HAVE_GTK
281 vpDisplayGTK d;
282#elif defined VISP_HAVE_OPENCV
284#endif
285
286#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
287 if (opt_display) {
288 d.init(I, 20, 10, "Photometric visual servoing : s");
291 }
292 if (opt_display && opt_click_allowed) {
293 std::cout << "Click in the image to continue..." << std::endl;
295 }
296#endif
297
298 // ----------------------------------------------------------
299 // position the robot at the initial position
300 // ----------------------------------------------------------
301
302 // camera desired position
304 cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20));
305 vpHomogeneousMatrix wMo; // Set to identity
306 vpHomogeneousMatrix wMc; // Camera position in the world frame
307
308 // set the robot at the desired position
309 sim.setCameraPosition(cMo);
310 I = 0;
311 sim.getImage(I, cam); // and aquire the image Id
312
313#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
314 if (opt_display) {
317 }
318 if (opt_display && opt_click_allowed) {
319 std::cout << "Click in the image to continue..." << std::endl;
321 }
322#endif
323
325 Idiff = I;
326
327 vpImageTools::imageDifference(I, Id, Idiff);
328
329// Affiche de l'image de difference
330#if defined VISP_HAVE_X11
331 vpDisplayX d1;
332#elif defined VISP_HAVE_GDI
333 vpDisplayGDI d1;
334#elif defined VISP_HAVE_GTK
335 vpDisplayGTK d1;
336#endif
337#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
338 if (opt_display) {
339 d1.init(Idiff, 40 + static_cast<int>(I.getWidth()), 10, "photometric visual servoing : s-s* ");
340 vpDisplay::display(Idiff);
341 vpDisplay::flush(Idiff);
342 }
343#endif
344 // create the robot (here a simulated free flying camera)
345 vpSimulatorCamera robot;
346 robot.setSamplingTime(0.04);
347 wMc = wMo * cMo.inverse();
348 robot.setPosition(wMc);
349
350 // ------------------------------------------------------
351 // Visual feature, interaction matrix, error
352 // s, Ls, Lsd, Lt, Lp, etc
353 // ------------------------------------------------------
354
355 // current visual feature built from the image
356 // (actually, this is the image...)
358 sI.init(I.getHeight(), I.getWidth(), Z);
359 sI.setCameraParameters(cam);
360 sI.buildFrom(I);
361
362 // desired visual feature built from the image
364 sId.init(I.getHeight(), I.getWidth(), Z);
365 sId.setCameraParameters(cam);
366 sId.buildFrom(Id);
367
368 // Create visual-servoing task
369 vpServo servo;
370 // define the task
371 // - we want an eye-in-hand control law
372 // - robot is controlled in the camera frame
374 // add current and desired visual features
375 servo.addFeature(sI, sId);
376 // set the gain
377 servo.setLambda(30);
378 // compute interaction matrix at the desired position
380
381 // set a velocity control mode
383
384 int iter = 1;
385 double normError = 0;
386 vpColVector v; // camera velocity send to the robot
387
388 vpChrono chrono;
389 chrono.start();
390 do {
391 std::cout << "--------------------------------------------" << iter++ << std::endl;
392
393 // Acquire the new image
394 sim.setCameraPosition(cMo);
395 sim.getImage(I, cam);
396#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
397 if (opt_display) {
400 }
401#endif
402 vpImageTools::imageDifference(I, Id, Idiff);
403#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
404 if (opt_display) {
405 vpDisplay::display(Idiff);
406 vpDisplay::flush(Idiff);
407 }
408#endif
409 // Compute current visual feature
410 sI.buildFrom(I);
411
412 v = servo.computeControlLaw(); // camera velocity send to the robot
413
414 normError = servo.getError().sumSquare();
415 std::cout << " |e| = " << normError << std::endl;
416 std::cout << " |v| = " << sqrt(v.sumSquare()) << std::endl;
417
418 // send the robot velocity
420 wMc = robot.getPosition();
421 cMo = wMc.inverse() * wMo;
422 } while (normError > 10000 && iter < opt_niter);
423
424 chrono.stop();
425 std::cout << "Time to convergence: " << chrono.getDurationMs() << " ms" << std::endl;
426
427 v = 0;
429
430 return EXIT_SUCCESS;
431 } catch (const vpException &e) {
432 std::cout << "Catch an exception: " << e << std::endl;
433 return EXIT_FAILURE;
434 }
435#else
436 (void)argc;
437 (void)argv;
438 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
439 return EXIT_SUCCESS;
440#endif
441}
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)
void buildFrom(vpImage< unsigned char > &I)
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
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
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void setLambda(double c)
Definition: vpServo.h:404
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector getError() const
Definition: vpServo.h:278
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ CURRENT
Definition: vpServo.h:182
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.