Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
tutorial-apriltag-detector-live-rgbd-structure-core.cpp
1
2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpOccipitalStructure.h>
5#endif
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>
13
14void usage(const char **argv, int error);
15
16void usage(const char **argv, int error)
17{
18 std::cout << "Synopsis" << std::endl
19 << " " << argv[0]
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)
29 << " [--display-tag]"
30 << " [--display-off]"
31 << " [--color <id>]"
32 << " [--thickness <thickness>"
33#endif
34 << " [--verbose, -v]"
35 << " [--help, -h]" << std::endl
36 << 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
39 << std::endl
40 << " --tag-size <size>" << std::endl
41 << " Apriltag size in [m]." << std::endl
42 << " Default: 0.03" << std::endl
43 << 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
75 << 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
81 << 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
87 << std::endl
88 << " --tag-quad-decimate <factor>" << std::endl
89 << " Decimation factor used to detect a tag. " << std::endl
90 << " Default: 1" << std::endl
91 << 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
95 << 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
99 << 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
110 << 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
115 << std::endl
116 << " --display-off" << std::endl
117 << " Flag used to turn display off." << std::endl
118 << " Default: enabled" << std::endl
119 << 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
128 << std::endl
129 << " --thickness <thickness>" << std::endl
130 << " Thickness of the drawings in overlay." << std::endl
131 << " Default: 2" << std::endl
132 << std::endl
133#endif
134 << " --verbose, -v" << std::endl
135 << " Enable extra verbosity." << std::endl
136 << std::endl
137 << " --help, -h" << std::endl
138 << " Print this helper message." << std::endl
139 << std::endl;
140
141 if (error) {
142 std::cout << "Error" << std::endl
143 << " "
144 << "Unsupported parameter " << argv[error] << std::endl;
145 }
146}
147
148int main(int argc, const char **argv)
149{
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
154 using namespace VISP_NAMESPACE_NAME;
155#endif
156
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;
168
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;
172#else
173 bool display_off = false;
174#endif
175
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]);
179 }
180 else if (std::string(argv[i]) == "--tag-family" && i + 1 < argc) {
181 opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[++i]);
182 }
183 else if (std::string(argv[i]) == "--tag-quad-decimate" && i + 1 < argc) {
184 opt_tag_quad_decimate = static_cast<float>(atof(argv[++i]));
185 }
186 else if (std::string(argv[i]) == "--tag-n-threads" && i + 1 < argc) {
187 opt_tag_nThreads = atoi(argv[++i]);
188 }
189 else if (std::string(argv[i]) == "--tag-z-aligned") {
190 opt_tag_z_align_frame = true;
191 }
192 else if (std::string(argv[i]) == "--tag-pose-method" && i + 1 < argc) {
193 opt_tag_pose_estimation_method = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[++i]);
194 }
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]));
197 }
198 else if (std::string(argv[i]) == "--tag-hamming-distance-threshold" && i + 1 < argc) {
199 opt_tag_hamming_distance_threshold = atoi(argv[++i]);
200 }
201#if defined(VISP_HAVE_DISPLAY)
202 else if (std::string(argv[i]) == "--display-tag") {
203 opt_display_tag = true;
204 }
205 else if (std::string(argv[i]) == "--display-off") {
206 display_off = true;
207 }
208 else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
209 opt_color_id = atoi(argv[++i]);
210 }
211 else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
212 opt_thickness = static_cast<unsigned int>(atoi(argv[++i]));
213 }
214#endif
215 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
216 usage(argv, 0);
217 return EXIT_SUCCESS;
218 }
219 else {
220 usage(argv, i);
221 return EXIT_FAILURE;
222 }
223 }
224
225#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
226 std::shared_ptr<vpDisplay> d1, d2, d3;
227#else
228 vpDisplay *d1 = nullptr;
229 vpDisplay *d2 = nullptr;
230 vpDisplay *d3 = nullptr;
231#endif
232
233 try {
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; // Apply a correction and clean filter to the depth before streaming.
241 unsigned int width = 640, height = 480;
242
244 vpImage<vpRGBa> I_color(height, width);
245 vpImage<float> I_depth_raw(height, width);
247
248 g.open(settings);
249
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;
252
253 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
254
255 std::cout << "Read camera parameters from Structure core device" << std::endl;
259
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;
270
271 vpImage<vpRGBa> I_color2 = I_color;
272 vpImage<float> depthMap;
273 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
274
275 if (!display_off) {
276#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
277 d1 = vpDisplayFactory::createDisplay(I_color, 100, 30, "Pose from Homography");
278 d2 = vpDisplayFactory::createDisplay(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
279 d3 = vpDisplayFactory::createDisplay(I_depth, 100, I_color.getHeight() + 70, "Depth");
280#else
281 d1 = vpDisplayFactory::allocateDisplay(I_color, 100, 30, "Pose from Homography");
282 d2 = vpDisplayFactory::allocateDisplay(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
283 d3 = vpDisplayFactory::allocateDisplay(I_depth, 100, I_color.getHeight() + 70, "Depth");
284#endif
285 }
286
288 vpDetectorAprilTag detector(opt_tag_family);
290
292 detector.setAprilTagQuadDecimate(opt_tag_quad_decimate);
293 detector.setAprilTagPoseEstimationMethod(opt_tag_pose_estimation_method);
294 detector.setAprilTagNbThreads(opt_tag_nThreads);
295 detector.setDisplayTag(opt_display_tag, opt_color_id < 0 ? vpColor::none : vpColor::getColor(opt_color_id), opt_thickness);
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;
301 for (;;) {
302 double t = vpTime::measureTimeMs();
303
305 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
306 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
308
309 I_color2 = I_color;
310 vpImageConvert::convert(I_color, I);
311 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
312
313 depthMap.resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
314#ifdef VISP_HAVE_OPENMP
315#pragma omp parallel for
316#endif
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++) {
319 if (!vpMath::isNaN(I_depth_raw[i][j])) {
320 float Z = I_depth_raw[i][j] * 0.001; // Transform depth to meters.
321 depthMap[i][j] = Z;
322 }
323 else {
324 depthMap[i][j] = 0;
325 }
326 }
327 }
328
329 vpDisplay::display(I_color);
330 vpDisplay::display(I_color2);
331 vpDisplay::display(I_depth);
332
333 std::vector<vpHomogeneousMatrix> cMo_vec;
334 detector.detect(I, opt_tag_size, cam, cMo_vec);
335
336 // Display camera pose for each tag
337 for (size_t i = 0; i < cMo_vec.size(); i++) {
338 vpDisplay::displayFrame(I_color, cMo_vec[i], cam, opt_tag_size / 2, vpColor::none, 3);
339 }
340
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; // Default 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;
350 if (vpPose::computePlanarObjectPoseFromRGBD(depthMap, tags_corners[i], cam, tags_points3d[i], cMo,
351 &confidence_index)) {
352 if (confidence_index > 0.5) {
353 vpDisplay::displayFrame(I_color2, cMo, cam, opt_tag_size / 2, vpColor::none, 3);
354 }
355 else if (confidence_index > 0.25) {
356 vpDisplay::displayFrame(I_color2, cMo, cam, opt_tag_size / 2, vpColor::orange, 3);
357 }
358 else {
359 vpDisplay::displayFrame(I_color2, cMo, cam, opt_tag_size / 2, vpColor::red, 3);
360 }
361 std::stringstream ss;
362 ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
363 vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);
364 }
365 }
367
368 vpDisplay::displayText(I_color, 20, 20, "Pose from homography + VVS", vpColor::red);
369 vpDisplay::displayText(I_color2, 20, 20, "Pose from RGBD fusion", vpColor::red);
370 vpDisplay::displayText(I_color, 35, 20, "Click to quit.", vpColor::red);
372 time_vec.push_back(t);
373
374 std::stringstream ss;
375 ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
376 vpDisplay::displayText(I_color, 50, 20, ss.str(), vpColor::red);
377
378 if (vpDisplay::getClick(I_color, false))
379 break;
380
381 vpDisplay::flush(I_color);
382 vpDisplay::flush(I_color2);
383 vpDisplay::flush(I_depth);
384 }
385
386 std::cout << "Benchmark loop processing time" << std::endl;
387 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
388 << " ; " << vpMath::getMedian(time_vec) << " ms"
389 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
390 }
391 catch (const vpException &e) {
392 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
393 }
394#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
395 if (!display_off) {
396 if (d1 != nullptr) {
397 delete d1;
398 }
399 if (d2 != nullptr) {
400 delete d2;
401 }
402 if (d3 != nullptr) {
403 delete d3;
404 }
405 }
406#endif
407
408 return EXIT_SUCCESS;
409#else
410 (void)argc;
411 (void)argv;
412#ifndef VISP_HAVE_APRILTAG
413 std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
414#else
415 std::cout << "Install Structure Core SDK, configure and build ViSP again to use this example" << std::endl;
416#endif
417#endif
418 return EXIT_SUCCESS;
419}
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static vpColor getColor(const unsigned int &i)
Definition vpColor.h:300
static const vpColor red
Definition vpColor.h:198
static const vpColor none
Definition vpColor.h:210
static const vpColor orange
Definition vpColor.h:208
@ TAG_36h11
AprilTag 36h11 pattern (recommended).
Class that defines generic functionalities for display.
Definition vpDisplay.h:171
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.
Definition vpException.h:60
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.
Definition vpImage.h:131
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:544
static bool isNaN(double value)
Definition vpMath.cpp:101
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:343
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:374
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:323
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()