Visual Servoing Platform version 3.5.0
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/gui/vpDisplayGDI.h>
10#include <visp3/gui/vpDisplayOpenCV.h>
11#include <visp3/gui/vpDisplayX.h>
12#include <visp3/core/vpXmlParserCamera.h>
13#include <visp3/core/vpImageConvert.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 }
61 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
62 std::cout << "Usage: " << argv[0]
63 << " [--tag_size <tag_size in m> (default: 0.053)]"
64 " [--quad_decimate <quad_decimate> (default: 1)]"
65 " [--nthreads <nb> (default: 1)]"
66 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
67 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
68 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
69 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
70 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
71 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
72 " [--display_tag] [--z_aligned]";
73#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
74 std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
75#endif
76 std::cout << " [--help]" << std::endl;
77 return EXIT_SUCCESS;
78 }
79 }
80
81 try {
83 std::cout << "Use Occipital Structure grabber" << std::endl;
85 ST::CaptureSessionSettings settings;
86 settings.source = ST::CaptureSessionSourceId::StructureCore;
87 settings.structureCore.visibleEnabled = true;
88 settings.applyExpensiveCorrection = true; // Apply a correction and clean filter to the depth before streaming.
89 unsigned int width = 640, height = 480;
90
92 vpImage<vpRGBa> I_color(height, width);
93 vpImage<float> I_depth_raw(height, width);
94 vpImage<vpRGBa> I_depth;
95
96 g.open(settings);
97
98 std::cout << "I_color: " << I_color.getWidth() << " " << I_color.getHeight() << std::endl;
99 std::cout << "I_depth_raw: " << I_depth_raw.getWidth() << " " << I_depth_raw.getHeight() << std::endl;
100
101 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
102
103 std::cout << "Read camera parameters from Structure core device" << std::endl;
107
108 std::cout << cam << std::endl;
109 std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
110 std::cout << "tagFamily: " << tagFamily << std::endl;
111 std::cout << "nThreads : " << nThreads << std::endl;
112 std::cout << "Z aligned: " << align_frame << std::endl;
113
114 vpImage<vpRGBa> I_color2 = I_color;
115 vpImage<float> depthMap;
116 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
117
118 vpDisplay *d1 = NULL;
119 vpDisplay *d2 = NULL;
120 vpDisplay *d3 = NULL;
121 if (! display_off) {
122#ifdef VISP_HAVE_X11
123 d1 = new vpDisplayX(I_color, 100, 30, "Pose from Homography");
124 d2 = new vpDisplayX(I_color2, I_color.getWidth()+120, 30, "Pose from RGBD fusion");
125 d3 = new vpDisplayX(I_depth, 100, I_color.getHeight()+70, "Depth");
126#elif defined(VISP_HAVE_GDI)
127 d1 = new vpDisplayGDI(I_color, 100, 30, "Pose from Homography");
128 d2 = new vpDisplayGDI(I_color2, I_color.getWidth()+120, 30, "Pose from RGBD fusion");
129 d3 = new vpDisplayGDI(I_depth, 100, I_color.getHeight()+70, "Depth");
130#elif defined(VISP_HAVE_OPENCV)
131 d1 = new vpDisplayOpenCV(I_color, 100, 30, "Pose from Homography");
132 d2 = new vpDisplayOpenCV(I_color2, I_color.getWidth()+120, 30, "Pose from RGBD fusion");
133 d3 = new vpDisplayOpenCV(I_depth, 100, I_color.getHeight()+70, "Depth");
134#endif
135 }
136
138 vpDetectorAprilTag detector(tagFamily);
140
142 detector.setAprilTagQuadDecimate(quad_decimate);
143 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
144 detector.setAprilTagNbThreads(nThreads);
145 detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
146 detector.setZAlignedWithCameraAxis(align_frame);
148 std::vector<double> time_vec;
149 for (;;) {
150 double t = vpTime::measureTimeMs();
151
153 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), 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, &confidence_index)) {
197 if (confidence_index > 0.5) {
198 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize/2, vpColor::none, 3);
199 }
200 else if (confidence_index > 0.25) {
201 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize/2, vpColor::orange, 3);
202 }
203 else {
204 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize/2, vpColor::red, 3);
205 }
206 std::stringstream ss;
207 ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
208 vpDisplay::displayText(I_color2, 35 + i*15, 20, ss.str(), vpColor::red);
209 }
210 }
212
213 vpDisplay::displayText(I_color, 20, 20, "Pose from homography + VVS", vpColor::red);
214 vpDisplay::displayText(I_color2, 20, 20, "Pose from RGBD fusion", vpColor::red);
215 vpDisplay::displayText(I_color, 35, 20, "Click to quit.", vpColor::red);
216 t = vpTime::measureTimeMs() - t;
217 time_vec.push_back(t);
218
219 std::stringstream ss;
220 ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
221 vpDisplay::displayText(I_color, 50, 20, ss.str(), vpColor::red);
222
223 if (vpDisplay::getClick(I_color, false))
224 break;
225
226 vpDisplay::flush(I_color);
227 vpDisplay::flush(I_color2);
228 vpDisplay::flush(I_depth);
229 }
230
231 std::cout << "Benchmark loop processing time" << std::endl;
232 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
233 << " ; " << vpMath::getMedian(time_vec) << " ms"
234 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
235
236 if (! display_off) {
237 delete d1;
238 delete d2;
239 delete d3;
240 }
241
242 } catch (const vpException &e) {
243 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
244 }
245
246 return EXIT_SUCCESS;
247#else
248 (void)argc;
249 (void)argv;
250#ifndef VISP_HAVE_APRILTAG
251 std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
252#else
253 std::cout << "Install Structure Core SDK, configure and build ViSP again to use this example" << std::endl;
254#endif
255#endif
256 return EXIT_SUCCESS;
257}
Generic class defining intrinsic camera parameters.
static vpColor getColor(const unsigned int &i)
Definition: vpColor.h:310
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
static const vpColor orange
Definition: vpColor.h:227
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:135
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:178
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(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))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:90
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)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
static bool isNaN(double value)
Definition: vpMath.cpp:85
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
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)
Definition: vpPoseRGBD.cpp:254
VISP_EXPORT double measureTimeMs()