2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpOccipitalStructure.h>
7#include <visp3/detection/vpDetectorAprilTag.h>
9#include <visp3/core/vpImageConvert.h>
10#include <visp3/core/vpXmlParserCamera.h>
11#include <visp3/gui/vpDisplayFactory.h>
12#include <visp3/vision/vpPose.h>
14void usage(
const char **argv,
int error);
16void usage(
const char **argv,
int error)
18 std::cout <<
"Synopsis" << std::endl
20 <<
" [--tag-size <size>]"
21 <<
" [--tag-family <family>]"
22 <<
" [--tag-decision-margin-threshold <threshold>]"
23 <<
" [--tag-hamming-distance-threshold <threshold>]"
24 <<
" [--tag-quad-decimate <factor>]"
25 <<
" [--tag-n-threads <number>]"
26 <<
" [--tag-z-aligned]"
27 <<
" [--tag-pose-method <method>]"
28#if defined(VISP_HAVE_DISPLAY)
32 <<
" [--thickness <thickness>"
35 <<
" [--help, -h]" << std::endl
37 std::cout <<
"Description" << std::endl
38 <<
" Compute the pose of an Apriltag in images acquired with a structure core camera." << std::endl
40 <<
" --tag-size <size>" << std::endl
41 <<
" Apriltag size in [m]." << std::endl
42 <<
" Default: 0.03" << std::endl
44 <<
" --tag-family <family>" << std::endl
45 <<
" Apriltag family. Supported values are:" << std::endl
46 <<
" 0: TAG_36h11" << std::endl
47 <<
" 1: TAG_36h10 (DEPRECATED)" << std::endl
48 <<
" 2: TAG_36ARTOOLKIT (DEPRECATED)" << std::endl
49 <<
" 3: TAG_25h9" << std::endl
50 <<
" 4: TAG_25h7 (DEPRECATED)" << std::endl
51 <<
" 5: TAG_16h5" << std::endl
52 <<
" 6: TAG_CIRCLE21h7" << std::endl
53 <<
" 7: TAG_CIRCLE49h12" << std::endl
54 <<
" 8: TAG_CUSTOM48h12" << std::endl
55 <<
" 9: TAG_STANDARD41h12" << std::endl
56 <<
" 10: TAG_STANDARD52h13" << std::endl
57 <<
" 11: TAG_ARUCO_4x4_50" << std::endl
58 <<
" 12: TAG_ARUCO_4x4_100" << std::endl
59 <<
" 13: TAG_ARUCO_4x4_250" << std::endl
60 <<
" 14: TAG_ARUCO_4x4_1000" << std::endl
61 <<
" 15: TAG_ARUCO_5x5_50" << std::endl
62 <<
" 16: TAG_ARUCO_5x5_100" << std::endl
63 <<
" 17: TAG_ARUCO_5x5_250" << std::endl
64 <<
" 18: TAG_ARUCO_5x5_1000" << std::endl
65 <<
" 19: TAG_ARUCO_6x6_50" << std::endl
66 <<
" 20: TAG_ARUCO_6x6_100" << std::endl
67 <<
" 21: TAG_ARUCO_6x6_250" << std::endl
68 <<
" 22: TAG_ARUCO_6x6_1000" << std::endl
69 <<
" 23: TAG_ARUCO_7x7_50" << std::endl
70 <<
" 24: TAG_ARUCO_7x7_100" << std::endl
71 <<
" 25: TAG_ARUCO_7x7_250" << std::endl
72 <<
" 26: TAG_ARUCO_7x7_1000" << std::endl
73 <<
" 27: TAG_ARUCO_MIP_36h12" << std::endl
74 <<
" Default: 0 (36h11)" << std::endl
76 <<
" --tag-decision-margin-threshold <threshold>" << std::endl
77 <<
" Threshold used to discard low-confident detections. A typical value is " << std::endl
78 <<
" around 100. The higher this value, the more false positives will be filtered" << std::endl
79 <<
" out. When this value is set to -1, false positives are not filtered out." << std::endl
80 <<
" Default: 50" << std::endl
82 <<
" --tag-hamming-distance-threshold <threshold>" << std::endl
83 <<
" Threshold used to discard low-confident detections with corrected bits." << std::endl
84 <<
" A typical value is between 0 and 3. The lower this value, the more false" << std::endl
85 <<
" positives will be filtered out." << std::endl
86 <<
" Default: 0" << std::endl
88 <<
" --tag-quad-decimate <factor>" << std::endl
89 <<
" Decimation factor used to detect a tag. " << std::endl
90 <<
" Default: 1" << std::endl
92 <<
" --tag-n-threads <number>" << std::endl
93 <<
" Number of threads used to detect a tag." << std::endl
94 <<
" Default: 1" << std::endl
96 <<
" --tag-z-aligned" << std::endl
97 <<
" When enabled, tag z-axis and camera z-axis are aligned." << std::endl
98 <<
" Default: false" << std::endl
100 <<
" --tag-pose-method <method>" << std::endl
101 <<
" Algorithm used to compute the tag pose from its 4 corners." << std::endl
102 <<
" Possible values are:" << std::endl
103 <<
" 0: HOMOGRAPHY" << std::endl
104 <<
" 1: HOMOGRAPHY_VIRTUAL_VS" << std::endl
105 <<
" 2: DEMENTHON_VIRTUAL_VS" << std::endl
106 <<
" 3: LAGRANGE_VIRTUAL_VS" << std::endl
107 <<
" 4: BEST_RESIDUAL_VIRTUAL_VS" << std::endl
108 <<
" 5: HOMOGRAPHY_ORTHOGONAL_ITERATION" << std::endl
109 <<
" Default: 1 (HOMOGRAPHY_VIRTUAL_VS)" << std::endl
111#if defined(VISP_HAVE_DISPLAY)
112 <<
" --display-tag" << std::endl
113 <<
" Flag used to enable displaying the edges of a tag." << std::endl
114 <<
" Default: disabled" << std::endl
116 <<
" --display-off" << std::endl
117 <<
" Flag used to turn display off." << std::endl
118 <<
" Default: enabled" << std::endl
120 <<
" --color <id>" << std::endl
121 <<
" Color id used to display the frame over each tag." << std::endl
122 <<
" Possible values are:" << std::endl
123 <<
" -1: R-G-B colors for X, Y, Z axis respectively" << std::endl
124 <<
" 0: all axis in black" << std::endl
125 <<
" 1: all axis in white" << std::endl
126 <<
" ..." << std::endl
127 <<
" Default: -1" << std::endl
129 <<
" --thickness <thickness>" << std::endl
130 <<
" Thickness of the drawings in overlay." << std::endl
131 <<
" Default: 2" << std::endl
134 <<
" --verbose, -v" << std::endl
135 <<
" Enable extra verbosity." << std::endl
137 <<
" --help, -h" << std::endl
138 <<
" Print this helper message." << std::endl
142 std::cout <<
"Error" << std::endl
144 <<
"Unsupported parameter " << argv[
error] << std::endl;
148int main(
int argc,
const char **argv)
151#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_THREADS)
153#ifdef ENABLE_VISP_NAMESPACE
159 double opt_tag_size = 0.053;
160 float opt_tag_quad_decimate = 1.0;
161 float opt_tag_decision_margin_threshold = 50;
162 float opt_tag_hamming_distance_threshold = 2;
163 int opt_tag_nThreads = 1;
164 bool opt_display_tag =
false;
165 int opt_color_id = -1;
166 unsigned int opt_thickness = 2;
167 bool opt_tag_z_align_frame =
false;
169#if !(defined(VISP_HAVE_DISPLAY))
170 bool display_off =
true;
171 std::cout <<
"Warning: There is no 3rd party (X11, GDI or openCV) to display images..." << std::endl;
173 bool display_off =
false;
176 for (
int i = 1;
i < argc;
i++) {
177 if (std::string(argv[i]) ==
"--tag-size" && i + 1 < argc) {
178 opt_tag_size = atof(argv[++i]);
180 else if (std::string(argv[i]) ==
"--tag-family" && i + 1 < argc) {
183 else if (std::string(argv[i]) ==
"--tag-quad-decimate" && i + 1 < argc) {
184 opt_tag_quad_decimate =
static_cast<float>(atof(argv[++i]));
186 else if (std::string(argv[i]) ==
"--tag-n-threads" && i + 1 < argc) {
187 opt_tag_nThreads = atoi(argv[++i]);
189 else if (std::string(argv[i]) ==
"--tag-z-aligned") {
190 opt_tag_z_align_frame =
true;
192 else if (std::string(argv[i]) ==
"--tag-pose-method" && i + 1 < argc) {
195 else if (std::string(argv[i]) ==
"--tag-decision-margin-threshold" && i + 1 < argc) {
196 opt_tag_decision_margin_threshold =
static_cast<float>(atof(argv[++i]));
198 else if (std::string(argv[i]) ==
"--tag-hamming-distance-threshold" && i + 1 < argc) {
199 opt_tag_hamming_distance_threshold = atoi(argv[++i]);
201#if defined(VISP_HAVE_DISPLAY)
202 else if (std::string(argv[i]) ==
"--display-tag") {
203 opt_display_tag =
true;
205 else if (std::string(argv[i]) ==
"--display-off") {
208 else if (std::string(argv[i]) ==
"--color" && i + 1 < argc) {
209 opt_color_id = atoi(argv[++i]);
211 else if (std::string(argv[i]) ==
"--thickness" && i + 1 < argc) {
212 opt_thickness =
static_cast<unsigned int>(atoi(argv[++i]));
215 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
225#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
226 std::shared_ptr<vpDisplay> d1, d2, d3;
235 std::cout <<
"Use Occipital Structure grabber" << std::endl;
237 ST::CaptureSessionSettings settings;
238 settings.source = ST::CaptureSessionSourceId::StructureCore;
239 settings.structureCore.visibleEnabled =
true;
240 settings.applyExpensiveCorrection =
true;
250 std::cout <<
"I_color: " << I_color.getWidth() <<
" " << I_color.getHeight() << std::endl;
251 std::cout <<
"I_depth_raw: " << I_depth_raw.getWidth() <<
" " << I_depth_raw.getHeight() << std::endl;
253 g.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap));
255 std::cout <<
"Read camera parameters from Structure core device" << std::endl;
260 std::cout <<
cam << std::endl;
261 std::cout <<
"Tag detector settings" << std::endl;
262 std::cout <<
" Tag size [m] : " << opt_tag_size << std::endl;
263 std::cout <<
" Tag family : " << opt_tag_family << std::endl;
264 std::cout <<
" Quad decimate : " << opt_tag_quad_decimate << std::endl;
265 std::cout <<
" Decision margin threshold : " << opt_tag_decision_margin_threshold << std::endl;
266 std::cout <<
" Hamming distance threshold: " << opt_tag_hamming_distance_threshold << std::endl;
267 std::cout <<
" Num threads : " << opt_tag_nThreads << std::endl;
268 std::cout <<
" Z aligned : " << opt_tag_z_align_frame << std::endl;
269 std::cout <<
" Pose estimation : " << opt_tag_pose_estimation_method << std::endl;
276#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
292 detector.setAprilTagQuadDecimate(opt_tag_quad_decimate);
293 detector.setAprilTagPoseEstimationMethod(opt_tag_pose_estimation_method);
294 detector.setAprilTagNbThreads(opt_tag_nThreads);
296 detector.setZAlignedWithCameraAxis(opt_tag_z_align_frame);
297 detector.setAprilTagDecisionMarginThreshold(opt_tag_decision_margin_threshold);
298 detector.setAprilTagHammingDistanceThreshold(opt_tag_hamming_distance_threshold);
300 std::vector<double> time_vec;
305 g.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
306 reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap));
313 depthMap.
resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
314#ifdef VISP_HAVE_OPENMP
315#pragma omp parallel for
317 for (
int i = 0; i < static_cast<int>(I_depth_raw.getHeight());
i++) {
318 for (
int j = 0; j < static_cast<int>(I_depth_raw.getWidth());
j++) {
320 float Z = I_depth_raw[
i][
j] * 0.001;
333 std::vector<vpHomogeneousMatrix> cMo_vec;
334 detector.detect(I, opt_tag_size, cam, cMo_vec);
337 for (
size_t i = 0;
i < cMo_vec.size();
i++) {
342 std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
343 std::vector<int> tags_id = detector.getTagsId();
344 std::map<int, double> tags_size;
345 tags_size[-1] = opt_tag_size;
346 std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
347 for (
size_t i = 0;
i < tags_corners.size();
i++) {
349 double confidence_index;
351 &confidence_index)) {
352 if (confidence_index > 0.5) {
355 else if (confidence_index > 0.25) {
361 std::stringstream ss;
362 ss <<
"Tag id " << tags_id[
i] <<
" confidence: " << confidence_index;
372 time_vec.push_back(t);
374 std::stringstream ss;
375 ss <<
"Detection time: " <<
t <<
" ms for " << detector.getNbObjects() <<
" tags";
386 std::cout <<
"Benchmark loop processing time" << std::endl;
387 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
392 std::cerr <<
"Catch an exception: " <<
e.getMessage() << std::endl;
394#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
412#ifndef VISP_HAVE_APRILTAG
413 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
415 std::cout <<
"Install Structure Core SDK, configure and build ViSP again to use this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static vpColor getColor(const unsigned int &i)
static const vpColor none
static const vpColor orange
@ TAG_36h11
AprilTag 36h11 pattern (recommended).
Class that defines generic functionalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
static bool isNaN(double value)
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static double getMean(const std::vector< double > &v)
vpCameraParameters getCameraParameters(const vpOccipitalStructureStream stream_type, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithoutDistortion)
void acquire(vpImage< unsigned char > &gray, bool undistorted=false, double *ts=nullptr)
bool open(const ST::CaptureSessionSettings &settings)
static bool computePlanarObjectPoseFromRGBD(const vpImage< float > &depthMap, const std::vector< vpImagePoint > &corners, const vpCameraParameters &colorIntrinsics, const std::vector< vpPoint > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index=nullptr)
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()