Visual Servoing Platform version 3.5.0
poseVirtualVS.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 * Pose computation on an object made of dots.
33 * reading of PGM image
34 * Display image using either the X11 or GTK or GDI display
35 * track 4 dots (vpDots) in the image
36 * compute the pose
37 *
38 * Authors:
39 * Eric Marchand
40 * Anthony Saunier
41 *
42 *****************************************************************************/
62#include <iomanip>
63#include <sstream>
64#include <stdio.h>
65#include <stdlib.h>
66#include <visp3/core/vpConfig.h>
67#include <visp3/core/vpDebug.h>
68
69#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) \
70 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
71
72
73#include <visp3/core/vpImage.h>
74#include <visp3/core/vpImagePoint.h>
75#include <visp3/io/vpImageIo.h>
76
77#include <visp3/gui/vpDisplayGDI.h>
78#include <visp3/gui/vpDisplayGTK.h>
79#include <visp3/gui/vpDisplayOpenCV.h>
80#include <visp3/gui/vpDisplayX.h>
81
82#include <visp3/blob/vpDot.h>
83#include <visp3/core/vpIoTools.h>
84#include <visp3/core/vpPixelMeterConversion.h>
85#include <visp3/io/vpParseArgv.h>
86#include <visp3/vision/vpPose.h>
87
88// List of allowed command line options
89#define GETOPTARGS "cdi:p:hf:n:s:"
90
91void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first,
92 unsigned nimages, unsigned step);
93bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages,
94 unsigned &step, bool &click_allowed, bool &display);
95
109void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first,
110 unsigned nimages, unsigned step)
111{
112 fprintf(stdout, "\n\
113Test dot tracking.\n\
114\n\
115SYNOPSIS\n\
116 %s [-i <input image path>] [-p <personal image path>]\n\
117 [-f <first image>] [-n <number of images>] [-s <step>][-c] [-d] [-h]\n", name);
118
119 fprintf(stdout, "\n\
120OPTIONS: Default\n\
121 -i <input image path> %s\n\
122 Set image input path.\n\
123 From this path read images \n\
124 \"cube/image.%%04d.pgm\"\n\
125 Setting the VISP_INPUT_IMAGE_PATH environment\n\
126 variable produces the same behaviour than using\n\
127 this option.\n\
128 \n\
129 -p <personal image path> %s\n\
130 Specify a personal sequence containing images \n\
131 to process.\n\
132 By image sequence, we mean one file per image.\n\
133 The following image file formats PNM (PGM P5, PPM P6)\n\
134 are supported. The format is selected by analysing \n\
135 the filename extension.\n\
136 Example : \"/Temp/ViSP-images/cube/image.%%04d.pgm\"\n\
137 %%04d is for the image numbering.\n\
138 \n\
139 -f <first image> %u\n\
140 First image number of the sequence.\n\
141 \n\
142 -n <number of images> %u\n\
143 Number of images to load from the sequence.\n\
144 \n\
145 -s <step> %u\n\
146 Step between two images.\n\
147\n\
148 -c\n\
149 Disable the mouse click. Useful to automaze the \n\
150 execution of this program without humain intervention.\n\
151\n\
152 -d \n\
153 Turn off the display.\n\
154\n\
155 -h\n\
156 Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
157
158 if (badparam)
159 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
160}
181bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages,
182 unsigned &step, bool &click_allowed, bool &display)
183{
184 const char *optarg_;
185 int c;
186 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
187
188 switch (c) {
189 case 'c':
190 click_allowed = false;
191 break;
192 case 'd':
193 display = false;
194 break;
195 case 'i':
196 ipath = optarg_;
197 break;
198 case 'p':
199 ppath = optarg_;
200 break;
201 case 'f':
202 first = (unsigned)atoi(optarg_);
203 break;
204 case 'n':
205 nimages = (unsigned)atoi(optarg_);
206 break;
207 case 's':
208 step = (unsigned)atoi(optarg_);
209 break;
210 case 'h':
211 usage(argv[0], NULL, ipath, ppath, first, nimages, step);
212 return false;
213 break;
214
215 default:
216 usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
217 return false;
218 break;
219 }
220 }
221
222 if ((c == 1) || (c == -1)) {
223 // standalone param or error
224 usage(argv[0], NULL, ipath, ppath, first, nimages, step);
225 std::cerr << "ERROR: " << std::endl;
226 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
227 return false;
228 }
229
230 return true;
231}
232
233int main(int argc, const char **argv)
234{
235 try {
236 std::string env_ipath;
237 std::string opt_ipath;
238 std::string ipath;
239 std::string opt_ppath;
240 std::string dirname;
241 std::string filename;
242 unsigned opt_first = 0;
243 unsigned opt_nimages = 80;
244 unsigned opt_step = 1;
245 bool opt_click_allowed = true;
246 bool opt_display = true;
247
248 int i;
249
250 std::cout << "-------------------------------------------------------" << std::endl;
251 std::cout << " poseVirtualVS.cpp" << std::endl << std::endl;
252
253 std::cout << " Example of dots tracking in an image sequence and pose "
254 "computation"
255 << std::endl;
256 std::cout << "-------------------------------------------------------" << std::endl;
257 std::cout << std::endl;
258
259 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
260 // environment variable value
262
263 // Set the default input path
264 if (!env_ipath.empty())
265 ipath = env_ipath;
266
267 // Read the command line options
268 if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
269 opt_display) == false) {
270 exit(-1);
271 }
272
273 // Get the option values
274 if (!opt_ipath.empty())
275 ipath = opt_ipath;
276
277 // Compare ipath and env_ipath. If they differ, we take into account
278 // the input path comming from the command line option
279 if (opt_ipath.empty() && opt_ppath.empty()) {
280 if (ipath != env_ipath) {
281 std::cout << std::endl << "WARNING: " << std::endl;
282 std::cout << " Since -i <visp image path=" << ipath << "> "
283 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
284 << " we skip the environment variable." << std::endl;
285 }
286 }
287 // Test if an input path is set
288 if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
289 usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
290 std::cerr << std::endl << "ERROR:" << std::endl;
291 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
292 << " environment variable to specify the location of the " << std::endl
293 << " image path where test images are located." << std::endl
294 << " Use -p <personal image path> option if you want to " << std::endl
295 << " use personal images" << std::endl
296 << std::endl;
297 exit(-1);
298 }
299
300 // Declare an image, this is a gray level image (unsigned char)
301 // it size is not defined yet, it will be defined when the image will
302 // read on the disk
304
305 unsigned iter = opt_first;
306 std::ostringstream s;
307 char cfilename[FILENAME_MAX];
308
309 if (opt_ppath.empty()) {
310
311 // Warning :
312 // the image sequence is not provided with the ViSP package
313 // therefore the program will return you an error :
314 // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
315 // /ViSP-images/cube/image.0001.pgm
316 // !! poseExample.cpp: main(#95) :Error while reading the image
317 // terminate called after throwing an instance of 'vpImageException'
318 //
319 // The sequence is available on the visp www site
320 // https://visp.inria.fr/download/
321 // in the download section. It is named "ViSP-images-x.y.z.tar.gz"
322
323 // directory name
324 dirname = vpIoTools::createFilePath(ipath, "cube");
325
326 // Build the name of the image file
327
328 s.setf(std::ios::right, std::ios::adjustfield);
329 s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
330 filename = vpIoTools::createFilePath(dirname, s.str());
331 } else {
332
333 sprintf(cfilename, opt_ppath.c_str(), iter);
334 filename = cfilename;
335 }
336
337 // define the vpDot structure, here 4 dots will tracked
338 vpDot d[4];
339
340 for (i = 0; i < 4; i++) {
341 // by using setGraphics, we request to see the all the pixel of the dot
342 // in green on the screen.
343 // It uses the overlay image plane.
344 // The default of this setting is that it is time consumming
345
346 if (opt_display) {
347 d[i].setGraphics(true);
348 } else {
349 d[i].setGraphics(false);
350 }
351 }
352
353 // Read the PGM image named "s" on the disk, and put the bitmap into the
354 // image structure I.
355 // I is initialized to the correct size
356 //
357 // exception readPGM may throw various exception if, for example,
358 // the file does not exist, or if the memory cannot be allocated
359 try {
360 vpImageIo::read(I, filename);
361 } catch (...) {
362 // an exception is throwned if an exception from readPGM has been
363 // catched here this will result in the end of the program Note that
364 // another error message has been printed from readPGM to give more
365 // information about the error
366 if (opt_ppath.empty()) {
367 std::cerr << std::endl << "ERROR:" << std::endl;
368 std::cerr << " Cannot read " << filename << std::endl;
369 std::cerr << " Check your -i " << ipath << " option, " << std::endl
370 << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl;
371 } else {
372 std::cerr << std::endl << "ERROR:" << std::endl;
373 std::cerr << " Cannot read " << filename << std::endl;
374 std::cerr << " or your -p " << opt_ppath << " option " << std::endl << std::endl;
375 }
376 exit(-1);
377 }
378
379// We open a window using either the X11 or GTK or GDI window manager
380// it will be located in 100,100 and titled "tracking using vpDot"
381// its size is automatically defined by the image (I) size
382#if defined VISP_HAVE_X11
383 vpDisplayX display;
384#elif defined VISP_HAVE_GTK
385 vpDisplayGTK display;
386#elif defined VISP_HAVE_GDI
387 vpDisplayGDI display;
388#elif defined VISP_HAVE_OPENCV
389 vpDisplayOpenCV display;
390#endif
391 if (opt_display) {
392 // Display size is automatically defined by the image (I) size
393 display.init(I, 100, 100, "tracking using vpDot");
394 // display the image
395 // The image class has a member that specify a pointer toward
396 // the display that has been initialized in the display declaration
397 // therefore is is no longuer necessary to make a reference to the
398 // display variable.
400 // Flush the display
402 }
403
404 vpImagePoint cog[4]; // Center of gravity of the dot
405 if (opt_display && opt_click_allowed) {
406 // dot coordinates (u,v) = (column,row)
407 std::cout << "Click the four white dots on the object corner clockwise" << std::endl;
408 for (i = 0; i < 4; i++) {
409 // tracking is initalized if no other parameters are given
410 // to the iniTracking(..) method a right mouse click on the
411 // dot is expected dot location can also be specified
412 // explicitely in the initTracking method :
413 // d.initTracking(I,ip) where ip is the image point from
414 // where the dot need to be searched.
415
416 d[i].initTracking(I);
417 // track the dot and returns its coordinates in the image
418 // results are given in float since many many are usually considered
419 //
420 // an expcetion is thrown by the track method if
421 // - dot is lost
422 // - the number of pixel is too small
423 // - too many pixels are detected (this is usual when a "big"
424 // specularity
425 // occurs. The threshold can be modified using the
426 // setMaxDotSize() method
427 d[i].track(I, cog[i]);
429 }
430 } else {
431 cog[0].set_u(194);
432 cog[0].set_v(88);
433 d[0].initTracking(I, cog[0]);
434 d[0].track(I, cog[0]);
436
437 cog[1].set_u(225);
438 cog[1].set_v(84);
439 d[1].initTracking(I, cog[1]);
440 d[1].track(I, cog[1]);
442
443 cog[2].set_u(242);
444 cog[2].set_v(114);
445 d[2].initTracking(I, cog[2]);
446 d[2].track(I, cog[2]);
448
449 cog[3].set_u(212);
450 cog[3].set_v(131);
451 d[3].initTracking(I, cog[3]);
452 d[3].track(I, cog[3]);
454 }
455
456 if (opt_display) {
457
458 // display a red cross (size 10) in the image at the dot center
459 // of gravity location
460 //
461 // WARNING
462 // in the vpDisplay class member's when pixel coordinates
463 // are considered the first element is the row index and the second
464 // is the column index:
465 // vpDisplay::displayCross(Image, row index, column index, size,
466 // color) therefore u and v are inverted wrt to the vpDot
467 // specification
468 // Alternatively, to avoid this problem another set of member have
469 // been defined in the vpDisplay class.
470 // If the method name is postfixe with _uv the specification is :
471 // vpDisplay::displayCross_uv(Image, column index, row index, size,
472 // color)
473
474 for (i = 0; i < 4; i++)
476
477 // flush the X11 buffer
479 }
480
481 // --------------------------------------------------------
482 // Now wil compute the pose
483 //
484
485 // The pose will be contained in an homogeneous matrix cMo
487
488 // We need a structure that content both the 3D coordinates of the point
489 // in the object frame and the 2D coordinates of the point expressed in
490 // meter the vpPoint class is ok for that
491 vpPoint P[4];
492
493 // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x,
494 // y) )
495 vpPose pose;
496 // the list of point is cleared (if that's not done before)
497 pose.clearPoint();
498
499 // we set the 3D points coordinates (in meter !) in the object/world frame
500 double L = 0.04;
501 P[0].setWorldCoordinates(-L, -L, 0); // (X,Y,Z)
502 P[1].setWorldCoordinates(L, -L, 0);
503 P[2].setWorldCoordinates(L, L, 0);
504 P[3].setWorldCoordinates(-L, L, 0);
505
506 // set the camera intrinsic parameters
507 // see more details about the model in vpCameraParameters
508 double px = 600;
509 double py = 600;
510 double u0 = 192;
511 double v0 = 144;
512 vpCameraParameters cam(px, py, u0, v0);
513
514 // pixel-> meter conversion
515 for (i = 0; i < 4; i++) {
516 // u[i]. v[i] are expressed in pixel
517 // conversion in meter is achieved using
518 // x = (u-u0)/px
519 // y = (v-v0)/py
520 // where px, py, u0, v0 are the intrinsic camera parameters
521 double x = 0, y = 0;
522 vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
523 P[i].set_x(x);
524 P[i].set_y(y);
525 }
526
527 // The pose structure is build, we put in the point list the set of point
528 // here both 2D and 3D world coordinates are known
529 for (i = 0; i < 4; i++) {
530 pose.addPoint(P[i]); // and added to the pose computation point list
531 }
532
533 // compute the initial pose using Dementhon method followed by a non
534 // linear minimisation method
535
536 // Pose by Lagrange it provides an initialization of the pose
537 pose.computePose(vpPose::LAGRANGE, cMo);
538 // the pose is now refined using the virtual visual servoing approach
539 // Warning: cMo needs to be initialized otherwise it may diverge
541 if (opt_display) {
542 // display the compute pose
543 pose.display(I, cMo, cam, 0.05, vpColor::red);
545 }
546
547 // Covariance Matrix Computation
548 // Uncomment if you want to compute the covariance matrix.
549 // pose.setCovarianceComputation(true); //Important if you want
550 // tracker.getCovarianceMatrix() to work.
551
552 unsigned niter = 0;
553 // this is the loop over the image sequence
554 while (iter < opt_nimages) {
555 // set the new image name
556
557 if (opt_ppath.empty()) {
558 s.str("");
559 s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
560 filename = vpIoTools::createFilePath(dirname, s.str());
561 } else {
562 sprintf(cfilename, opt_ppath.c_str(), iter);
563 filename = cfilename;
564 }
565
566 // read the image
567 vpImageIo::read(I, filename);
568 if (opt_display) {
569 // Display the image
571 // Flush the display
573 }
574 // kill the point list
575 pose.clearPoint();
576
577 // track the dot
578 for (i = 0; i < 4; i++) {
579 // track the point
580 d[i].track(I, cog[i]);
581 if (opt_display) {
582 // display point location
584 }
585 // pixel->meter conversion
586 {
587 double x = 0, y = 0;
588 vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
589 P[i].set_x(x);
590 P[i].set_y(y);
591 }
592
593 // and added to the pose computation point list
594 pose.addPoint(P[i]);
595 }
596 // the pose structure has been updated
597
598 // the pose is now updated using the virtual visual servoing approach
599 // Dementhon or lagrange is no longuer necessary, pose at the
600 // previous iteration is sufficient
602 if (opt_display) {
603 // display the compute pose
604 pose.display(I, cMo, cam, 0.05, vpColor::red);
605
607 }
608
609 // Covariance Matrix Display
610 // Uncomment if you want to print the covariance matrix.
611 // Make sure pose.setCovarianceComputation(true) has been called
612 // (uncomment below). std::cout << pose.getCovarianceMatrix() <<
613 // std::endl << std::endl;
614
615 niter++;
616
617 iter += opt_step;
618 }
619 return EXIT_SUCCESS;
620 } catch (const vpException &e) {
621 std::cout << "Catch a ViSP exception: " << e << std::endl;
622 return EXIT_FAILURE;
623 }
624}
625#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
626int main()
627{
628 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
629 return EXIT_SUCCESS;
630}
631#else
632int main()
633{
634 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
635 std::cout << "Tip if you are on a unix-like system:" << std::endl;
636 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
637 std::cout << "Tip if you are on a windows-like system:" << std::endl;
638 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
639 return EXIT_SUCCESS;
640}
641#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
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 void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
Definition: vpDot.h:116
error that can be emited by ViSP classes.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_u(double u)
Definition: vpImagePoint.h:225
void set_v(double v)
Definition: vpImagePoint.h:236
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 bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:81
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
void clearPoint()
Definition: vpPose.cpp:134
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:489