Visual Servoing Platform  version 3.1.0
testAprilTag.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Test AprilTag detection.
33  *
34  *****************************************************************************/
41 #include <iostream>
42 #include <map>
43 #include <visp3/core/vpDisplay.h>
44 #include <visp3/core/vpIoTools.h>
45 #include <visp3/detection/vpDetectorAprilTag.h>
46 #include <visp3/gui/vpDisplayGDI.h>
47 #include <visp3/gui/vpDisplayOpenCV.h>
48 #include <visp3/gui/vpDisplayX.h>
49 #include <visp3/io/vpImageIo.h>
50 #include <visp3/io/vpParseArgv.h>
51 
52 #if defined(VISP_HAVE_APRILTAG)
53 
54 // List of allowed command line options
55 #define GETOPTARGS "cdi:p:h"
56 
57 namespace
58 {
59 /*
60  Print the program options.
61 
62  \param name : Program name.
63  \param badparam : Bad parameter name.
64  \param ipath: Input image path.
65  */
66 void usage(const char *name, const char *badparam, std::string ipath)
67 {
68  fprintf(stdout, "\n\
69  Test AprilTag detection.\n\
70  \n\
71  SYNOPSIS\n\
72  %s [-c] [-d] [-i <input image path>] [-p <personal image path>]\n\
73  [-h]\n \
74  ", name);
75 
76  fprintf(stdout, "\n\
77  OPTIONS: Default\n\
78  -i <input image path> %s\n\
79  Set image input path.\n\
80  From this path read \"AprilTag/AprilTag.pgm image.\n\
81  Setting the VISP_INPUT_IMAGE_PATH environment\n\
82  variable produces the same behaviour than using\n\
83  this option.\n\
84  \n\
85  -p <personal image path> \n\
86  Path to an image used to test image reading function.\n\
87  Example: -p /my_path_to/image.png\n\
88  \n\
89  -c \n\
90  Disable the mouse click. Useful to automaze the \n\
91  execution of this program without human intervention.\n\
92  \n\
93  -d \n\
94  Turn off the display.\n\
95  \n\
96  -h\n\
97  Print the help.\n\n", ipath.c_str());
98 
99  if (badparam)
100  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
101 }
102 
114 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed, bool &display)
115 {
116  const char *optarg_;
117  int c;
118  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
119 
120  switch (c) {
121  case 'i':
122  ipath = optarg_;
123  break;
124  case 'p':
125  ppath = optarg_;
126  break;
127  case 'h':
128  usage(argv[0], NULL, ipath);
129  return false;
130  break;
131  case 'c':
132  click_allowed = false;
133  break;
134  case 'd':
135  display = false;
136  break;
137 
138  default:
139  usage(argv[0], optarg_, ipath);
140  return false;
141  break;
142  }
143  }
144 
145  if ((c == 1) || (c == -1)) {
146  // standalone param or error
147  usage(argv[0], NULL, ipath);
148  std::cerr << "ERROR: " << std::endl;
149  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
150  return false;
151  }
152 
153  return true;
154 }
155 
156 struct TagGroundTruth {
157  std::string message;
158  std::vector<vpImagePoint> corners;
159 
160  TagGroundTruth(const std::string &msg, const std::vector<vpImagePoint> &c) : message(msg), corners(c) {}
161 
162  bool operator==(const TagGroundTruth &b) const
163  {
164  if (message != b.message || corners.size() != b.corners.size())
165  return false;
166 
167  for (size_t i = 0; i < corners.size(); i++) {
168  // Allow 0.5 pixel of difference
169  if (!vpMath::equal(corners[i].get_u(), b.corners[i].get_u(), 0.5) ||
170  !vpMath::equal(corners[i].get_v(), b.corners[i].get_v(), 0.5)) {
171  return false;
172  }
173  }
174 
175  return true;
176  }
177 
178  bool operator!=(const TagGroundTruth &b) const { return !(*this == b); }
179 };
180 
181 std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
182 {
183  os << t.message << std::endl;
184  for (size_t i = 0; i < t.corners.size(); i++)
185  os << t.corners[i] << std::endl;
186 
187  return os;
188 }
189 }
190 
191 int main(int argc, const char *argv[])
192 {
193  try {
194  std::string env_ipath;
195  std::string opt_ipath;
196  std::string opt_ppath;
197  std::string ipath;
198  std::string filename;
199  bool opt_click_allowed = true;
200  bool opt_display = true;
201 
202  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
203  // environment variable value
204  env_ipath = vpIoTools::getViSPImagesDataPath();
205 
206  // Set the default input path
207  if (!env_ipath.empty())
208  ipath = env_ipath;
209 
210  // Read the command line options
211  if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) == false) {
212  exit(EXIT_FAILURE);
213  }
214 
215  // Get the option values
216  if (!opt_ipath.empty())
217  ipath = opt_ipath;
218 
219  // Compare ipath and env_ipath. If they differ, we take into account
220  // the input path comming from the command line option
221  if (!opt_ipath.empty() && !env_ipath.empty()) {
222  if (ipath != env_ipath) {
223  std::cout << std::endl << "WARNING: " << std::endl;
224  std::cout << " Since -i <visp image path=" << ipath << "> "
225  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
226  << " we skip the environment variable." << std::endl;
227  }
228  }
229 
230  //
231  // Here starts really the test
232  //
233 
235  if (opt_ppath.empty()) {
236  filename = vpIoTools::createFilePath(ipath, "AprilTag/AprilTag.pgm");
237  } else {
238  filename = opt_ppath;
239  }
240 
241  if (!vpIoTools::checkFilename(filename)) {
242  std::cerr << "Filename: " << filename << " does not exist." << std::endl;
243  return EXIT_SUCCESS;
244  }
245  vpImageIo::read(I, filename);
246 
247 #ifdef VISP_HAVE_X11
248  vpDisplayX d;
249 #elif defined(VISP_HAVE_GDI)
250  vpDisplayGDI d;
251 #elif defined(VISP_HAVE_OPENCV)
252  vpDisplayOpenCV d;
253 #else
254  opt_display = false;
255 #endif
256 
259  double tagSize = 0.053;
260  float quad_decimate = 1.0;
261  int nThreads = 1;
262  bool display_tag = true;
263 
264  vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
265  dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
266  dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
267  dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagNbThreads(nThreads);
268  dynamic_cast<vpDetectorAprilTag *>(detector)->setDisplayTag(display_tag);
269 
270  vpCameraParameters cam;
271  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
272 
273  if (opt_display) {
274 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
275  d.init(I, 0, 0, "AprilTag detection");
276 #endif
278  }
279 
280  std::vector<vpHomogeneousMatrix> cMo_vec;
281  dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
282 
283  // Ground truth
284  std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
285  bool use_detection_ground_truth = false;
286  {
287  std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_detection.txt");
288  std::ifstream file_ground_truth(filename_ground_truth.c_str());
289  if (file_ground_truth.is_open() && opt_ppath.empty()) {
290  use_detection_ground_truth = true;
291 
292  std::string message = "";
293  double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0;
294  double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0;
295  while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) {
296  std::vector<vpImagePoint> tagCorners(4);
297  tagCorners[0].set_ij(v1, u1);
298  tagCorners[1].set_ij(v2, u2);
299  tagCorners[2].set_ij(v3, u3);
300  tagCorners[3].set_ij(v4, u4);
301  mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners)));
302  }
303  }
304  }
305 
306  std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
307  bool use_pose_ground_truth = false;
308  {
309  std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_pose.txt");
310  std::ifstream file_ground_truth(filename_ground_truth.c_str());
311  if (file_ground_truth.is_open() && opt_ppath.empty()) {
312  use_pose_ground_truth = true;
313 
314  std::string message = "";
315  double tx = 0.0, ty = 0.0, tz = 0.0;
316  double tux = 0.0, tuy = 0.0, tuz = 0.0;
317  while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) {
318  mapOfPosesGroundTruth.insert(std::make_pair(message, vpPoseVector(tx, ty, tz, tux, tuy, tuz)));
319  }
320  }
321  }
322 
323  for (size_t i = 0; i < detector->getNbObjects(); i++) {
324  std::vector<vpImagePoint> p = detector->getPolygon(i);
325 
326  if (use_detection_ground_truth) {
327  std::string message = detector->getMessage(i);
328  std::replace(message.begin(), message.end(), ' ', '_');
329  std::map<std::string, TagGroundTruth>::iterator it = mapOfTagsGroundTruth.find(message);
330  TagGroundTruth current(message, p);
331  if (it == mapOfTagsGroundTruth.end()) {
332  std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
333  return EXIT_FAILURE;
334  } else if (it->second != current) {
335  std::cerr << "Problem, current detection:\n" << current << "\nGround truth:\n" << it->second << std::endl;
336  return EXIT_FAILURE;
337  }
338  }
339 
340  if (opt_display) {
341  vpRect bbox = detector->getBBox(i);
343  vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), detector->getMessage(i),
344  vpColor::red);
345  }
346  }
347 
348  if (opt_display) {
349  vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
350  vpDisplay::flush(I);
351  if (opt_click_allowed)
353 
355  }
356 
357  for (size_t i = 0; i < cMo_vec.size(); i++) {
358  if (opt_display)
359  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
360 
361  if (use_pose_ground_truth) {
362  vpPoseVector pose_vec(cMo_vec[i]);
363 
364  std::string message = detector->getMessage(i);
365  std::replace(message.begin(), message.end(), ' ', '_');
366  std::map<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
367  if (it == mapOfPosesGroundTruth.end()) {
368  std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl;
369  return EXIT_FAILURE;
370  } else {
371  for (unsigned int cpt = 0; cpt < 6; cpt++) {
372  if (!vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005)) {
373  std::cerr << "Problem, current pose: " << pose_vec.t() << "\nGround truth pose: " << it->second.t()
374  << std::endl;
375  return EXIT_FAILURE;
376  }
377  }
378  }
379  }
380  }
381 
382  if (opt_display) {
383  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
384  vpDisplay::flush(I);
385  if (opt_click_allowed)
387  }
388 
389  delete detector;
390  } catch (const vpException &e) {
391  std::cerr << "Catch an exception: " << e.what() << std::endl;
392  return EXIT_FAILURE;
393  }
394 
395  std::cout << "\ntestAprilTag is ok." << std::endl;
396  return EXIT_SUCCESS;
397 }
398 #else
399 int main()
400 {
401  std::cout << "Need ViSP AprilTag." << std::endl;
402  return 0;
403 }
404 #endif
vpRect getBBox(size_t i) const
double getTop() const
Definition: vpRect.h:175
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1214
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:290
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
static const vpColor none
Definition: vpColor.h:192
error that can be emited by ViSP classes.
Definition: vpException.h:71
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
size_t getNbObjects() const
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:180
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:577
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1439
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.
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
double getLeft() const
Definition: vpRect.h:156
const char * what() const
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:207
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))
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
Defines a rectangle in the plane.
Definition: vpRect.h:78
std::vector< std::string > & getMessage()
std::vector< std::vector< vpImagePoint > > & getPolygon()