Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
tutorial-apriltag-detector-live.cpp
1 #include <visp3/core/vpConfig.h>
3 #ifdef VISP_HAVE_MODULE_SENSOR
4 #include <visp3/sensor/vpV4l2Grabber.h>
5 #include <visp3/sensor/vp1394CMUGrabber.h>
6 #include <visp3/sensor/vp1394TwoGrabber.h>
7 #include <visp3/sensor/vpFlyCaptureGrabber.h>
8 #include <visp3/sensor/vpRealSense2.h>
9 #endif
10 #include <visp3/detection/vpDetectorAprilTag.h>
13 #include <visp3/gui/vpDisplayGDI.h>
14 #include <visp3/gui/vpDisplayOpenCV.h>
15 #include <visp3/gui/vpDisplayX.h>
16 #include <visp3/core/vpXmlParserCamera.h>
17 
19 //#undef VISP_HAVE_V4L2
20 //#undef VISP_HAVE_DC1394
21 //#undef VISP_HAVE_CMU1394
22 //#undef VISP_HAVE_FLYCAPTURE
23 //#undef VISP_HAVE_REALSENSE2
24 //#undef VISP_HAVE_OPENCV
26 
27 int main(int argc, const char **argv)
28 {
30 #if defined(VISP_HAVE_APRILTAG) && \
31  (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100) || \
32  defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2) )
33 
35  int opt_device = 0; // For OpenCV and V4l2 grabber to set the camera device
38  double tagSize = 0.053;
39  float quad_decimate = 1.0;
40  int nThreads = 1;
41  std::string intrinsic_file = "";
42  std::string camera_name = "";
43  bool display_tag = false;
44  int color_id = -1;
45  unsigned int thickness = 2;
46 
47 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
48  bool display_off = true;
49  std::cout << "Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
50 #else
51  bool display_off = false;
52 #endif
53 
55 
56  for (int i = 1; i < argc; i++) {
57  if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
58  poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
59  } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
60  tagSize = atof(argv[i + 1]);
61  } else if (std::string(argv[i]) == "--camera_device" && i + 1 < argc) {
62  opt_device = atoi(argv[i + 1]);
63  } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
64  quad_decimate = (float)atof(argv[i + 1]);
65  } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
66  nThreads = atoi(argv[i + 1]);
67  } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
68  intrinsic_file = std::string(argv[i + 1]);
69  } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
70  camera_name = std::string(argv[i + 1]);
71  } else if (std::string(argv[i]) == "--display_tag") {
72  display_tag = true;
73  } else if (std::string(argv[i]) == "--display_off") {
74  display_off = true;
75  } else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
76  color_id = atoi(argv[i+1]);
77  } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
78  thickness = (unsigned int) atoi(argv[i+1]);
79  } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
80  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
81  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
82  std::cout << "Usage: " << argv[0]
83  << " [--camera_device <camera device> (default: 0)]"
84  << " [--tag_size <tag_size in m> (default: 0.053)]"
85  " [--quad_decimate <quad_decimate> (default: 1)]"
86  " [--nthreads <nb> (default: 1)]"
87  " [--intrinsic <intrinsic file> (default: empty)]"
88  " [--camera_name <camera name>] (default: empty)"
89  " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
90  " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
91  " 4: BEST_RESIDUAL_VIRTUAL_VS) (default: 0)]"
92  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT,"
93  " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5) (default: 0)]"
94  " [--display_tag]";
95 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
96  std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
97 #endif
98  std::cout << " [--help]" << std::endl;
99  return EXIT_SUCCESS;
100  }
101  }
102 
103  try {
104  vpCameraParameters cam;
105  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
106 #ifdef VISP_HAVE_XML2
107  vpXmlParserCamera parser;
108  if (!intrinsic_file.empty() && !camera_name.empty())
109  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
110 #endif
111 
113 #if defined(VISP_HAVE_V4L2)
114  vpV4l2Grabber g;
115  std::ostringstream device;
116  device << "/dev/video" << opt_device;
117  std::cout << "Use Video 4 Linux grabber on device " << device.str() << std::endl;
118  g.setDevice(device.str());
119  g.setScale(1);
120  g.open(I);
121 #elif defined(VISP_HAVE_DC1394)
122  (void)opt_device; // To avoid non used warning
123  std::cout << "Use DC1394 grabber" << std::endl;
125  g.open(I);
126 #elif defined(VISP_HAVE_CMU1394)
127  (void)opt_device; // To avoid non used warning
128  std::cout << "Use CMU1394 grabber" << std::endl;
130  g.open(I);
131 #elif defined(VISP_HAVE_FLYCAPTURE)
132  (void)opt_device; // To avoid non used warning
133  std::cout << "Use FlyCapture grabber" << std::endl;
135  g.open(I);
136 #elif defined(VISP_HAVE_REALSENSE2)
137  (void)opt_device; // To avoid non used warning
138  std::cout << "Use Realsense 2 grabber" << std::endl;
139  vpRealSense2 g;
140  rs2::config config;
141  config.disable_stream(RS2_STREAM_DEPTH);
142  config.disable_stream(RS2_STREAM_INFRARED);
143  config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
144  g.open(config);
145  g.acquire(I);
146 
147  std::cout << "Read camera parameters from Realsense device" << std::endl;
149 #elif defined(VISP_HAVE_OPENCV)
150  std::cout << "Use OpenCV grabber on device " << opt_device << std::endl;
151  cv::VideoCapture g(opt_device); // Open the default camera
152  if (!g.isOpened()) { // Check if we succeeded
153  std::cout << "Failed to open the camera" << std::endl;
154  return -1;
155  }
156  cv::Mat frame;
157  g >> frame; // get a new frame from camera
158  vpImageConvert::convert(frame, I);
159 #endif
160 
162  std::cout << "cam:\n" << cam << std::endl;
163  std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
164  std::cout << "tagFamily: " << tagFamily << std::endl;
165 
166  vpDisplay *d = NULL;
167  if (! display_off) {
168 #ifdef VISP_HAVE_X11
169  d = new vpDisplayX(I);
170 #elif defined(VISP_HAVE_GDI)
171  d = new vpDisplayGDI(I);
172 #elif defined(VISP_HAVE_OPENCV)
173  d = new vpDisplayOpenCV(I);
174 #endif
175  }
176 
178  vpDetectorAprilTag detector(tagFamily);
180 
182  detector.setAprilTagQuadDecimate(quad_decimate);
183  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
184  detector.setAprilTagNbThreads(nThreads);
185  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
187 
188  std::vector<double> time_vec;
189  for (;;) {
191 #if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)
192  g.acquire(I);
193 #elif defined(VISP_HAVE_OPENCV)
194  g >> frame;
195  vpImageConvert::convert(frame, I);
196 #endif
197 
200 
201  double t = vpTime::measureTimeMs();
203  std::vector<vpHomogeneousMatrix> cMo_vec;
204  detector.detect(I, tagSize, cam, cMo_vec);
206  t = vpTime::measureTimeMs() - t;
207  time_vec.push_back(t);
208 
209  std::stringstream ss;
210  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
211  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
212 
214  for (size_t i = 0; i < cMo_vec.size(); i++) {
215  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
216  }
218 
219  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
220  vpDisplay::flush(I);
221  if (vpDisplay::getClick(I, false))
222  break;
223  }
224 
225  std::cout << "Benchmark computation time" << std::endl;
226  std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
227  << " ; " << vpMath::getMedian(time_vec) << " ms"
228  << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
229 
230  if (! display_off)
231  delete d;
232 
233  } catch (const vpException &e) {
234  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
235  }
236 
237  return EXIT_SUCCESS;
238 #else
239  (void)argc;
240  (void)argv;
241 #ifndef VISP_HAVE_APRILTAG
242  std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
243 #else
244  std::cout << "Install a 3rd party dedicated to frame grabbing (dc1394, cmu1394, v4l2, OpenCV, FlyCapture, Realsense2), configure and build ViSP again to use this example" << std::endl;
245 #endif
246 #endif
247  return EXIT_SUCCESS;
248 }
void setAprilTagQuadDecimate(const float quadDecimate)
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:171
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static double getStdev(const std::vector< double > &v, const bool useBesselCorrection=false)
Definition: vpMath.cpp:252
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
void open(vpImage< unsigned char > &I)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void open(vpImage< unsigned char > &I)
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
void setDevice(const std::string &devname)
static const vpColor none
Definition: vpColor.h:192
error that can be emited by ViSP classes.
Definition: vpException.h:71
void open(const rs2::config &cfg=rs2::config())
size_t getNbObjects() const
XML parser to load and save intrinsic camera parameters.
static void flush(const vpImage< unsigned char > &I)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
void open(vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:180
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void open(vpImage< unsigned char > &I)
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:202
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
const char * getMessage(void) const
Definition: vpException.cpp:90
void acquire(vpImage< unsigned char > &grey)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Class for firewire ieee1394 video devices using libdc1394-2.x api.
static vpColor getColor(const unsigned int &i)
Definition: vpColor.h:249
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
void setAprilTagNbThreads(const int nThreads)
void setDisplayTag(const bool display, const vpColor &color=vpColor::none, const unsigned int thickness=2)
bool detect(const vpImage< unsigned char > &I)