Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
poseVirtualVS.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 * 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*****************************************************************************/
58#include <iomanip>
59#include <sstream>
60#include <stdio.h>
61#include <stdlib.h>
62#include <visp3/core/vpConfig.h>
63#include <visp3/core/vpDebug.h>
64
65#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
66 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
67
68#include <visp3/core/vpImage.h>
69#include <visp3/core/vpImagePoint.h>
70#include <visp3/io/vpImageIo.h>
71
72#include <visp3/gui/vpDisplayGDI.h>
73#include <visp3/gui/vpDisplayGTK.h>
74#include <visp3/gui/vpDisplayOpenCV.h>
75#include <visp3/gui/vpDisplayX.h>
76
77#include <visp3/blob/vpDot.h>
78#include <visp3/core/vpIoTools.h>
79#include <visp3/core/vpPixelMeterConversion.h>
80#include <visp3/io/vpParseArgv.h>
81#include <visp3/vision/vpPose.h>
82
83// List of allowed command line options
84#define GETOPTARGS "cdi:p:hf:l:s:"
85
99void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first,
100 unsigned last, unsigned step)
101{
102#if VISP_HAVE_DATASET_VERSION >= 0x030600
103 std::string ext("png");
104#else
105 std::string ext("pgm");
106#endif
107 fprintf(stdout, "\n\
108Test dot tracking.\n\
109\n\
110SYNOPSIS\n\
111 %s [-i <input image path>] [-p <personal image path>]\n\
112 [-f <first image>] [-l <last image>] [-s <step>][-c] [-d] [-h]\n",
113 name);
114
115 fprintf(stdout, "\n\
116OPTIONS: Default\n\
117 -i <input image path> %s\n\
118 Set image input path.\n\
119 From this path read images \n\
120 \"cube/image.%%04d.%s\"\n\
121 Setting the VISP_INPUT_IMAGE_PATH environment\n\
122 variable produces the same behaviour than using\n\
123 this option.\n\
124 \n\
125 -p <personal image path> %s\n\
126 Specify a personal sequence containing images \n\
127 to process.\n\
128 By image sequence, we mean one file per image.\n\
129 The format is selected by analysing the filename extension.\n\
130 Example : \"/Temp/visp-images/cube/image.%%04d.%s\"\n\
131 %%04d is for the image numbering.\n\
132 \n\
133 -f <first image> %u\n\
134 First image number of the sequence.\n\
135 \n\
136 -l <last image> %u\n\
137 Last image number of the sequence.\n\
138 \n\
139 -s <step> %u\n\
140 Step between two images.\n\
141\n\
142 -c\n\
143 Disable the mouse click. Useful to automate the \n\
144 execution of this program without human intervention.\n\
145\n\
146 -d \n\
147 Turn off the display.\n\
148\n\
149 -h\n\
150 Print the help.\n",
151 ipath.c_str(), ext.c_str(), ppath.c_str(), ext.c_str(), first, last, step);
152
153 if (badparam)
154 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
155}
176bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &last,
177 unsigned &step, bool &click_allowed, bool &display)
178{
179 const char *optarg_;
180 int c;
181 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
182
183 switch (c) {
184 case 'c':
185 click_allowed = false;
186 break;
187 case 'd':
188 display = false;
189 break;
190 case 'i':
191 ipath = optarg_;
192 break;
193 case 'p':
194 ppath = optarg_;
195 break;
196 case 'f':
197 first = (unsigned)atoi(optarg_);
198 break;
199 case 'n':
200 last = (unsigned)atoi(optarg_);
201 break;
202 case 's':
203 step = (unsigned)atoi(optarg_);
204 break;
205 case 'h':
206 usage(argv[0], NULL, ipath, ppath, first, last, step);
207 return false;
208 break;
209
210 default:
211 usage(argv[0], optarg_, ipath, ppath, first, last, step);
212 return false;
213 break;
214 }
215 }
216
217 if ((c == 1) || (c == -1)) {
218 // standalone param or error
219 usage(argv[0], NULL, ipath, ppath, first, last, step);
220 std::cerr << "ERROR: " << std::endl;
221 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
222 return false;
223 }
224
225 return true;
226}
227
228int main(int argc, const char **argv)
229{
230 try {
231 std::string env_ipath;
232 std::string opt_ipath;
233 std::string ipath;
234 std::string opt_ppath;
235 std::string dirname;
236 std::string filename;
237 unsigned opt_first = 0;
238 unsigned opt_last = 80;
239 unsigned opt_step = 1;
240 bool opt_click_allowed = true;
241 bool opt_display = true;
242 int i;
243
244#if VISP_HAVE_DATASET_VERSION >= 0x030600
245 std::string ext("png");
246#else
247 std::string ext("pgm");
248#endif
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_last, opt_step, opt_click_allowed,
269 opt_display) == false) {
270 return EXIT_FAILURE;
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_last, 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 return EXIT_FAILURE;
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 : the datset is available on https://visp.inria.fr/download/
312 dirname = vpIoTools::createFilePath(ipath, "cube");
313
314 // Build the name of the image file
315
316 s.setf(std::ios::right, std::ios::adjustfield);
317 s << "image." << std::setw(4) << std::setfill('0') << iter << "." << ext;
318 filename = vpIoTools::createFilePath(dirname, s.str());
319 } else {
320
321 snprintf(cfilename, FILENAME_MAX, opt_ppath.c_str(), iter);
322 filename = cfilename;
323 }
324
325 // define the vpDot structure, here 4 dots will tracked
326 vpDot d[4];
327
328 for (i = 0; i < 4; i++) {
329 // by using setGraphics, we request to see the all the pixel of the dot
330 // in green on the screen.
331 // It uses the overlay image plane.
332 // The default of this setting is that it is time consuming
333
334 if (opt_display) {
335 d[i].setGraphics(true);
336 } else {
337 d[i].setGraphics(false);
338 }
339 }
340
341 // Read image named filename and put the bitmap into in I.
342 try {
343 vpImageIo::read(I, filename);
344 } catch (...) {
345 if (opt_ppath.empty()) {
346 std::cerr << std::endl << "ERROR:" << std::endl;
347 std::cerr << " Cannot read " << filename << std::endl;
348 std::cerr << " Check your -i " << ipath << " option, " << std::endl
349 << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl;
350 } else {
351 std::cerr << std::endl << "ERROR:" << std::endl;
352 std::cerr << " Cannot read " << filename << std::endl;
353 std::cerr << " or your -p " << opt_ppath << " option " << std::endl << std::endl;
354 }
355 return EXIT_FAILURE;
356 }
357
358// We open a window using either the X11 or GTK or GDI window manager
359// it will be located in 100,100 and titled "tracking using vpDot"
360// its size is automatically defined by the image (I) size
361#if defined(VISP_HAVE_X11)
362 vpDisplayX display;
363#elif defined(VISP_HAVE_GTK)
364 vpDisplayGTK display;
365#elif defined(VISP_HAVE_GDI)
366 vpDisplayGDI display;
367#elif defined(HAVE_OPENCV_HIGHGUI)
368 vpDisplayOpenCV display;
369#endif
370 if (opt_display) {
371 // Display size is automatically defined by the image (I) size
372 display.init(I, 100, 100, "tracking using vpDot");
373 // display the image
374 // The image class has a member that specify a pointer toward
375 // the display that has been initialized in the display declaration
376 // therefore is is no longer necessary to make a reference to the
377 // display variable.
379 // Flush the display
381 }
382
383 vpImagePoint cog[4]; // Center of gravity of the dot
384 if (opt_display && opt_click_allowed) {
385 // dot coordinates (u,v) = (column,row)
386 std::cout << "Click the four white dots on the object corner clockwise" << std::endl;
387 for (i = 0; i < 4; i++) {
388 // tracking is initalized if no other parameters are given
389 // to the iniTracking(..) method a right mouse click on the
390 // dot is expected dot location can also be specified
391 // explicitly in the initTracking method :
392 // d.initTracking(I,ip) where ip is the image point from
393 // where the dot need to be searched.
394
395 d[i].initTracking(I);
396 // track the dot and returns its coordinates in the image
397 // results are given in float since many many are usually considered
398 //
399 // an exception is thrown by the track method if
400 // - dot is lost
401 // - the number of pixel is too small
402 // - too many pixels are detected (this is usual when a "big"
403 // specularity
404 // occurs. The threshold can be modified using the
405 // setMaxDotSize() method
406 d[i].track(I, cog[i]);
408 }
409 } else {
410 cog[0].set_u(194);
411 cog[0].set_v(88);
412 d[0].initTracking(I, cog[0]);
413 d[0].track(I, cog[0]);
415
416 cog[1].set_u(225);
417 cog[1].set_v(84);
418 d[1].initTracking(I, cog[1]);
419 d[1].track(I, cog[1]);
421
422 cog[2].set_u(242);
423 cog[2].set_v(114);
424 d[2].initTracking(I, cog[2]);
425 d[2].track(I, cog[2]);
427
428 cog[3].set_u(212);
429 cog[3].set_v(131);
430 d[3].initTracking(I, cog[3]);
431 d[3].track(I, cog[3]);
433 }
434
435 if (opt_display) {
436
437 // display a red cross (size 10) in the image at the dot center
438 // of gravity location
439 //
440 // WARNING
441 // in the vpDisplay class member's when pixel coordinates
442 // are considered the first element is the row index and the second
443 // is the column index:
444 // vpDisplay::displayCross(Image, row index, column index, size,
445 // color) therefore u and v are inverted wrt to the vpDot
446 // specification
447 // Alternatively, to avoid this problem another set of member have
448 // been defined in the vpDisplay class.
449 // If the method name is postfixe with _uv the specification is :
450 // vpDisplay::displayCross_uv(Image, column index, row index, size,
451 // color)
452
453 for (i = 0; i < 4; i++)
455
456 // flush the X11 buffer
458 }
459
460 // --------------------------------------------------------
461 // Now wil compute the pose
462 //
463
464 // The pose will be contained in an homogeneous matrix cMo
466
467 // We need a structure that content both the 3D coordinates of the point
468 // in the object frame and the 2D coordinates of the point expressed in
469 // meter the vpPoint class is ok for that
470 vpPoint P[4];
471
472 // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x,
473 // y) )
474 vpPose pose;
475 // the list of point is cleared (if that's not done before)
476 pose.clearPoint();
477
478 // we set the 3D points coordinates (in meter !) in the object/world frame
479 double L = 0.04;
480 P[0].setWorldCoordinates(-L, -L, 0); // (X,Y,Z)
481 P[1].setWorldCoordinates(L, -L, 0);
482 P[2].setWorldCoordinates(L, L, 0);
483 P[3].setWorldCoordinates(-L, L, 0);
484
485 // set the camera intrinsic parameters
486 // see more details about the model in vpCameraParameters
487 double px = 600;
488 double py = 600;
489 double u0 = 192;
490 double v0 = 144;
491 vpCameraParameters cam(px, py, u0, v0);
492
493 // pixel-> meter conversion
494 for (i = 0; i < 4; i++) {
495 // u[i]. v[i] are expressed in pixel
496 // conversion in meter is achieved using
497 // x = (u-u0)/px
498 // y = (v-v0)/py
499 // where px, py, u0, v0 are the intrinsic camera parameters
500 double x = 0, y = 0;
501 vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
502 P[i].set_x(x);
503 P[i].set_y(y);
504 }
505
506 // The pose structure is build, we put in the point list the set of point
507 // here both 2D and 3D world coordinates are known
508 for (i = 0; i < 4; i++) {
509 pose.addPoint(P[i]); // and added to the pose computation point list
510 }
511
512 // compute the initial pose using Dementhon method followed by a non
513 // linear minimization method
514
515 // Pose by Dementhon or Lagrange provides an initialization of the non linear virtual visual-servoing pose estimation
517 if (opt_display) {
518 // display the compute pose
519 pose.display(I, cMo, cam, 0.05, vpColor::red);
521 }
522
523 // Covariance Matrix Computation
524 // Uncomment if you want to compute the covariance matrix.
525 // pose.setCovarianceComputation(true); //Important if you want
526 // tracker.getCovarianceMatrix() to work.
527
528 // this is the loop over the image sequence
529 while (iter < opt_last) {
530 // set the new image name
531
532 if (opt_ppath.empty()) {
533 s.str("");
534 s << "image." << std::setw(4) << std::setfill('0') << iter << "." << ext;
535 filename = vpIoTools::createFilePath(dirname, s.str());
536 } else {
537 snprintf(cfilename, FILENAME_MAX, opt_ppath.c_str(), iter);
538 filename = cfilename;
539 }
540
541 // read the image
542 vpImageIo::read(I, filename);
543 if (opt_display) {
544 // Display the image
546 // Flush the display
548 }
549 // kill the point list
550 pose.clearPoint();
551
552 // track the dot
553 for (i = 0; i < 4; i++) {
554 // track the point
555 d[i].track(I, cog[i]);
556 if (opt_display) {
557 // display point location
559 }
560 // pixel->meter conversion
561 {
562 double x = 0, y = 0;
563 vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
564 P[i].set_x(x);
565 P[i].set_y(y);
566 }
567
568 // and added to the pose computation point list
569 pose.addPoint(P[i]);
570 }
571 // the pose structure has been updated
572
573 // the pose is now updated using the virtual visual servoing approach
574 // Dementhon or lagrange is no longer necessary, pose at the
575 // previous iteration is sufficient
577 if (opt_display) {
578 // display the compute pose
579 pose.display(I, cMo, cam, 0.05, vpColor::red);
580
582 }
583
584 // Covariance Matrix Display
585 // Uncomment if you want to print the covariance matrix.
586 // Make sure pose.setCovarianceComputation(true) has been called
587 // (uncomment below). std::cout << pose.getCovarianceMatrix() <<
588 // std::endl << std::endl;
589
590 iter += opt_step;
591 }
592 return EXIT_SUCCESS;
593 } catch (const vpException &e) {
594 std::cout << "Catch a ViSP exception: " << e << std::endl;
595 return EXIT_FAILURE;
596 }
597}
598#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
599int main()
600{
601 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
602 return EXIT_SUCCESS;
603}
604#else
605int main()
606{
607 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
608 << std::endl;
609 std::cout << "Tip if you are on a unix-like system:" << std::endl;
610 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
611 std::cout << "Tip if you are on a windows-like system:" << std::endl;
612 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
613 return EXIT_SUCCESS;
614}
615#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition vpColor.h:211
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
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:112
error that can be emitted by ViSP classes.
Definition vpException.h:59
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_u(double u)
void set_v(double v)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static std::string getViSPImagesDataPath()
static std::string createFilePath(const std::string &parent, const std::string &child)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
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:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
@ VIRTUAL_VS
Definition vpPose.h:96
void clearPoint()
Definition vpPose.cpp:125
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition vpPose.cpp:694