Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-apriltag-detector-live-rgbd-realsense.cpp
1
2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpRealSense2.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_REALSENSE2)
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 Realsense 2 grabber" << std::endl;
84 rs2::config config;
85 unsigned int width = 640, height = 480;
86 config.enable_stream(RS2_STREAM_COLOR, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_RGBA8, 30);
87 config.enable_stream(RS2_STREAM_DEPTH, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_Z16, 30);
88 config.enable_stream(RS2_STREAM_INFRARED, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_Y8, 30);
89
91 vpImage<vpRGBa> I_color(height, width);
92 vpImage<uint16_t> I_depth_raw(height, width);
93 vpImage<vpRGBa> I_depth;
94
95 g.open(config);
96 const float depth_scale = g.getDepthScale();
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 rs2::align align_to_color = RS2_STREAM_COLOR;
101 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap),
102 NULL, NULL, &align_to_color);
103
104 std::cout << "Read camera parameters from Realsense device" << std::endl;
108
109 std::cout << cam << std::endl;
110 std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
111 std::cout << "tagFamily: " << tagFamily << std::endl;
112 std::cout << "nThreads : " << nThreads << std::endl;
113 std::cout << "Z aligned: " << align_frame << std::endl;
114
115 vpImage<vpRGBa> I_color2 = I_color;
116 vpImage<float> depthMap;
117 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
118
119 vpDisplay *d1 = NULL;
120 vpDisplay *d2 = NULL;
121 vpDisplay *d3 = NULL;
122 if (!display_off) {
123#ifdef VISP_HAVE_X11
124 d1 = new vpDisplayX(I_color, 100, 30, "Pose from Homography");
125 d2 = new vpDisplayX(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
126 d3 = new vpDisplayX(I_depth, 100, I_color.getHeight() + 70, "Depth");
127#elif defined(VISP_HAVE_GDI)
128 d1 = new vpDisplayGDI(I_color, 100, 30, "Pose from Homography");
129 d2 = new vpDisplayGDI(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
130 d3 = new vpDisplayGDI(I_depth, 100, I_color.getHeight() + 70, "Depth");
131#elif defined(HAVE_OPENCV_HIGHGUI)
132 d1 = new vpDisplayOpenCV(I_color, 100, 30, "Pose from Homography");
133 d2 = new vpDisplayOpenCV(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
134 d3 = new vpDisplayOpenCV(I_depth, 100, I_color.getHeight() + 70, "Depth");
135#endif
136 }
137
139 vpDetectorAprilTag detector(tagFamily);
141
143 detector.setAprilTagQuadDecimate(quad_decimate);
144 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
145 detector.setAprilTagNbThreads(nThreads);
146 detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
147 detector.setZAlignedWithCameraAxis(align_frame);
149 std::vector<double> time_vec;
150 for (;;) {
151 double t = vpTime::measureTimeMs();
152
154 g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
155 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap), NULL, NULL, &align_to_color);
157
158 I_color2 = I_color;
159 vpImageConvert::convert(I_color, I);
160 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
161
162 depthMap.resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
163#ifdef VISP_HAVE_OPENMP
164#pragma omp parallel for
165#endif
166 for (int i = 0; i < static_cast<int>(I_depth_raw.getHeight()); i++) {
167 for (int j = 0; j < static_cast<int>(I_depth_raw.getWidth()); j++) {
168 if (I_depth_raw[i][j]) {
169 float Z = I_depth_raw[i][j] * depth_scale;
170 depthMap[i][j] = Z;
171 } else {
172 depthMap[i][j] = 0;
173 }
174 }
175 }
176
177 vpDisplay::display(I_color);
178 vpDisplay::display(I_color2);
179 vpDisplay::display(I_depth);
180
181 std::vector<vpHomogeneousMatrix> cMo_vec;
182 detector.detect(I, tagSize, cam, cMo_vec);
183
184 // Display camera pose for each tag
185 for (size_t i = 0; i < cMo_vec.size(); i++) {
186 vpDisplay::displayFrame(I_color, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
187 }
188
190 std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
191 std::vector<int> tags_id = detector.getTagsId();
192 std::map<int, double> tags_size;
193 tags_size[-1] = tagSize; // Default tag size
194 std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
195 for (size_t i = 0; i < tags_corners.size(); i++) {
197 double confidence_index;
198 if (vpPose::computePlanarObjectPoseFromRGBD(depthMap, tags_corners[i], cam, tags_points3d[i], cMo,
199 &confidence_index)) {
200 if (confidence_index > 0.5) {
201 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::none, 3);
202 } else if (confidence_index > 0.25) {
203 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::orange, 3);
204 } else {
205 vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::red, 3);
206 }
207 std::stringstream ss;
208 ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
209 vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);
210 }
211 }
213
214 vpDisplay::displayText(I_color, 20, 20, "Pose from homography + VVS", vpColor::red);
215 vpDisplay::displayText(I_color2, 20, 20, "Pose from RGBD fusion", vpColor::red);
216 vpDisplay::displayText(I_color, 35, 20, "Click to quit.", vpColor::red);
217 t = vpTime::measureTimeMs() - t;
218 time_vec.push_back(t);
219
220 std::stringstream ss;
221 ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
222 vpDisplay::displayText(I_color, 50, 20, ss.str(), vpColor::red);
223
224 if (vpDisplay::getClick(I_color, false))
225 break;
226
227 vpDisplay::flush(I_color);
228 vpDisplay::flush(I_color2);
229 vpDisplay::flush(I_depth);
230 }
231
232 std::cout << "Benchmark loop processing time" << std::endl;
233 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
234 << " ; " << vpMath::getMedian(time_vec) << " ms"
235 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
236
237 if (!display_off) {
238 delete d1;
239 delete d2;
240 delete d3;
241 }
242
243 } catch (const vpException &e) {
244 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
245 }
246
247 return EXIT_SUCCESS;
248#else
249 (void)argc;
250 (void)argv;
251#ifndef VISP_HAVE_APRILTAG
252 std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
253#else
254 std::cout << "Install librealsense 3rd party, configure and build ViSP again to use this example" << std::endl;
255#endif
256#endif
257 return EXIT_SUCCESS;
258}
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 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
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)
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
float getDepthScale()
VISP_EXPORT double measureTimeMs()