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