Visual Servoing Platform version 3.5.0
tutorial-mb-generic-tracker-apriltag-webcam.cpp
1
2#include <fstream>
3#include <ios>
4#include <iostream>
5
6#include <visp3/gui/vpDisplayGDI.h>
7#include <visp3/gui/vpDisplayOpenCV.h>
8#include <visp3/gui/vpDisplayX.h>
9#include <visp3/core/vpXmlParserCamera.h>
10#include <visp3/sensor/vpV4l2Grabber.h>
11#include <visp3/detection/vpDetectorAprilTag.h>
12#include <visp3/mbt/vpMbGenericTracker.h>
13
14typedef enum {
15 state_detection,
16 state_tracking,
17 state_quit
18} 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,
58 double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
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,
82 double projection_error_threshold, vpHomogeneousMatrix &cMo)
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(VISP_HAVE_OPENCV)) && \
120 defined(VISP_HAVE_MODULE_MBT)
122
123 int opt_device = 0;
125 double opt_tag_size = 0.08;
126 float opt_quad_decimate = 1.0;
127 int opt_nthreads = 1;
128 std::string opt_intrinsic_file = "";
129 std::string opt_camera_name = "";
130 double opt_cube_size = 0.125; // 12.5cm by default
131#ifdef VISP_HAVE_OPENCV
132 bool opt_use_texture = false;
133#endif
134 double opt_projection_error_threshold = 40.;
135
136#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
137 bool display_off = true;
138#else
139 bool display_off = false;
140#endif
141
142 for (int i = 1; i < argc; i++) {
143 if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
144 opt_tag_size = atof(argv[i + 1]);
145 } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
146 opt_device = atoi(argv[i + 1]);
147 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
148 opt_quad_decimate = (float)atof(argv[i + 1]);
149 } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
150 opt_nthreads = atoi(argv[i + 1]);
151 } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
152 opt_intrinsic_file = std::string(argv[i + 1]);
153 } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
154 opt_camera_name = std::string(argv[i + 1]);
155 } else if (std::string(argv[i]) == "--display_off") {
156 display_off = true;
157 } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
158 opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
159 } else if (std::string(argv[i]) == "--cube_size" && i + 1 < argc) {
160 opt_cube_size = atof(argv[i + 1]);
161#ifdef VISP_HAVE_OPENCV
162 } else if (std::string(argv[i]) == "--texture") {
163 opt_use_texture = true;
164#endif
165 } else if (std::string(argv[i]) == "--projection_error" && i + 1 < argc) {
166 opt_projection_error_threshold = atof(argv[i + 1]);
167 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
168 std::cout << "Usage: " << argv[0] << " [--input <camera id>] [--cube_size <size in m>] [--tag_size <size in m>]"
169 " [--quad_decimate <decimation>] [--nthreads <nb>]"
170 " [--intrinsic <xml intrinsic file>] [--camera_name <camera name in xml file>]"
171 " [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
172 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
173#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
174 std::cout << " [--display_off]";
175#endif
176 std::cout << " [--texture] [--projection_error <30 - 100>] [--help]" << std::endl;
177 return EXIT_SUCCESS;
178 }
179 }
180
181 createCaoFile(opt_cube_size);
182
184 bool camIsInit = false;
185 vpXmlParserCamera parser;
186 if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
187 parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
188 camIsInit = true;
189 }
190
191 try {
193
195#if defined(VISP_HAVE_V4L2)
197 std::ostringstream device;
198 device << "/dev/video" << opt_device;
199 std::cout << "Use device " << device.str() << " (v4l2 grabber)" << std::endl;
200 g.setDevice(device.str());
201 g.setScale(1);
202 g.acquire(I);
203#elif defined(VISP_HAVE_OPENCV)
204 std::cout << "Use device " << opt_device << " (OpenCV grabber)" << std::endl;
205 cv::VideoCapture cap(opt_device); // open the default camera
206 if (!cap.isOpened()) { // check if we succeeded
207 std::cout << "Failed to open the camera" << std::endl;
208 return EXIT_FAILURE;
209 }
210 cv::Mat frame;
211 cap >> frame; // get a new frame from camera
212 vpImageConvert::convert(frame, I);
213#endif
214 if (!camIsInit) {
215 cam.initPersProjWithoutDistortion(600, 600, I.getWidth() / 2., I.getHeight() / 2.);
216 }
217
218 std::cout << "Cube size: " << opt_cube_size << std::endl;
219 std::cout << "AprilTag size: " << opt_tag_size << std::endl;
220 std::cout << "AprilTag family: " << opt_tag_family << std::endl;
221 std::cout << "Camera parameters:\n" << cam << std::endl;
222 std::cout << "Detection: " << std::endl;
223 std::cout << " Quad decimate: " << opt_quad_decimate << std::endl;
224 std::cout << " Threads number: " << opt_nthreads << std::endl;
225 std::cout << "Tracker: " << std::endl;
226 std::cout << " Use edges : 1"<< std::endl;
227 std::cout << " Use texture: "
228#ifdef VISP_HAVE_OPENCV
229 << opt_use_texture << std::endl;
230#else
231 << " na" << std::endl;
232#endif
233 std::cout << " Projection error: " << opt_projection_error_threshold << std::endl;
234
235 // Construct display
236 vpDisplay *d = NULL;
237 if (!display_off) {
238#ifdef VISP_HAVE_X11
239 d = new vpDisplayX(I);
240#elif defined(VISP_HAVE_GDI)
241 d = new vpDisplayGDI(I);
242#elif defined(VISP_HAVE_OPENCV)
243 d = new vpDisplayOpenCV(I);
244#endif
245 }
246
247 // Initialize AprilTag detector
248 vpDetectorAprilTag detector(opt_tag_family);
249 detector.setAprilTagQuadDecimate(opt_quad_decimate);
250 detector.setAprilTagNbThreads(opt_nthreads);
251
252 // Prepare MBT
253 vpMbGenericTracker tracker;
254#ifdef VISP_HAVE_OPENCV
255 if (opt_use_texture)
257 else
258#endif
260 // edges
261 vpMe me;
262 me.setMaskSize(5);
263 me.setMaskNumber(180);
264 me.setRange(12);
265 me.setThreshold(10000);
266 me.setMu1(0.5);
267 me.setMu2(0.5);
268 me.setSampleStep(4);
269 tracker.setMovingEdge(me);
270
271#ifdef VISP_HAVE_OPENCV
272 if (opt_use_texture) {
273 vpKltOpencv klt_settings;
274 klt_settings.setMaxFeatures(300);
275 klt_settings.setWindowSize(5);
276 klt_settings.setQuality(0.015);
277 klt_settings.setMinDistance(8);
278 klt_settings.setHarrisFreeParameter(0.01);
279 klt_settings.setBlockSize(3);
280 klt_settings.setPyramidLevels(3);
281 tracker.setKltOpencv(klt_settings);
282 tracker.setKltMaskBorder(5);
283 }
284#endif
285
286 // camera calibration params
287 tracker.setCameraParameters(cam);
288 // model definition
289 tracker.loadModel("cube.cao");
290 tracker.setDisplayFeatures(true);
291 tracker.setAngleAppear(vpMath::rad(70));
292 tracker.setAngleDisappear(vpMath::rad(80));
293
295 state_t state = state_detection;
296
297 // wait for a tag detection
298 while (state != state_quit) {
299
300#if defined(VISP_HAVE_V4L2)
301 g.acquire(I);
302#elif defined(VISP_HAVE_OPENCV)
303 cap >> frame;
304 vpImageConvert::convert(frame, I);
305#endif
306
308
309 if (state == state_detection) {
310 state = detectAprilTag(I, detector, opt_tag_size, cam, cMo);
311
312 // Initialize the tracker with the result of the detection
313 if (state == state_tracking) {
315 tracker.initFromPose(I, cMo);
317 }
318 }
319
320 if (state == state_tracking) {
321 state = track(I, tracker, opt_projection_error_threshold, cMo);
322 }
323
324 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
325 if (vpDisplay::getClick(I, false)) { // exit
326 state = state_quit;
327 }
328
330 }
331
332 if (!display_off)
333 delete d;
334 } catch (const vpException &e) {
335 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
336 }
337
338 return EXIT_SUCCESS;
339#else
340 (void)argc;
341 (void)argv;
342#ifndef VISP_HAVE_APRILTAG
343 std::cout << "ViSP is not build with Apriltag support" << std::endl;
344#endif
345#if !(defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_OPENCV))
346 std::cout << "ViSP is not build with v4l2 or OpenCV support" << std::endl;
347#endif
348 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
349#endif
350 return EXIT_SUCCESS;
351}
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
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:90
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
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:110
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: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
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)