Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
AROgreBasic.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 * Implementation of a simple augmented reality application using the vpAROgre
33 * class.
34 *
35*****************************************************************************/
36
42#include <iostream>
43#include <visp3/core/vpConfig.h>
44
45//#if defined(VISP_HAVE_OGRE) && (defined(VISP_HAVE_OPENCV) ||
46// defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
47//|| (defined(VISP_HAVE_X11) && ! defined(APPLE)))
48#if defined(VISP_HAVE_OGRE) && \
49 (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) || \
50 (defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))))
51
52//#if defined(VISP_HAVE_X11) && ! defined(APPLE)
53#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
54// produce an error on OSX: ‘typedef int Cursor’
55// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
56// declaration as ‘typedef XID Cursor’. That's why it should not be
57// used on APPLE platforms
58#include <visp3/gui/vpDisplayX.h>
59#endif
60#include <visp3/ar/vpAROgre.h>
61#include <visp3/blob/vpDot2.h>
62#include <visp3/core/vpDebug.h>
63#include <visp3/core/vpImagePoint.h>
64#include <visp3/core/vpIoTools.h>
65#include <visp3/core/vpPixelMeterConversion.h>
66#include <visp3/core/vpPoint.h>
67#include <visp3/gui/vpDisplayD3D.h>
68#include <visp3/gui/vpDisplayGDI.h>
69#include <visp3/gui/vpDisplayGTK.h>
70#include <visp3/gui/vpDisplayOpenCV.h>
71#include <visp3/io/vpParseArgv.h>
72#include <visp3/io/vpVideoReader.h>
73#include <visp3/vision/vpPose.h>
74
75// List of allowed command line options
76#define GETOPTARGS "ci:p:h"
77
88void usage(const char *name, const char *badparam, std::string ipath, std::string ppath)
89{
90#if VISP_HAVE_DATASET_VERSION >= 0x030600
91 std::string ext("png");
92#else
93 std::string ext("pgm");
94#endif
95 fprintf(stdout, "\n\
96Test augmented reality using the vpAROgre class.\n\
97\n\
98SYNOPSIS\n\
99 %s [-i <test image path>] [-p <personal image path>]\n\
100 [-c] [-h]\n", name);
101
102 fprintf(stdout, "\n\
103OPTIONS: Default\n\
104 -i <input image path> %s\n\
105 Set image input path.\n\
106 From this path read images \n\
107 \"mire-2/image.%%04d.%s\". These \n\
108 images come from ViSP-images-x.y.z.tar.gz available \n\
109 on the ViSP website.\n\
110 Setting the VISP_INPUT_IMAGE_PATH environment\n\
111 variable produces the same behaviour than using\n\
112 this option.\n\
113 \n\
114 -p <personal image path> %s\n\
115 Specify a personal sequence containing images \n\
116 to process.\n\
117 By image sequence, we mean one file per image.\n\
118 Example : \"/Temp/visp-images/cube/image.%%04d.%s\"\n\
119 %%04d is for the image numbering.\n\
120\n\
121 -c\n\
122 Disable the mouse click. Useful to automate the \n\
123 execution of this program without human intervention.\n\
124\n\
125 -h\n\
126 Print the help.\n",
127 ipath.c_str(), ext.c_str(), ppath.c_str(), ext.c_str());
128
129 if (badparam)
130 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
131}
145bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed)
146{
147 const char *optarg_;
148 int c;
149 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
150
151 switch (c) {
152 case 'c':
153 click_allowed = false;
154 break;
155 case 'i':
156 ipath = optarg_;
157 break;
158 case 'p':
159 ppath = optarg_;
160 break;
161 case 'h':
162 usage(argv[0], NULL, ipath, ppath);
163 return false;
164 break;
165
166 default:
167 usage(argv[0], optarg_, ipath, ppath);
168 return false;
169 break;
170 }
171 }
172
173 if ((c == 1) || (c == -1)) {
174 // standalone param or error
175 usage(argv[0], NULL, ipath, ppath);
176 std::cerr << "ERROR: " << std::endl;
177 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
178 return false;
179 }
180
181 return true;
182}
183
190void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I, vpPose *mPose, vpDot2 *md,
191 vpImagePoint *mcog, vpHomogeneousMatrix *cMo, vpPoint *mP, const bool &opt_click_allowed)
192{
193 // ---------------------------------------------------
194 // Code inspired from ViSP example of camera pose
195 // ----------------------------------------------------
196 bool opt_display = true;
197
198 //#if defined(VISP_HAVE_X11) && ! defined(APPLE)
199#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
200 // produce an error on OSX: ‘typedef int Cursor’
201 // /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
202 // declaration as ‘typedef XID Cursor’. That's why it should not be
203 // used on APPLE platforms
204 vpDisplayX display;
205#elif defined(VISP_HAVE_GTK)
206 vpDisplayGTK display;
207#elif defined(VISP_HAVE_GDI)
208 vpDisplayGDI display;
209#elif defined(HAVE_OPENCV_HIGHGUI)
210 vpDisplayOpenCV display;
211#elif defined(VISP_HAVE_D3D9)
212 vpDisplayD3D display;
213#endif
214
215 for (unsigned int i = 0; i < 4; i++) {
216 if (opt_display) {
217 md[i].setGraphics(true);
218 }
219 else {
220 md[i].setGraphics(false);
221 }
222 }
223
224 if (opt_display) {
225 try {
226 // Display size is automatically defined by the image (I) size
227 display.init(I, 100, 100, "Preliminary Pose Calculation");
228 // display the image
229 // The image class has a member that specify a pointer toward
230 // the display that has been initialized in the display declaration
231 // therefore is is no longer necessary to make a reference to the
232 // display variable.
234 // Flush the display
236
237 }
238 catch (...) {
239 vpERROR_TRACE("Error while displaying the image");
240 return;
241 }
242 }
243
244 std::cout << "*************************************************************"
245 "***********************"
246 << std::endl;
247 std::cout << "*************************** Preliminary Pose Calculation "
248 "***************************"
249 << std::endl;
250 std::cout << "****************************** Click on the 4 dots "
251 "*******************************"
252 << std::endl;
253 std::cout << "********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), "
254 "Dot4 : (-x,y,0)**********"
255 << std::endl;
256 std::cout << "*************************************************************"
257 "***********************"
258 << std::endl;
259
260 try {
261 vpImagePoint ip[4];
262 if (!opt_click_allowed) {
263 ip[0].set_i(265);
264 ip[0].set_j(93);
265 ip[1].set_i(248);
266 ip[1].set_j(242);
267 ip[2].set_i(166);
268 ip[2].set_j(215);
269 ip[3].set_i(178);
270 ip[3].set_j(85);
271 }
272
273 for (unsigned int i = 0; i < 4; i++) {
274 // by using setGraphics, we request to see the edges of the dot
275 // in red on the screen.
276 // It uses the overlay image plane.
277 // The default of this setting is that it is time consuming
278
279 md[i].setGraphics(true);
280 md[i].setGrayLevelPrecision(0.7);
281 md[i].setSizePrecision(0.5);
282
283 for (unsigned int j = 0; j < i; j++)
284 md[j].display(I);
285
286 // flush the display buffer
288 try {
289 if (opt_click_allowed) {
290 md[i].initTracking(I);
291 // std::cout << "click " << i << " " << md[i] << std::endl;
292 }
293 else {
294 md[i].initTracking(I, ip[i]);
295 }
296 }
297 catch (...) {
298 }
299
300 mcog[i] = md[i].getCog();
301 // an exception is thrown by the track method if
302 // - dot is lost
303 // - the number of pixel is too small
304 // - too many pixels are detected (this is usual when a "big"
305 // specularity
306 // occurs. The threshold can be modified using the
307 // setNbMaxPoint(int) method
308 if (opt_display) {
309 md[i].display(I);
310 // flush the display buffer
312 }
313 }
314 }
315 catch (const vpException &e) {
316 vpERROR_TRACE("Error while tracking dots");
317 vpCTRACE << e;
318 return;
319 }
320
321 if (opt_display) {
322 // display a red cross (size 10) in the image at the dot center
323 // of gravity location
324 //
325 // WARNING
326 // in the vpDisplay class member's when pixel coordinates
327 // are considered the first element is the row index and the second
328 // is the column index:
329 // vpDisplay::displayCross(Image, row index, column index, size, color)
330 // therefore u and v are inverted wrt to the vpDot specification
331 // Alternatively, to avoid this problem another set of member have
332 // been defined in the vpDisplay class.
333 // If the method name is postfixe with _uv the specification is :
334 // vpDisplay::displayCross_uv(Image, column index, row index, size,
335 // color)
336
337 for (unsigned int i = 0; i < 4; i++)
338 vpDisplay::displayCross(I, mcog[i], 10, vpColor::red);
339
340 // flush the X11 buffer
342 }
343
344 // --------------------------------------------------------
345 // Now we will compute the pose
346 // --------------------------------------------------------
347
348 // the list of point is cleared (if that's not done before)
349 mPose->clearPoint();
350
351 // we set the 3D points coordinates (in meter !) in the object/world frame
352 double l = 0.06;
353 double L = 0.07;
354 mP[0].setWorldCoordinates(-L, -l, 0); // (X,Y,Z)
355 mP[1].setWorldCoordinates(L, -l, 0);
356 mP[2].setWorldCoordinates(L, l, 0);
357 mP[3].setWorldCoordinates(-L, l, 0);
358
359 // pixel-> meter conversion
360 for (unsigned int i = 0; i < 4; i++) {
361 // u[i]. v[i] are expressed in pixel
362 // conversion in meter is achieved using
363 // x = (u-u0)/px
364 // y = (v-v0)/py
365 // where px, py, u0, v0 are the intrinsic camera parameters
366 double x = 0, y = 0;
367 vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x, y);
368 mP[i].set_x(x);
369 mP[i].set_y(y);
370 }
371
372 // The pose structure is build, we put in the point list the set of point
373 // here both 2D and 3D world coordinates are known
374 for (unsigned int i = 0; i < 4; i++) {
375 mPose->addPoint(mP[i]); // and added to the pose computation point list
376 }
377
378 // compute the initial pose using Dementhon method followed by a non linear
379 // minimization method
380
381 // Compute initial pose
383
384 // Display briefly just to have a glimpse a the ViSP pose
385 if (opt_display) {
386 // Display the computed pose
387 mPose->display(I, *cMo, *mcam, 0.05, vpColor::red);
389 vpTime::wait(1000);
390 }
391}
392
393int main(int argc, const char **argv)
394{
395#if VISP_HAVE_DATASET_VERSION >= 0x030600
396 std::string ext("png");
397#else
398 std::string ext("pgm");
399#endif
400 try {
401 std::string env_ipath;
402 std::string opt_ipath;
403 std::string ipath;
404 std::string opt_ppath;
405 std::string dirname;
406 std::string filename;
407 bool opt_click_allowed = true;
408
409 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
410 // environment variable value
412
413 // Set the default input path
414 if (!env_ipath.empty())
415 ipath = env_ipath;
416
417 // Read the command line options
418 if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) {
419 return EXIT_FAILURE;
420 }
421
422 // Get the option values
423 if (!opt_ipath.empty())
424 ipath = opt_ipath;
425
426 // Compare ipath and env_ipath. If they differ, we take into account
427 // the input path comming from the command line option
428 if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
429 if (ipath != env_ipath) {
430 std::cout << std::endl << "WARNING: " << std::endl;
431 std::cout << " Since -i <visp image path=" << ipath << "> "
432 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
433 << " we skip the environment variable." << std::endl;
434 }
435 }
436
437 // Test if an input path is set
438 if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
439 usage(argv[0], NULL, ipath, opt_ppath);
440 std::cerr << std::endl << "ERROR:" << std::endl;
441 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
442 << " environment variable to specify the location of the " << std::endl
443 << " image path where test images are located." << std::endl
444 << " Use -p <personal image path> option if you want to " << std::endl
445 << " use personal images." << std::endl
446 << std::endl;
447
448 return EXIT_FAILURE;
449 }
450
451 std::ostringstream s;
452
453 if (opt_ppath.empty()) {
454 // Set the path location of the image sequence
455 dirname = vpIoTools::createFilePath(ipath, "mire-2");
456
457 // Build the name of the image file
458
459 s.setf(std::ios::right, std::ios::adjustfield);
460 s << "image.%04d.";
461 s << ext;
462 filename = vpIoTools::createFilePath(dirname, s.str());
463 }
464 else {
465 filename = opt_ppath;
466 }
467
468 // We will read a sequence of images
469 vpVideoReader grabber;
470 grabber.setFirstFrameIndex(1);
471 grabber.setFileName(filename.c_str());
472 // Grey level image associated to a display in the initial pose
473 // computation
474 vpImage<unsigned char> Idisplay;
475 // Grey level image to track points
477 // RGBa image to get background
479 // Matrix representing camera parameters
481
482 // Variables used for pose computation purposes
483 vpPose mPose;
484 vpDot2 md[4];
485 vpImagePoint mcog[4];
486 vpPoint mP[4];
487
488 // CameraParameters we got from calibration
489 // Keep u0 and v0 as center of the screen
491
492 try {
493 vpCTRACE << "Load: " << filename << std::endl;
494 grabber.open(Idisplay);
495 grabber.acquire(Idisplay);
496 vpCameraParameters mcamTmp(592, 570, grabber.getWidth() / 2, grabber.getHeight() / 2);
497 // Compute the initial pose of the camera
498 computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cMo, mP, opt_click_allowed);
499 // Close the framegrabber
500 grabber.close();
501
502 // Associate the grabber to the RGBa image
503 grabber.open(IC);
504 mcam.init(mcamTmp);
505 }
506 catch (...) {
507 std::cerr << std::endl << "ERROR:" << std::endl;
508 std::cerr << " Cannot read " << filename << std::endl;
509 std::cerr << " Check your -i " << ipath << " option " << std::endl
510 << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
511 return EXIT_FAILURE;
512 }
513
514 // Create a vpRAOgre object with color background
515 vpAROgre ogre(mcam, grabber.getWidth(), grabber.getHeight());
516 // Initialize it
517 ogre.init(IC);
518 ogre.load("Robot", "robot.mesh");
519 ogre.setScale("Robot", 0.001f, 0.001f, 0.001f);
520 ogre.setRotation("Robot", vpRotationMatrix(vpRxyzVector(M_PI / 2, -M_PI / 2, 0)));
521
522 // Add an optional point light source
523 Ogre::Light *light = ogre.getSceneManager()->createLight();
524 light->setDiffuseColour(1, 1, 1); // scaled RGB values
525 light->setSpecularColour(1, 1, 1); // scaled RGB values
526 light->setPosition(-5, -5, 10);
527 light->setType(Ogre::Light::LT_POINT);
528
529 // Rendering loop
530 while (ogre.continueRendering() && !grabber.end()) {
531 // Acquire a frame
532 grabber.acquire(IC);
533
534 // Convert it to a grey level image for tracking purpose
536
537 // kill the point list
538 mPose.clearPoint();
539
540 // track the dot
541 for (int i = 0; i < 4; i++) {
542 // track the point
543 md[i].track(I, mcog[i]);
544 md[i].setGrayLevelPrecision(0.90);
545 // pixel->meter conversion
546 {
547 double x = 0, y = 0;
548 vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y);
549 mP[i].set_x(x);
550 mP[i].set_y(y);
551 }
552
553 // and added to the pose computation point list
554 mPose.addPoint(mP[i]);
555 }
556 // the pose structure has been updated
557
558 // the pose is now updated using the virtual visual servoing approach
559 // Dementhon or lagrange is no longer necessary, pose at the
560 // previous iteration is sufficient
562
563 // Display with ogre
564 ogre.display(IC, cMo);
565
566 // Wait so that the video does not go too fast
567 vpTime::wait(15);
568 }
569 // Close the grabber
570 grabber.close();
571 return EXIT_SUCCESS;
572 }
573 catch (const vpException &e) {
574 std::cout << "Catch a ViSP exception: " << e << std::endl;
575 return EXIT_FAILURE;
576 }
577 catch (Ogre::Exception &e) {
578 std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
579 return EXIT_FAILURE;
580 }
581 catch (...) {
582 std::cout << "Catch an exception " << std::endl;
583 return EXIT_FAILURE;
584 }
585}
586#else // VISP_HAVE_OGRE && VISP_HAVE_DISPLAY
587int main()
588{
589#if (!(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)))
590 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
591 << std::endl;
592 std::cout << "Tip if you are on a unix-like system:" << std::endl;
593 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
594 std::cout << "Tip if you are on a windows-like system:" << std::endl;
595 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
596#else
597 std::cout << "You do not have Ogre functionalities" << std::endl;
598 std::cout << "Tip:" << std::endl;
599 std::cout << "- Install Ogre3D, configure again ViSP using cmake and build again this example" << std::endl;
600#endif
601 return EXIT_SUCCESS;
602}
603#endif
Implementation of an augmented reality viewer using Ogre3D 3rd party.
Definition vpAROgre.h:96
Generic class defining intrinsic camera parameters.
void init()
basic initialization with the default parameters
static const vpColor red
Definition vpColor.h:211
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
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 blob (connex pixels with same gray level) on a vpImage.
Definition vpDot2.h:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
void setGraphics(bool activate)
Definition vpDot2.h:311
void display(const vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1) const
Definition vpDot2.cpp:211
void setSizePrecision(const double &sizePrecision)
Definition vpDot2.cpp:745
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition vpDot2.cpp:717
vpImagePoint getCog() const
Definition vpDot2.h:177
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
error that can be emitted by ViSP classes.
Definition vpException.h:59
unsigned int getWidth() const
Return the number of columns in the image.
unsigned int getHeight() const
Return the number of rows in the image.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_i(double ii)
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
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void setFirstFrameIndex(const long first_frame)
#define vpCTRACE
Definition vpDebug.h:333
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)