Visual Servoing Platform version 3.6.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/vpDisplayGDI.h>
12#include <visp3/gui/vpDisplayOpenCV.h>
13#include <visp3/gui/vpDisplayX.h>
14#include <visp3/vision/vpPose.h>
15
16int main(int argc, const char **argv)
17{
19#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_OCCIPITAL_STRUCTURE)
21
24 double tagSize = 0.053;
25 float quad_decimate = 1.0;
26 int nThreads = 1;
27 bool display_tag = false;
28 int color_id = -1;
29 unsigned int thickness = 2;
30 bool align_frame = false;
31
32#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
33 bool display_off = true;
34 std::cout << "Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
35#else
36 bool display_off = false;
37#endif
38
39 for (int i = 1; i < argc; i++) {
40 if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
41 poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
42 } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
43 tagSize = atof(argv[i + 1]);
44 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
45 quad_decimate = (float)atof(argv[i + 1]);
46 } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
47 nThreads = atoi(argv[i + 1]);
48 } else if (std::string(argv[i]) == "--display_tag") {
49 display_tag = true;
50 } else if (std::string(argv[i]) == "--display_off") {
51 display_off = true;
52 } else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
53 color_id = atoi(argv[i + 1]);
54 } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
55 thickness = (unsigned int)atoi(argv[i + 1]);
56 } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
57 tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
58 } else if (std::string(argv[i]) == "--z_aligned") {
59 align_frame = true;
60 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
61 std::cout << "Usage: " << argv[0]
62 << " [--tag_size <tag_size in m> (default: 0.053)]"
63 " [--quad_decimate <quad_decimate> (default: 1)]"
64 " [--nthreads <nb> (default: 1)]"
65 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
66 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
67 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
68 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
69 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
70 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
71 " [--display_tag] [--z_aligned]";
72#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
73 std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
74#endif
75 std::cout << " [--help]" << std::endl;
76 return EXIT_SUCCESS;
77 }
78 }
79
80 try {
82 std::cout << "Use Occipital Structure grabber" << std::endl;
84 ST::CaptureSessionSettings settings;
85 settings.source = ST::CaptureSessionSourceId::StructureCore;
86 settings.structureCore.visibleEnabled = true;
87 settings.applyExpensiveCorrection = true; // Apply a correction and clean filter to the depth before streaming.
88 unsigned int width = 640, height = 480;
89
91 vpImage<vpRGBa> I_color(height, width);
92 vpImage<float> I_depth_raw(height, width);
93 vpImage<vpRGBa> I_depth;
94
95 g.open(settings);
96
97 std::cout << "I_color: " << I_color.getWidth() << " " << I_color.getHeight() << std::endl;
98 std::cout << "I_depth_raw: " << I_depth_raw.getWidth() << " " << I_depth_raw.getHeight() << std::endl;
99
100 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
101
102 std::cout << "Read camera parameters from Structure core device" << std::endl;
106
107 std::cout << cam << std::endl;
108 std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
109 std::cout << "tagFamily: " << tagFamily << std::endl;
110 std::cout << "nThreads : " << nThreads << std::endl;
111 std::cout << "Z aligned: " << align_frame << std::endl;
112
113 vpImage<vpRGBa> I_color2 = I_color;
114 vpImage<float> depthMap;
115 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
116
117 vpDisplay *d1 = NULL;
118 vpDisplay *d2 = NULL;
119 vpDisplay *d3 = NULL;
120 if (!display_off) {
121#ifdef VISP_HAVE_X11
122 d1 = new vpDisplayX(I_color, 100, 30, "Pose from Homography");
123 d2 = new vpDisplayX(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
124 d3 = new vpDisplayX(I_depth, 100, I_color.getHeight() + 70, "Depth");
125#elif defined(VISP_HAVE_GDI)
126 d1 = new vpDisplayGDI(I_color, 100, 30, "Pose from Homography");
127 d2 = new vpDisplayGDI(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
128 d3 = new vpDisplayGDI(I_depth, 100, I_color.getHeight() + 70, "Depth");
129#elif defined(HAVE_OPENCV_HIGHGUI)
130 d1 = new vpDisplayOpenCV(I_color, 100, 30, "Pose from Homography");
131 d2 = new vpDisplayOpenCV(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
132 d3 = new vpDisplayOpenCV(I_depth, 100, I_color.getHeight() + 70, "Depth");
133#endif
134 }
135
137 vpDetectorAprilTag detector(tagFamily);
139
141 detector.setAprilTagQuadDecimate(quad_decimate);
142 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
143 detector.setAprilTagNbThreads(nThreads);
144 detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
145 detector.setZAlignedWithCameraAxis(align_frame);
147 std::vector<double> time_vec;
148 for (;;) {
149 double t = vpTime::measureTimeMs();
150
152 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
153 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
155
156 I_color2 = I_color;
157 vpImageConvert::convert(I_color, I);
158 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
159
160 depthMap.resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
161#ifdef VISP_HAVE_OPENMP
162#pragma omp parallel for
163#endif
164 for (unsigned int i = 0; i < I_depth_raw.getHeight(); i++) {
165 for (unsigned int j = 0; j < I_depth_raw.getWidth(); j++) {
166 if (!vpMath::isNaN(I_depth_raw[i][j])) {
167 float Z = I_depth_raw[i][j] * 0.001; // Transform depth to meters.
168 depthMap[i][j] = Z;
169 } else {
170 depthMap[i][j] = 0;
171 }
172 }
173 }
174
175 vpDisplay::display(I_color);
176 vpDisplay::display(I_color2);
177 vpDisplay::display(I_depth);
178
179 std::vector<vpHomogeneousMatrix> cMo_vec;
180 detector.detect(I, tagSize, cam, cMo_vec);
181
182 // Display camera pose for each tag
183 for (size_t i = 0; i < cMo_vec.size(); i++) {
184 vpDisplay::displayFrame(I_color, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
185 }
186
188 std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
189 std::vector<int> tags_id = detector.getTagsId();
190 std::map<int, double> tags_size;
191 tags_size[-1] = tagSize; // Default tag size
192 std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
193 for (size_t i = 0; i < tags_corners.size(); i++) {
195 double confidence_index;
196 if (vpPose::computePlanarObjectPoseFromRGBD(depthMap, tags_corners[i], cam, tags_points3d[i], cMo,
197 &confidence_index)) {
198 if (confidence_index > 0.5) {
199 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::none, 3);
200 } else if (confidence_index > 0.25) {
201 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::orange, 3);
202 } else {
203 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::red, 3);
204 }
205 std::stringstream ss;
206 ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
207 vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);
208 }
209 }
211
212 vpDisplay::displayText(I_color, 20, 20, "Pose from homography + VVS", vpColor::red);
213 vpDisplay::displayText(I_color2, 20, 20, "Pose from RGBD fusion", vpColor::red);
214 vpDisplay::displayText(I_color, 35, 20, "Click to quit.", vpColor::red);
215 t = vpTime::measureTimeMs() - t;
216 time_vec.push_back(t);
217
218 std::stringstream ss;
219 ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
220 vpDisplay::displayText(I_color, 50, 20, ss.str(), vpColor::red);
221
222 if (vpDisplay::getClick(I_color, false))
223 break;
224
225 vpDisplay::flush(I_color);
226 vpDisplay::flush(I_color2);
227 vpDisplay::flush(I_depth);
228 }
229
230 std::cout << "Benchmark loop processing time" << std::endl;
231 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
232 << " ; " << vpMath::getMedian(time_vec) << " ms"
233 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
234
235 if (!display_off) {
236 delete d1;
237 delete d2;
238 delete d3;
239 }
240
241 } catch (const vpException &e) {
242 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
243 }
244
245 return EXIT_SUCCESS;
246#else
247 (void)argc;
248 (void)argv;
249#ifndef VISP_HAVE_APRILTAG
250 std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
251#else
252 std::cout << "Install Structure Core SDK, configure and build ViSP again to use this example" << std::endl;
253#endif
254#endif
255 return EXIT_SUCCESS;
256}
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor orange
Definition vpColor.h:221
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
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:59
const char * getMessage() const
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:135
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
static bool isNaN(double value)
Definition vpMath.cpp:93
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
vpCameraParameters getCameraParameters(const vpOccipitalStructureStream stream_type, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithoutDistortion)
void acquire(vpImage< unsigned char > &gray, bool undistorted=false, double *ts=NULL)
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=NULL)
VISP_EXPORT double measureTimeMs()