Visual Servoing Platform  version 3.1.0
vpDetectorAprilTag.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  * Base class for April Tag detection.
33  *
34  *****************************************************************************/
35 #include <visp3/core/vpConfig.h>
36 
37 #ifdef VISP_HAVE_APRILTAG
38 #include <map>
39 
40 #include <apriltag.h>
41 #include <common/homography.h>
42 #include <tag16h5.h>
43 #include <tag25h7.h>
44 #include <tag25h9.h>
45 #include <tag36artoolkit.h>
46 #include <tag36h10.h>
47 #include <tag36h11.h>
48 
49 #include <visp3/core/vpDisplay.h>
50 #include <visp3/core/vpPixelMeterConversion.h>
51 #include <visp3/core/vpPoint.h>
52 #include <visp3/detection/vpDetectorAprilTag.h>
53 #include <visp3/vision/vpPose.h>
54 
55 #ifndef DOXYGEN_SHOULD_SKIP_THIS
56 class vpDetectorAprilTag::Impl
57 {
58 public:
59  Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method)
60  : m_cam(), m_poseEstimationMethod(method), m_tagFamily(tagFamily), m_tagPoses(), m_tagSize(1.0), m_td(NULL),
61  m_tf(NULL)
62  {
63  switch (m_tagFamily) {
64  case TAG_36h11:
65  m_tf = tag36h11_create();
66  break;
67 
68  case TAG_36h10:
69  m_tf = tag36h10_create();
70  break;
71 
72  case TAG_36ARTOOLKIT:
73  m_tf = tag36artoolkit_create();
74  break;
75 
76  case TAG_25h9:
77  m_tf = tag25h9_create();
78  break;
79 
80  case TAG_25h7:
81  m_tf = tag25h7_create();
82  break;
83 
84  case TAG_16h5:
85  m_tf = tag16h5_create();
86  break;
87 
88  default:
89  throw vpException(vpException::fatalError, "Unknow Tag family!");
90  break;
91  }
92 
93  m_td = apriltag_detector_create();
94  apriltag_detector_add_family(m_td, m_tf);
95 
96  m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
97  m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
98  }
99 
100  ~Impl()
101  {
102  apriltag_detector_destroy(m_td);
103 
104  switch (m_tagFamily) {
105  case TAG_36h11:
106  tag36h11_destroy(m_tf);
107  break;
108 
109  case TAG_36h10:
110  tag36h10_destroy(m_tf);
111  break;
112 
113  case TAG_36ARTOOLKIT:
114  tag36artoolkit_destroy(m_tf);
115  break;
116 
117  case TAG_25h9:
118  tag25h9_destroy(m_tf);
119  break;
120 
121  case TAG_25h7:
122  tag25h7_destroy(m_tf);
123  break;
124 
125  case TAG_16h5:
126  tag16h5_destroy(m_tf);
127  break;
128 
129  default:
130  break;
131  }
132  }
133 
134  bool detect(const vpImage<unsigned char> &I, std::vector<std::vector<vpImagePoint> > &polygons,
135  std::vector<std::string> &messages, const bool computePose, const bool displayTag)
136  {
137  m_tagPoses.clear();
138 
139  image_u8_t im = {/*.width =*/(int32_t)I.getWidth(),
140  /*.height =*/(int32_t)I.getHeight(),
141  /*.stride =*/(int32_t)I.getWidth(),
142  /*.buf =*/I.bitmap};
143 
144  zarray_t *detections = apriltag_detector_detect(m_td, &im);
145  int nb_detections = zarray_size(detections);
146  bool detected = nb_detections > 0;
147 
148  polygons.resize((size_t)nb_detections);
149  messages.resize((size_t)nb_detections);
150 
151  for (int i = 0; i < zarray_size(detections); i++) {
152  apriltag_detection_t *det;
153  zarray_get(detections, i, &det);
154 
155  std::vector<vpImagePoint> polygon;
156  for (int j = 0; j < 4; j++) {
157  polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
158  }
159  polygons[i] = polygon;
160  std::stringstream ss;
161  ss << m_tagFamily << " id: " << det->id;
162  messages[i] = ss.str();
163 
164  if (displayTag) {
165  vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[1][1], (int)det->p[1][0],
166  vpColor::red, 2);
167  vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[3][1], (int)det->p[3][0],
168  vpColor::green, 2);
169  vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], (int)det->p[2][1], (int)det->p[2][0],
170  vpColor::blue, 2);
171  vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], (int)det->p[3][1], (int)det->p[3][0],
172  vpColor::yellow, 2);
173  }
174 
175  if (computePose) {
178  double fx = m_cam.get_px(), fy = m_cam.get_py();
179  double cx = m_cam.get_u0(), cy = m_cam.get_v0();
180 
181  matd_t *M = homography_to_pose(det->H, fx, fy, cx, cy, m_tagSize / 2);
182 
183  for (int i = 0; i < 3; i++) {
184  for (int j = 0; j < 3; j++) {
185  cMo[i][j] = MATD_EL(M, i, j);
186  }
187  cMo[i][3] = MATD_EL(M, i, 3);
188  }
189 
190  matd_destroy(M);
191  }
192 
193  // Add marker object points
194  vpPoint pt;
195  vpImagePoint imPt;
196  double x = 0.0, y = 0.0;
197  std::vector<vpPoint> pts(4);
198  pt.setWorldCoordinates(-m_tagSize / 2.0, -m_tagSize / 2.0, 0.0);
199  imPt.set_uv(det->p[0][0], det->p[0][1]);
200  vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
201  pt.set_x(x);
202  pt.set_y(y);
203  pts[0] = pt;
204 
205  pt.setWorldCoordinates(m_tagSize / 2.0, -m_tagSize / 2.0, 0.0);
206  imPt.set_uv(det->p[1][0], det->p[1][1]);
207  vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
208  pt.set_x(x);
209  pt.set_y(y);
210  pts[1] = pt;
211 
212  pt.setWorldCoordinates(m_tagSize / 2.0, m_tagSize / 2.0, 0.0);
213  imPt.set_uv(det->p[2][0], det->p[2][1]);
214  vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
215  pt.set_x(x);
216  pt.set_y(y);
217  pts[2] = pt;
218 
219  pt.setWorldCoordinates(-m_tagSize / 2.0, m_tagSize / 2.0, 0.0);
220  imPt.set_uv(det->p[3][0], det->p[3][1]);
221  vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
222  pt.set_x(x);
223  pt.set_y(y);
224  pts[3] = pt;
225 
226  vpPose pose;
227  pose.addPoints(pts);
228 
231  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange, cMo_homography = cMo;
232 
233  double residual_dementhon = std::numeric_limits<double>::max(),
234  residual_lagrange = std::numeric_limits<double>::max();
235  double residual_homography = pose.computeResidual(cMo_homography);
236 
237  if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
238  residual_dementhon = pose.computeResidual(cMo_dementhon);
239  }
240 
241  if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
242  residual_lagrange = pose.computeResidual(cMo_lagrange);
243  }
244 
245  if (residual_dementhon < residual_lagrange) {
246  if (residual_dementhon < residual_homography) {
247  cMo = cMo_dementhon;
248  } else {
249  cMo = cMo_homography;
250  }
251  } else if (residual_lagrange < residual_homography) {
252  cMo = cMo_lagrange;
253  } else {
254  // cMo = cMo_homography; //already the case
255  }
256  } else {
257  pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
258  }
259  }
260 
261  // Compute final pose using VVS
262  pose.computePose(vpPose::VIRTUAL_VS, cMo);
263  m_tagPoses.push_back(cMo);
264  }
265  }
266 
267  apriltag_detections_destroy(detections);
268 
269  return detected;
270  }
271 
272  void getTagPoses(std::vector<vpHomogeneousMatrix> &tagPoses) const { tagPoses = m_tagPoses; }
273 
274  void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; }
275 
276  void setNbThreads(const int nThreads) { m_td->nthreads = nThreads; }
277 
278  void setQuadDecimate(const float quadDecimate) { m_td->quad_decimate = quadDecimate; }
279 
280  void setQuadSigma(const float quadSigma) { m_td->quad_sigma = quadSigma; }
281 
282  void setRefineDecode(const bool refineDecode) { m_td->refine_decode = refineDecode ? 1 : 0; }
283 
284  void setRefineEdges(const bool refineEdges) { m_td->refine_edges = refineEdges ? 1 : 0; }
285 
286  void setRefinePose(const bool refinePose) { m_td->refine_pose = refinePose ? 1 : 0; }
287 
288  void setTagSize(const double tagSize) { m_tagSize = tagSize; }
289 
290  void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; }
291 
292 protected:
293  vpCameraParameters m_cam;
294  std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
297  std::vector<vpHomogeneousMatrix> m_tagPoses;
298  double m_tagSize;
299  apriltag_detector_t *m_td;
300  apriltag_family_t *m_tf;
301 };
302 #endif // DOXYGEN_SHOULD_SKIP_THIS
303 
308  const vpPoseEstimationMethod &poseEstimationMethod)
309  : m_displayTag(false), m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily),
310  m_impl(new Impl(tagFamily, poseEstimationMethod))
311 {
312 }
313 
318 
327 {
328  m_message.clear();
329  m_polygon.clear();
330  m_nb_objects = 0;
331 
332  bool detected = m_impl->detect(I, m_polygon, m_message, false, m_displayTag);
333  m_nb_objects = m_message.size();
334 
335  return detected;
336 }
337 
346 bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I, const double tagSize, const vpCameraParameters &cam,
347  std::vector<vpHomogeneousMatrix> &cMo_vec)
348 {
349  m_message.clear();
350  m_polygon.clear();
351  m_nb_objects = 0;
352 
353  m_impl->setTagSize(tagSize);
354  m_impl->setCameraParameters(cam);
355  bool detected = m_impl->detect(I, m_polygon, m_message, true, m_displayTag);
356  m_nb_objects = m_message.size();
357  m_impl->getTagPoses(cMo_vec);
358 
359  return detected;
360 }
361 
368 {
369  if (nThreads > 0)
370  m_impl->setNbThreads(nThreads);
371 }
372 
379 {
380  m_poseEstimationMethod = poseEstimationMethod;
381  m_impl->setPoseEstimationMethod(poseEstimationMethod);
382 }
383 
396 void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); }
397 
410 void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma) { m_impl->setQuadSigma(quadSigma); }
411 
424 void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) { m_impl->setRefineDecode(refineDecode); }
425 
440 void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges) { m_impl->setRefineEdges(refineEdges); }
441 
458 void vpDetectorAprilTag::setAprilTagRefinePose(const bool refinePose) { m_impl->setRefinePose(refinePose); }
459 #elif !defined(VISP_BUILD_SHARED_LIBS)
460 // Work arround to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has
461 // no symbols
462 void dummy_vpDetectorAprilTag() {}
463 #endif
void setAprilTagQuadDecimate(const float quadDecimate)
void setAprilTagRefineEdges(const bool refineEdges)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Type * bitmap
points toward the bitmap
Definition: vpImage.h:133
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:152
error that can be emited by ViSP classes.
Definition: vpException.h:71
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
static const vpColor green
Definition: vpColor.h:183
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
size_t m_nb_objects
Number of detected objects.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:79
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
Definition: vpPose.cpp:362
vpPoseEstimationMethod m_poseEstimationMethod
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
unsigned int getHeight() const
Definition: vpImage.h:178
void setAprilTagRefineDecode(const bool refineDecode)
std::vector< std::vector< vpImagePoint > > m_polygon
vpDetectorAprilTag(const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS)
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:248
void setAprilTagQuadSigma(const float quadSigma)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix &#39;cMo&#39;.
Definition: vpPose.cpp:324
static const vpColor yellow
Definition: vpColor.h:188
unsigned int getWidth() const
Definition: vpImage.h:229
std::vector< std::string > m_message
Message attached to each object.
vpAprilTagFamily m_tagFamily
void setAprilTagRefinePose(const bool refinePose)
void setAprilTagNbThreads(const int nThreads)
static const vpColor blue
Definition: vpColor.h:186
bool detect(const vpImage< unsigned char > &I)