Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
tutorial-mb-generic-tracker-apriltag-rs2.cpp
1 #include <fstream>
3 #include <ios>
4 #include <iostream>
5 
6 #include <visp3/core/vpXmlParserCamera.h>
7 #include <visp3/detection/vpDetectorAprilTag.h>
8 #include <visp3/gui/vpDisplayGDI.h>
9 #include <visp3/gui/vpDisplayOpenCV.h>
10 #include <visp3/gui/vpDisplayX.h>
11 #include <visp3/mbt/vpMbGenericTracker.h>
12 #include <visp3/sensor/vpRealSense2.h>
13 
14 typedef enum { state_detection, state_tracking, state_quit } state_t;
15 
16 // Creates a cube.cao file in your current directory
17 // cubeEdgeSize : size of cube edges in meters
18 void createCaoFile(double cubeEdgeSize)
19 {
20  std::ofstream fileStream;
21  fileStream.open("cube.cao", std::ofstream::out | std::ofstream::trunc);
22  fileStream << "V1\n";
23  fileStream << "# 3D Points\n";
24  fileStream << "8 # Number of points\n";
25  fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 0: (X, Y, Z)\n";
26  fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 1\n";
27  fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 2\n";
28  fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 3\n";
29  fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 4\n";
30  fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 5\n";
31  fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 6\n";
32  fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 7\n";
33  fileStream << "# 3D Lines\n";
34  fileStream << "0 # Number of lines\n";
35  fileStream << "# Faces from 3D lines\n";
36  fileStream << "0 # Number of faces\n";
37  fileStream << "# Faces from 3D points\n";
38  fileStream << "6 # Number of faces\n";
39  fileStream << "4 0 3 2 1 # Face 0: [number of points] [index of the 3D points]...\n";
40  fileStream << "4 1 2 5 6\n";
41  fileStream << "4 4 7 6 5\n";
42  fileStream << "4 0 7 4 3\n";
43  fileStream << "4 5 2 3 4\n";
44  fileStream << "4 0 1 6 7 # Face 5\n";
45  fileStream << "# 3D cylinders\n";
46  fileStream << "0 # Number of cylinders\n";
47  fileStream << "# 3D circles\n";
48  fileStream << "0 # Number of circles\n";
49  fileStream.close();
50 }
51 
52 #if defined(VISP_HAVE_APRILTAG)
53 state_t detectAprilTag(const vpImage<unsigned char> &I, vpDetectorAprilTag &detector, double tagSize,
54  const vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
55 {
56  std::vector<vpHomogeneousMatrix> cMo_vec;
57 
58  // Detection
59  bool ret = detector.detect(I, tagSize, cam, cMo_vec);
60 
61  // Display camera pose
62  for (size_t i = 0; i < cMo_vec.size(); i++) {
63  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
64  }
65 
66  vpDisplay::displayText(I, 40, 20, "State: waiting tag detection", vpColor::red);
67 
68  if (ret && detector.getNbObjects() > 0) { // if tag detected, we pick the first one
69  cMo = cMo_vec[0];
70  return state_tracking;
71  }
72 
73  return state_detection;
74 }
75 #endif // #if defined(VISP_HAVE_APRILTAG)
76 
77 state_t track(const vpImage<unsigned char> &I, vpMbGenericTracker &tracker, double projection_error_threshold,
79 {
81  tracker.getCameraParameters(cam);
82 
83  // Track the object
84  try {
85  tracker.track(I);
86  } catch (...) {
87  return state_detection;
88  }
89 
90  tracker.getPose(cMo);
91 
92  // Detect tracking error
93  double projection_error = tracker.computeCurrentProjectionError(I, cMo, cam);
94  if (projection_error > projection_error_threshold) {
95  return state_detection;
96  }
97 
98  // Display
99  tracker.display(I, cMo, cam, vpColor::red, 2);
100  vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
101  vpDisplay::displayText(I, 40, 20, "State: tracking in progress", vpColor::red);
102  {
103  std::stringstream ss;
104  ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt();
105  vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
106  }
107 
108  return state_tracking;
109 }
110 
111 state_t track(std::map<std::string, const vpImage<unsigned char> *> mapOfImages,
112 #ifdef VISP_HAVE_PCL
113  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds,
114 #else
115  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds,
116  std::map<std::string, unsigned int> mapOfWidths, std::map<std::string, unsigned int> mapOfHeights,
117 #endif
118  const vpImage<unsigned char> &I_gray, const vpImage<unsigned char> &I_depth,
119  const vpHomogeneousMatrix &depth_M_color, vpMbGenericTracker &tracker, double projection_error_threshold,
120  vpHomogeneousMatrix &cMo)
121 {
122  vpCameraParameters cam_color, cam_depth;
123  tracker.getCameraParameters(cam_color, cam_depth);
124 
125  // Track the object
126  try {
127 #ifdef VISP_HAVE_PCL
128  tracker.track(mapOfImages, mapOfPointclouds);
129 #else
130  tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
131 #endif
132  } catch (...) {
133  return state_detection;
134  }
135 
136  tracker.getPose(cMo);
137 
138  // Detect tracking error
139  double projection_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
140  if (projection_error > projection_error_threshold) {
141  return state_detection;
142  }
143 
144  // Display
145  tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3);
146  vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3);
147  vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3);
148 
149  return state_tracking;
150 }
151 
152 int main(int argc, const char **argv)
153 {
155 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_MODULE_MBT)
157 
159  double opt_tag_size = 0.08;
160  float opt_quad_decimate = 1.0;
161  int opt_nthreads = 1;
162  double opt_cube_size = 0.125; // 12.5cm by default
163 #ifdef VISP_HAVE_OPENCV
164  bool opt_use_texture = false;
165 #endif
166  bool opt_use_depth = false;
167  double opt_projection_error_threshold = 40.;
168 
169 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
170  bool display_off = true;
171 #else
172  bool display_off = false;
173 #endif
174 
175  for (int i = 1; i < argc; i++) {
176  if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
177  opt_tag_size = atof(argv[i + 1]);
178  } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
179  opt_quad_decimate = (float)atof(argv[i + 1]);
180  } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
181  opt_nthreads = atoi(argv[i + 1]);
182  } else if (std::string(argv[i]) == "--display_off") {
183  display_off = true;
184  } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
185  opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
186  } else if (std::string(argv[i]) == "--cube_size" && i + 1 < argc) {
187  opt_cube_size = atof(argv[i + 1]);
188 #ifdef VISP_HAVE_OPENCV
189  } else if (std::string(argv[i]) == "--texture") {
190  opt_use_texture = true;
191 #endif
192  } else if (std::string(argv[i]) == "--depth") {
193  opt_use_depth = true;
194  } else if (std::string(argv[i]) == "--projection_error" && i + 1 < argc) {
195  opt_projection_error_threshold = atof(argv[i + 1]);
196  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
197  std::cout << "Usage: " << argv[0]
198  << " [--cube_size <size in m>] [--tag_size <size in m>]"
199  " [--quad_decimate <decimation>] [--nthreads <nb>]"
200  " [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
201  " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
202 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
203  std::cout << " [--display_off]";
204 #endif
205  std::cout << " [--texture] [--depth] [--projection_error <30 - 100>] [--help]" << std::endl;
206  return EXIT_SUCCESS;
207  }
208  }
209 
210  createCaoFile(opt_cube_size);
211 
212  try {
214  vpRealSense2 realsense;
215  rs2::config config;
216  int width = 640, height = 480, stream_fps = 30;
217  config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
218  config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
219  config.disable_stream(RS2_STREAM_INFRARED);
220  realsense.open(config);
221 
222  vpCameraParameters cam_color, cam_depth;
223  vpHomogeneousMatrix depth_M_color;
224  cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion);
225  if (opt_use_depth) {
226  cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion);
227  depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
228  }
229 
230  vpImage<vpRGBa> I_color(height, width);
231  vpImage<unsigned char> I_gray(height, width);
232  vpImage<unsigned char> I_depth(height, width);
233  vpImage<uint16_t> I_depth_raw(height, width);
234  std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
235  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
236 #ifdef VISP_HAVE_PCL
237  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
238  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
239 #else
240  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
241  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
242  std::vector<vpColVector> pointcloud;
243 #endif
244 
245  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
246 
247  std::cout << "Cube size: " << opt_cube_size << std::endl;
248  std::cout << "AprilTag size: " << opt_tag_size << std::endl;
249  std::cout << "AprilTag family: " << opt_tag_family << std::endl;
250  std::cout << "Camera parameters:" << std::endl;
251  std::cout << " Color:\n" << cam_color << std::endl;
252  if (opt_use_depth)
253  std::cout << " Depth:\n" << cam_depth << std::endl;
254  std::cout << "Detection: " << std::endl;
255  std::cout << " Quad decimate: " << opt_quad_decimate << std::endl;
256  std::cout << " Threads number: " << opt_nthreads << std::endl;
257  std::cout << "Tracker: " << std::endl;
258  std::cout << " Use edges : 1" << std::endl;
259  std::cout << " Use texture: "
260 #ifdef VISP_HAVE_OPENCV
261  << opt_use_texture << std::endl;
262 #else
263  << " na" << std::endl;
264 #endif
265  std::cout << " Use depth : " << opt_use_depth << std::endl;
266  std::cout << " Projection error: " << opt_projection_error_threshold << std::endl;
267 
268  // Construct display
269  vpDisplay *d_gray = NULL;
270  vpDisplay *d_depth = NULL;
271 
272  if (!display_off) {
273 #ifdef VISP_HAVE_X11
274  d_gray = new vpDisplayX(I_gray, 50, 50, "Color stream");
275  if (opt_use_depth)
276  d_depth = new vpDisplayX(I_depth, 80 + I_gray.getWidth(), 50, "Depth stream");
277 #elif defined(VISP_HAVE_GDI)
278  d_gray = new vpDisplayGDI(I_gray);
279  if (opt_use_depth)
280  d_depth = new vpDisplayGDI(I_depth);
281 #elif defined(VISP_HAVE_OPENCV)
282  d_gray = new vpDisplayOpenCV(I_gray);
283  if (opt_use_depth)
284  d_depth = new vpDisplayOpenCV(I_depth);
285 #endif
286  }
287 
288  // Initialize AprilTag detector
289  vpDetectorAprilTag detector(opt_tag_family);
290  detector.setAprilTagQuadDecimate(opt_quad_decimate);
291  detector.setAprilTagNbThreads(opt_nthreads);
292 
293  // Prepare MBT
294  std::vector<int> trackerTypes;
295 #ifdef VISP_HAVE_OPENCV
296  if (opt_use_texture)
298  else
299 #endif
300  trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER);
301 
302  if (opt_use_depth)
303  trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
304 
305  vpMbGenericTracker tracker(trackerTypes);
306  // edges
307  vpMe me;
308  me.setMaskSize(5);
309  me.setMaskNumber(180);
311  me.setRange(12);
313  me.setThreshold(10000);
314  me.setMu1(0.5);
315  me.setMu2(0.5);
316  me.setSampleStep(4);
317  tracker.setMovingEdge(me);
318 
319 #ifdef VISP_HAVE_OPENCV
320  if (opt_use_texture) {
321  vpKltOpencv klt_settings;
322  klt_settings.setMaxFeatures(300);
323  klt_settings.setWindowSize(5);
324  klt_settings.setQuality(0.015);
325  klt_settings.setMinDistance(8);
326  klt_settings.setHarrisFreeParameter(0.01);
327  klt_settings.setBlockSize(3);
328  klt_settings.setPyramidLevels(3);
329  tracker.setKltOpencv(klt_settings);
330  tracker.setKltMaskBorder(5);
331  }
332 #endif
333 
334  if (opt_use_depth) {
335  // camera calibration params
336  tracker.setCameraParameters(cam_color, cam_depth);
337  // model definition
338  tracker.loadModel("cube.cao", "cube.cao");
339  mapOfCameraTransformations["Camera2"] = depth_M_color;
340  tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
341  tracker.setAngleAppear(vpMath::rad(70), vpMath::rad(70));
342  tracker.setAngleDisappear(vpMath::rad(80), vpMath::rad(80));
343  } else {
344  // camera calibration params
345  tracker.setCameraParameters(cam_color);
346  // model definition
347  tracker.loadModel("cube.cao");
348  tracker.setAngleAppear(vpMath::rad(70));
349  tracker.setAngleDisappear(vpMath::rad(80));
350  }
351  tracker.setDisplayFeatures(true);
352 
354  state_t state = state_detection;
355 
356  // wait for a tag detection
357  while (state != state_quit) {
358  if (opt_use_depth) {
359 #ifdef VISP_HAVE_PCL
360  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, NULL, pointcloud, NULL);
361 #else
362  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointcloud, NULL,
363  NULL);
364 #endif
365  vpImageConvert::convert(I_color, I_gray);
366  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
367  vpDisplay::display(I_gray);
368  vpDisplay::display(I_depth);
369 
370  mapOfImages["Camera1"] = &I_gray;
371  mapOfImages["Camera2"] = &I_depth;
372 #ifdef VISP_HAVE_PCL
373  mapOfPointclouds["Camera2"] = pointcloud;
374 #else
375  mapOfPointclouds["Camera2"] = &pointcloud;
376  mapOfWidths["Camera2"] = width;
377  mapOfHeights["Camera2"] = height;
378 #endif
379  } else {
380  realsense.acquire(I_gray);
381  vpDisplay::display(I_gray);
382  }
383 
384  if (state == state_detection) {
385  state = detectAprilTag(I_gray, detector, opt_tag_size, cam_color, cMo);
386 
387  // Initialize the tracker with the result of the detection
388  if (state == state_tracking) {
389  if (opt_use_depth) {
390  mapOfCameraPoses["Camera1"] = cMo;
391  mapOfCameraPoses["Camera2"] = depth_M_color * cMo;
392  tracker.initFromPose(mapOfImages, mapOfCameraPoses);
393  } else {
394  tracker.initFromPose(I_gray, cMo);
395  }
396  }
397  }
398 
399  if (state == state_tracking) {
400  if (opt_use_depth) {
401 #ifdef VISP_HAVE_PCL
402  state = track(mapOfImages, mapOfPointclouds, I_gray, I_depth, depth_M_color, tracker,
403  opt_projection_error_threshold, cMo);
404 #else
405  state = track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights, I_gray, I_depth, depth_M_color,
406  tracker, opt_projection_error_threshold, cMo);
407 #endif
408  } else {
409  state = track(I_gray, tracker, opt_projection_error_threshold, cMo);
410  }
411  {
412  std::stringstream ss;
413  ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt()
414  << ", depth " << tracker.getNbFeaturesDepthDense();
415  vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red);
416  }
417  }
418 
419  vpDisplay::displayText(I_gray, 20, 20, "Click to quit...", vpColor::red);
420  if (vpDisplay::getClick(I_gray, false)) { // exit
421  state = state_quit;
422  }
423  vpDisplay::flush(I_gray);
424 
425  if (opt_use_depth) {
426  vpDisplay::displayText(I_depth, 20, 20, "Click to quit...", vpColor::red);
427  if (vpDisplay::getClick(I_depth, false)) { // exit
428  state = state_quit;
429  }
430  vpDisplay::flush(I_depth);
431  }
432  }
433 
434  if (!display_off) {
435  delete d_gray;
436  if (opt_use_depth)
437  delete d_depth;
438  }
439  } catch (const vpException &e) {
440  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
441  }
442 
443  return EXIT_SUCCESS;
444 #else
445  (void)argc;
446  (void)argv;
447 #ifndef VISP_HAVE_APRILTAG
448  std::cout << "ViSP is not build with Apriltag support" << std::endl;
449 #endif
450 #ifndef VISP_HAVE_REALSENSE2
451  std::cout << "ViSP is not build with librealsense2 support" << std::endl;
452 #endif
453  std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
454 #endif
455  return EXIT_SUCCESS;
456 }
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
void setAprilTagQuadDecimate(float quadDecimate)
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
void setAprilTagNbThreads(int nThreads)
bool detect(const vpImage< unsigned char > &I)
size_t getNbObjects() const
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:87
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)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:117
Real-time 6D object pose tracking using its CAD model.
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setDisplayFeatures(bool displayF)
virtual void setKltMaskBorder(const unsigned int &e)
virtual unsigned int getNbFeaturesEdge() const
virtual void setAngleAppear(const double &a)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual unsigned int getNbFeaturesKlt() const
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void setAngleDisappear(const double &a)
virtual void setMovingEdge(const vpMe &me)
virtual unsigned int getNbFeaturesDepthDense() const
virtual void setKltOpencv(const vpKltOpencv &t)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
Definition: vpMe.h:61
void setMu1(const double &mu_1)
Definition: vpMe.h:241
void setSampleStep(const double &s)
Definition: vpMe.h:278
void setRange(const unsigned int &r)
Definition: vpMe.h:271
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:459
void setMu2(const double &mu_2)
Definition: vpMe.h:248
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:452
void setThreshold(const double &t)
Definition: vpMe.h:300
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())
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const