Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-mb-generic-tracker-apriltag-webcam.cpp
1
2#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/vpV4l2Grabber.h>
13
14#if defined(HAVE_OPENCV_VIDEOIO)
15#include <opencv2/videoio.hpp>
16#endif
17
18typedef enum { state_detection, state_tracking, state_quit } state_t;
19
20// Creates a cube.cao file in your current directory
21// cubeEdgeSize : size of cube edges in meters
22void createCaoFile(double cubeEdgeSize)
23{
24 std::ofstream fileStream;
25 fileStream.open("cube.cao", std::ofstream::out | std::ofstream::trunc);
26 fileStream << "V1\n";
27 fileStream << "# 3D Points\n";
28 fileStream << "8 # Number of points\n";
29 fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 0: (X, Y, Z)\n";
30 fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 1\n";
31 fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 2\n";
32 fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 3\n";
33 fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 4\n";
34 fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 5\n";
35 fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 6\n";
36 fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 7\n";
37 fileStream << "# 3D Lines\n";
38 fileStream << "0 # Number of lines\n";
39 fileStream << "# Faces from 3D lines\n";
40 fileStream << "0 # Number of faces\n";
41 fileStream << "# Faces from 3D points\n";
42 fileStream << "6 # Number of faces\n";
43 fileStream << "4 0 3 2 1 # Face 0: [number of points] [index of the 3D points]...\n";
44 fileStream << "4 1 2 5 6\n";
45 fileStream << "4 4 7 6 5\n";
46 fileStream << "4 0 7 4 3\n";
47 fileStream << "4 5 2 3 4\n";
48 fileStream << "4 0 1 6 7 # Face 5\n";
49 fileStream << "# 3D cylinders\n";
50 fileStream << "0 # Number of cylinders\n";
51 fileStream << "# 3D circles\n";
52 fileStream << "0 # Number of circles\n";
53 fileStream.close();
54}
55
56#if defined(VISP_HAVE_APRILTAG)
57state_t detectAprilTag(const vpImage<unsigned char> &I, vpDetectorAprilTag &detector, double tagSize,
59{
60 std::vector<vpHomogeneousMatrix> cMo_vec;
61
62 // Detection
63 bool ret = detector.detect(I, tagSize, cam, cMo_vec);
64
65 // Display camera pose
66 for (size_t i = 0; i < cMo_vec.size(); i++) {
67 vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
68 }
69
70 vpDisplay::displayText(I, 40, 20, "State: waiting tag detection", vpColor::red);
71
72 if (ret && detector.getNbObjects() > 0) { // if tag detected, we pick the first one
73 cMo = cMo_vec[0];
74 return state_tracking;
75 }
76
77 return state_detection;
78}
79#endif // #if defined(VISP_HAVE_APRILTAG)
80
81state_t track(const vpImage<unsigned char> &I, vpMbGenericTracker &tracker, double projection_error_threshold,
83{
85 tracker.getCameraParameters(cam);
86
87 // Track the object
88 try {
89 tracker.track(I);
90 }
91 catch (...) {
92 return state_detection;
93 }
94
95 tracker.getPose(cMo);
96
97 // Detect tracking error
98 double projection_error = tracker.computeCurrentProjectionError(I, cMo, cam);
99 if (projection_error > projection_error_threshold) {
100 return state_detection;
101 }
102
103 // Display
104 tracker.display(I, cMo, cam, vpColor::red, 2);
105 vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
106 vpDisplay::displayText(I, 40, 20, "State: tracking in progress", vpColor::red);
107 {
108 std::stringstream ss;
109 ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt();
110 vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
111 }
112
113 return state_tracking;
114}
115
116int main(int argc, const char **argv)
117{
119#if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_V4L2) || defined(HAVE_OPENCV_VIDEOIO)) && defined(VISP_HAVE_MODULE_MBT)
121
122 int opt_device = 0;
124 double opt_tag_size = 0.08;
125 float opt_quad_decimate = 1.0;
126 int opt_nthreads = 1;
127 std::string opt_intrinsic_file = "";
128 std::string opt_camera_name = "";
129 double opt_cube_size = 0.125; // 12.5cm by default
130#ifdef VISP_HAVE_OPENCV
131 bool opt_use_texture = false;
132#endif
133 double opt_projection_error_threshold = 40.;
134
135#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
136 bool display_off = true;
137#else
138 bool display_off = false;
139#endif
140
141 for (int i = 1; i < argc; i++) {
142 if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
143 opt_tag_size = atof(argv[i + 1]);
144 }
145 else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
146 opt_device = atoi(argv[i + 1]);
147 }
148 else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
149 opt_quad_decimate = (float)atof(argv[i + 1]);
150 }
151 else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
152 opt_nthreads = atoi(argv[i + 1]);
153 }
154 else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
155 opt_intrinsic_file = std::string(argv[i + 1]);
156 }
157 else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
158 opt_camera_name = std::string(argv[i + 1]);
159 }
160 else if (std::string(argv[i]) == "--display_off") {
161 display_off = true;
162 }
163 else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
164 opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
165 }
166 else if (std::string(argv[i]) == "--cube_size" && i + 1 < argc) {
167 opt_cube_size = atof(argv[i + 1]);
168#ifdef VISP_HAVE_OPENCV
169 }
170 else if (std::string(argv[i]) == "--texture") {
171 opt_use_texture = true;
172#endif
173 }
174 else if (std::string(argv[i]) == "--projection_error" && i + 1 < argc) {
175 opt_projection_error_threshold = atof(argv[i + 1]);
176 }
177 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
178 std::cout << "Usage: " << argv[0]
179 << " [--input <camera id>] [--cube_size <size in m>] [--tag_size <size in m>]"
180 " [--quad_decimate <decimation>] [--nthreads <nb>]"
181 " [--intrinsic <xml intrinsic file>] [--camera_name <camera name in xml file>]"
182 " [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
183 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
184#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
185 std::cout << " [--display_off]";
186#endif
187 std::cout << " [--texture] [--projection_error <30 - 100>] [--help]" << std::endl;
188 return EXIT_SUCCESS;
189 }
190 }
191
192 createCaoFile(opt_cube_size);
193
195 bool camIsInit = false;
196 vpXmlParserCamera parser;
197 if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
198 parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
199 camIsInit = true;
200 }
201
202 try {
204
206#if defined(VISP_HAVE_V4L2)
208 std::ostringstream device;
209 device << "/dev/video" << opt_device;
210 std::cout << "Use device " << device.str() << " (v4l2 grabber)" << std::endl;
211 g.setDevice(device.str());
212 g.setScale(1);
213 g.acquire(I);
214#elif defined(HAVE_OPENCV_VIDEOIO)
215 std::cout << "Use device " << opt_device << " (OpenCV grabber)" << std::endl;
216 cv::VideoCapture cap(opt_device); // open the default camera
217 if (!cap.isOpened()) { // check if we succeeded
218 std::cout << "Failed to open the camera" << std::endl;
219 return EXIT_FAILURE;
220 }
221 cv::Mat frame;
222 cap >> frame; // get a new frame from camera
223 vpImageConvert::convert(frame, I);
224#endif
225 if (!camIsInit) {
226 cam.initPersProjWithoutDistortion(600, 600, I.getWidth() / 2., I.getHeight() / 2.);
227 }
228
229 std::cout << "Cube size: " << opt_cube_size << std::endl;
230 std::cout << "AprilTag size: " << opt_tag_size << std::endl;
231 std::cout << "AprilTag family: " << opt_tag_family << std::endl;
232 std::cout << "Camera parameters:\n" << cam << std::endl;
233 std::cout << "Detection: " << std::endl;
234 std::cout << " Quad decimate: " << opt_quad_decimate << std::endl;
235 std::cout << " Threads number: " << opt_nthreads << std::endl;
236 std::cout << "Tracker: " << std::endl;
237 std::cout << " Use edges : 1" << std::endl;
238 std::cout << " Use texture: "
239#ifdef VISP_HAVE_OPENCV
240 << opt_use_texture << std::endl;
241#else
242 << " na" << std::endl;
243#endif
244 std::cout << " Projection error: " << opt_projection_error_threshold << std::endl;
245
246 // Construct display
247 vpDisplay *d = NULL;
248 if (!display_off) {
249#ifdef VISP_HAVE_X11
250 d = new vpDisplayX(I);
251#elif defined(VISP_HAVE_GDI)
252 d = new vpDisplayGDI(I);
253#elif defined(HAVE_OPENCV_HIGHGUI)
254 d = new vpDisplayOpenCV(I);
255#endif
256 }
257
258 // Initialize AprilTag detector
259 vpDetectorAprilTag detector(opt_tag_family);
260 detector.setAprilTagQuadDecimate(opt_quad_decimate);
261 detector.setAprilTagNbThreads(opt_nthreads);
262
263 // Prepare MBT
264 vpMbGenericTracker tracker;
265#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
266 if (opt_use_texture)
268 else
269#endif
271 // edges
272 vpMe me;
273 me.setMaskSize(5);
274 me.setMaskNumber(180);
275 me.setRange(12);
277 me.setThreshold(20);
278 me.setMu1(0.5);
279 me.setMu2(0.5);
280 me.setSampleStep(4);
281 tracker.setMovingEdge(me);
282
283#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
284 if (opt_use_texture) {
285 vpKltOpencv klt_settings;
286 klt_settings.setMaxFeatures(300);
287 klt_settings.setWindowSize(5);
288 klt_settings.setQuality(0.015);
289 klt_settings.setMinDistance(8);
290 klt_settings.setHarrisFreeParameter(0.01);
291 klt_settings.setBlockSize(3);
292 klt_settings.setPyramidLevels(3);
293 tracker.setKltOpencv(klt_settings);
294 tracker.setKltMaskBorder(5);
295 }
296#endif
297
298 // camera calibration params
299 tracker.setCameraParameters(cam);
300 // model definition
301 tracker.loadModel("cube.cao");
302 tracker.setDisplayFeatures(true);
303 tracker.setAngleAppear(vpMath::rad(70));
304 tracker.setAngleDisappear(vpMath::rad(80));
305
307 state_t state = state_detection;
308
309 // wait for a tag detection
310 while (state != state_quit) {
311
312#if defined(VISP_HAVE_V4L2)
313 g.acquire(I);
314#elif defined(HAVE_OPENCV_VIDEOIO)
315 cap >> frame;
316 vpImageConvert::convert(frame, I);
317#endif
318
320
321 if (state == state_detection) {
322 state = detectAprilTag(I, detector, opt_tag_size, cam, cMo);
323
324 // Initialize the tracker with the result of the detection
325 if (state == state_tracking) {
327 tracker.initFromPose(I, cMo);
329 }
330 }
331
332 if (state == state_tracking) {
333 state = track(I, tracker, opt_projection_error_threshold, cMo);
334 }
335
336 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
337 if (vpDisplay::getClick(I, false)) { // exit
338 state = state_quit;
339 }
340
342 }
343
344 if (!display_off)
345 delete d;
346 }
347 catch (const vpException &e) {
348 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
349 }
350
351 return EXIT_SUCCESS;
352#else
353 (void)argc;
354 (void)argv;
355#ifndef VISP_HAVE_APRILTAG
356 std::cout << "ViSP is not build with Apriltag support" << std::endl;
357#endif
358#if !(defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_OPENCV))
359 std::cout << "ViSP is not build with v4l2 or OpenCV support" << std::endl;
360#endif
361 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
362#endif
363 return EXIT_SUCCESS;
364}
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
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).
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 convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition vpKltOpencv.h:73
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:116
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 void setKltOpencv(const vpKltOpencv &t)
virtual void setTrackerType(int type)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
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:122
void setMu1(const double &mu_1)
Definition vpMe.h:353
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition vpMe.h:445
void setMaskSize(const unsigned int &a)
Definition vpMe.cpp:452
void setMu2(const double &mu_2)
Definition vpMe.h:360
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
void setMaskNumber(const unsigned int &a)
Definition vpMe.cpp:445
void setThreshold(const double &t)
Definition vpMe.h:435
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)