Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
tutorial-apriltag-detector-live-rgbd-realsense.cpp
1 #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/gui/vpDisplayGDI.h>
11 #include <visp3/gui/vpDisplayOpenCV.h>
12 #include <visp3/gui/vpDisplayX.h>
13 #include <visp3/vision/vpPose.h>
14 
15 int main(int argc, const char **argv)
16 {
18 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2)
20 
23  double tagSize = 0.053;
24  float quad_decimate = 1.0;
25  int nThreads = 1;
26  bool display_tag = false;
27  int color_id = -1;
28  unsigned int thickness = 2;
29  bool align_frame = false;
30  bool opt_verbose = 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  }
43  else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
44  tagSize = atof(argv[i + 1]);
45  }
46  else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
47  quad_decimate = (float)atof(argv[i + 1]);
48  }
49  else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
50  nThreads = atoi(argv[i + 1]);
51  }
52  else if (std::string(argv[i]) == "--display_tag") {
53  display_tag = true;
54  }
55  else if (std::string(argv[i]) == "--display_off") {
56  display_off = true;
57  }
58  else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
59  color_id = atoi(argv[i + 1]);
60  }
61  else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
62  thickness = (unsigned int)atoi(argv[i + 1]);
63  }
64  else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
65  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
66  }
67  else if (std::string(argv[i]) == "--z_aligned") {
68  align_frame = true;
69  }
70  else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
71  opt_verbose = true;
72  }
73  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
74  std::cout << "Usage: " << argv[0]
75  << " [--tag_size <tag_size in m> (default: 0.053)]"
76  " [--quad_decimate <quad_decimate> (default: 1)]"
77  " [--nthreads <nb> (default: 1)]"
78  " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
79  " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
80  " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
81  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
82  " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
83  " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
84  " [--display_tag] [--z_aligned]";
85 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
86  std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
87 #endif
88  std::cout << " [--verbose,-v] [--help,-h]" << std::endl;
89  return EXIT_SUCCESS;
90  }
91  }
92 
93  try {
95  std::cout << "Use Realsense 2 grabber" << std::endl;
96  vpRealSense2 g;
97  rs2::config config;
98  unsigned int width = 640, height = 480;
99  config.enable_stream(RS2_STREAM_COLOR, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_RGBA8, 30);
100  config.enable_stream(RS2_STREAM_DEPTH, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_Z16, 30);
101  config.enable_stream(RS2_STREAM_INFRARED, static_cast<int>(width), static_cast<int>(height), RS2_FORMAT_Y8, 30);
102 
104  vpImage<vpRGBa> I_color(height, width);
105  vpImage<uint16_t> I_depth_raw(height, width);
106  vpImage<vpRGBa> I_depth;
107 
108  g.open(config);
109  const float depth_scale = g.getDepthScale();
110  std::cout << "I_color: " << I_color.getWidth() << " " << I_color.getHeight() << std::endl;
111  std::cout << "I_depth_raw: " << I_depth_raw.getWidth() << " " << I_depth_raw.getHeight() << std::endl;
112 
113  rs2::align align_to_color = RS2_STREAM_COLOR;
114  g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap),
115  nullptr, nullptr, &align_to_color);
116 
117  std::cout << "Read camera parameters from Realsense device" << std::endl;
118  vpCameraParameters cam;
121 
122  std::cout << cam << std::endl;
123  std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
124  std::cout << "tagFamily: " << tagFamily << std::endl;
125  std::cout << "nThreads : " << nThreads << std::endl;
126  std::cout << "Z aligned: " << align_frame << std::endl;
127 
128  vpImage<vpRGBa> I_color2 = I_color;
129  vpImage<float> depthMap;
130  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
131 
132  vpDisplay *d1 = nullptr;
133  vpDisplay *d2 = nullptr;
134  vpDisplay *d3 = nullptr;
135  if (!display_off) {
136 #ifdef VISP_HAVE_X11
137  d1 = new vpDisplayX(I_color, 100, 30, "Pose from Homography");
138  d2 = new vpDisplayX(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
139  d3 = new vpDisplayX(I_depth, 100, I_color.getHeight() + 70, "Depth");
140 #elif defined(VISP_HAVE_GDI)
141  d1 = new vpDisplayGDI(I_color, 100, 30, "Pose from Homography");
142  d2 = new vpDisplayGDI(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
143  d3 = new vpDisplayGDI(I_depth, 100, I_color.getHeight() + 70, "Depth");
144 #elif defined(HAVE_OPENCV_HIGHGUI)
145  d1 = new vpDisplayOpenCV(I_color, 100, 30, "Pose from Homography");
146  d2 = new vpDisplayOpenCV(I_color2, I_color.getWidth() + 120, 30, "Pose from RGBD fusion");
147  d3 = new vpDisplayOpenCV(I_depth, 100, I_color.getHeight() + 70, "Depth");
148 #endif
149  }
150 
152  vpDetectorAprilTag detector(tagFamily);
154 
156  detector.setAprilTagQuadDecimate(quad_decimate);
157  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
158  detector.setAprilTagNbThreads(nThreads);
159  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
160  detector.setZAlignedWithCameraAxis(align_frame);
162  std::vector<double> time_vec;
163  for (;;) {
164  double t = vpTime::measureTimeMs();
165 
167  g.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
168  reinterpret_cast<unsigned char *>(I_depth_raw.bitmap), nullptr, nullptr, &align_to_color);
170 
171  I_color2 = I_color;
172  vpImageConvert::convert(I_color, I);
173  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
174 
175  depthMap.resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
176 #ifdef VISP_HAVE_OPENMP
177 #pragma omp parallel for
178 #endif
179  for (int i = 0; i < static_cast<int>(I_depth_raw.getHeight()); i++) {
180  for (int j = 0; j < static_cast<int>(I_depth_raw.getWidth()); j++) {
181  if (I_depth_raw[i][j]) {
182  float Z = I_depth_raw[i][j] * depth_scale;
183  depthMap[i][j] = Z;
184  }
185  else {
186  depthMap[i][j] = 0;
187  }
188  }
189  }
190 
191  vpDisplay::display(I_color);
192  vpDisplay::display(I_color2);
193  vpDisplay::display(I_depth);
194 
195  std::vector<vpHomogeneousMatrix> cMo_vec;
196  detector.detect(I, tagSize, cam, cMo_vec);
197 
198  // Display camera pose for each tag
199  for (size_t i = 0; i < cMo_vec.size(); i++) {
200  vpDisplay::displayFrame(I_color, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
201  }
202 
204  std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
205  std::vector<int> tags_id = detector.getTagsId();
206  std::map<int, double> tags_size;
207  tags_size[-1] = tagSize; // Default tag size
208  std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
209  for (size_t i = 0; i < tags_corners.size(); i++) {
211  double confidence_index;
212  if (vpPose::computePlanarObjectPoseFromRGBD(depthMap, tags_corners[i], cam, tags_points3d[i], cMo,
213  &confidence_index)) {
214  if (confidence_index > 0.5) {
215  vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::none, 3);
216  }
217  else if (confidence_index > 0.25) {
218  vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::orange, 3);
219  }
220  else {
221  vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::red, 3);
222  }
223  std::stringstream ss;
224  ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
225  vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);
226 
227  if (opt_verbose) {
228  std::cout << "cMo[" << i << "]: \n" << cMo_vec[i] << std::endl;
229  std::cout << "cMo[" << i << "] using depth: \n" << cMo << std::endl;
230  }
231  }
232  }
234 
235  vpDisplay::displayText(I_color, 20, 20, "Pose from homography + VVS", vpColor::red);
236  vpDisplay::displayText(I_color2, 20, 20, "Pose from RGBD fusion", vpColor::red);
237  vpDisplay::displayText(I_color, 35, 20, "Click to quit.", vpColor::red);
238  t = vpTime::measureTimeMs() - t;
239  time_vec.push_back(t);
240 
241  std::stringstream ss;
242  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
243  vpDisplay::displayText(I_color, 50, 20, ss.str(), vpColor::red);
244 
245  if (vpDisplay::getClick(I_color, false))
246  break;
247 
248  vpDisplay::flush(I_color);
249  vpDisplay::flush(I_color2);
250  vpDisplay::flush(I_depth);
251  }
252 
253  std::cout << "Benchmark loop processing time" << std::endl;
254  std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
255  << " ; " << vpMath::getMedian(time_vec) << " ms"
256  << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
257 
258  if (!display_off) {
259  delete d1;
260  delete d2;
261  delete d3;
262  }
263 
264  }
265  catch (const vpException &e) {
266  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
267  }
268 
269  return EXIT_SUCCESS;
270 #else
271  (void)argc;
272  (void)argv;
273 #ifndef VISP_HAVE_APRILTAG
274  std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
275 #else
276  std::cout << "Install librealsense 3rd party, configure and build ViSP again to use this example" << std::endl;
277 #endif
278 #endif
279  return EXIT_SUCCESS;
280 }
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static vpColor getColor(const unsigned int &i)
Definition: vpColor.h:307
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).
Definition: vpDisplayGDI.h:128
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:128
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
Definition: vpException.cpp:64
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:798
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:323
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:354
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:303
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)
Definition: vpPoseRGBD.cpp:151
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
float getDepthScale()
VISP_EXPORT double measureTimeMs()