Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
mbot-apriltag-pbvs.cpp
1
2#include <visp3/core/vpConfig.h>
3#include <visp3/core/vpSerial.h>
4#include <visp3/core/vpXmlParserCamera.h>
5#include <visp3/detection/vpDetectorAprilTag.h>
6#include <visp3/gui/vpDisplayFactory.h>
7#include <visp3/io/vpImageIo.h>
8#include <visp3/robot/vpUnicycle.h>
9#include <visp3/sensor/vpV4l2Grabber.h>
10#include <visp3/visual_features/vpFeaturePoint3D.h>
11#include <visp3/vs/vpServo.h>
12
13void usage(const char **argv, int error)
14{
15 std::cout << "Synopsis" << std::endl
16 << " " << argv[0]
17 << " [--camera-device <id>]"
18 << " [--tag-size <size>]"
19 << " [--tag-family <family>]"
20 << " [--tag-decision-margin-threshold <threshold>]"
21 << " [--tag-hamming-distance-threshold <threshold>]"
22 << " [--tag-quad-decimate <factor>]"
23 << " [--tag-n-threads <number>]"
24 << " [--tag-pose-method <method>]"
25#if defined(VISP_HAVE_PUGIXML)
26 << " [--intrinsic <xmlfile>]"
27 << " [--camera-name <name>]"
28#endif
29#if defined(VISP_HAVE_DISPLAY)
30 << " [--display-tag]"
31 << " [--display-on]"
32 << " [--save-image>]"
33#endif
34 << " [--serial-off]"
35 << " [--help, -h]" << std::endl
36 << std::endl;
37 std::cout << "Description" << std::endl
38 << " Position-based visual servoing using an Apriltag." << std::endl
39 << std::endl
40 << " --camera-device <id>" << std::endl
41 << " Camera device id." << std::endl
42 << " Default: 0" << std::endl
43 << std::endl
44 << " --tag-size <size>" << std::endl
45 << " Apriltag size in [m]." << std::endl
46 << " Default: 0.03" << std::endl
47 << std::endl
48 << " --tag-family <family>" << std::endl
49 << " Apriltag family. Supported values are:" << std::endl
50 << " 0: TAG_36h11" << std::endl
51 << " 1: TAG_36h10 (DEPRECATED)" << std::endl
52 << " 2: TAG_36ARTOOLKIT (DEPRECATED)" << std::endl
53 << " 3: TAG_25h9" << std::endl
54 << " 4: TAG_25h7 (DEPRECATED)" << std::endl
55 << " 5: TAG_16h5" << std::endl
56 << " 6: TAG_CIRCLE21h7" << std::endl
57 << " 7: TAG_CIRCLE49h12" << std::endl
58 << " 8: TAG_CUSTOM48h12" << std::endl
59 << " 9: TAG_STANDARD41h12" << std::endl
60 << " 10: TAG_STANDARD52h13" << std::endl
61 << " 11: TAG_ARUCO_4x4_50" << std::endl
62 << " 12: TAG_ARUCO_4x4_100" << std::endl
63 << " 13: TAG_ARUCO_4x4_250" << std::endl
64 << " 14: TAG_ARUCO_4x4_1000" << std::endl
65 << " 15: TAG_ARUCO_5x5_50" << std::endl
66 << " 16: TAG_ARUCO_5x5_100" << std::endl
67 << " 17: TAG_ARUCO_5x5_250" << std::endl
68 << " 18: TAG_ARUCO_5x5_1000" << std::endl
69 << " 19: TAG_ARUCO_6x6_50" << std::endl
70 << " 20: TAG_ARUCO_6x6_100" << std::endl
71 << " 21: TAG_ARUCO_6x6_250" << std::endl
72 << " 22: TAG_ARUCO_6x6_1000" << std::endl
73 << " 23: TAG_ARUCO_7x7_50" << std::endl
74 << " 24: TAG_ARUCO_7x7_100" << std::endl
75 << " 25: TAG_ARUCO_7x7_250" << std::endl
76 << " 26: TAG_ARUCO_7x7_1000" << std::endl
77 << " 27: TAG_ARUCO_MIP_36h12" << std::endl
78 << " Default: 0 (36h11)" << std::endl
79 << std::endl
80 << " --tag-decision-margin-threshold <threshold>" << std::endl
81 << " Threshold used to discard low-confident detections. A typical value is " << std::endl
82 << " around 100. The higher this value, the more false positives will be filtered" << std::endl
83 << " out. When this value is set to -1, false positives are not filtered out." << std::endl
84 << " Default: 50" << std::endl
85 << std::endl
86 << " --tag-hamming-distance-threshold <threshold>" << std::endl
87 << " Threshold used to discard low-confident detections with corrected bits." << std::endl
88 << " A typical value is between 0 and 3. The lower this value, the more false" << std::endl
89 << " positives will be filtered out." << std::endl
90 << " Default: 0" << std::endl
91 << std::endl
92 << " --tag-quad-decimate <factor>" << std::endl
93 << " Decimation factor used to detect a tag. " << std::endl
94 << " Default: 1" << std::endl
95 << std::endl
96 << " --tag-n-threads <number>" << std::endl
97 << " Number of threads used to detect a tag." << std::endl
98 << " Default: 1" << std::endl
99 << std::endl
100#if defined(VISP_HAVE_PUGIXML)
101 << " --intrinsic <xmlfile>" << std::endl
102 << " Camera intrinsic parameters file in xml format." << std::endl
103 << " Default: empty" << std::endl
104 << std::endl
105 << " --camera-name <name>" << std::endl
106 << " Camera name in the intrinsic parameters file in xml format." << std::endl
107 << " Default: empty" << std::endl
108 << std::endl
109#endif
110#if defined(VISP_HAVE_DISPLAY)
111 << " --display-tag" << std::endl
112 << " Flag used to enable displaying the edges of a tag." << std::endl
113 << " Default: disabled" << std::endl
114 << std::endl
115 << " --display-on" << std::endl
116 << " Flag used to turn display on." << std::endl
117 << " Default: disabled" << std::endl
118 << std::endl
119 << " --save-image" << std::endl
120 << " Flag used to save images with overlay drawings." << std::endl
121 << " Default: disabled" << std::endl
122 << std::endl
123#endif
124 << " --serial-off" << std::endl
125 << " Flag used to disable serial link." << std::endl
126 << " Default: enabled" << std::endl
127 << std::endl
128 << " --help, -h" << std::endl
129 << " Print this helper message." << std::endl
130 << std::endl;
131
132 if (error) {
133 std::cout << "Error" << std::endl
134 << " "
135 << "Unsupported parameter " << argv[error] << std::endl;
136 }
137}
138
139int main(int argc, const char **argv)
140{
141#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
142#ifdef ENABLE_VISP_NAMESPACE
143 using namespace VISP_NAMESPACE_NAME;
144#endif
145
146 int device = 0;
149 double opt_tag_size = 0.065;
150 float opt_tag_quad_decimate = 4.0;
151 float opt_tag_decision_margin_threshold = 50;
152 float opt_tag_hamming_distance_threshold = 2;
153 int opt_tag_nThreads = 2;
154 std::string intrinsic_file = "";
155 std::string camera_name = "";
156 bool display_tag = false;
157 bool display_on = false;
158 bool serial_off = false;
159 bool save_image = false; // Only possible if display_on = true
160
161 for (int i = 1; i < argc; i++) {
162 if (std::string(argv[i]) == "--camera-device" && i + 1 < argc) {
163 device = std::atoi(argv[++i]);
164 }
165 else if (std::string(argv[i]) == "--tag-size" && i + 1 < argc) {
166 opt_tag_size = std::atof(argv[++i]);
167 }
168 else if (std::string(argv[i]) == "--tag-family" && i + 1 < argc) {
169 opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[++i]);
170 }
171 else if (std::string(argv[i]) == "--tag-decision-margin-threshold" && i + 1 < argc) {
172 opt_tag_decision_margin_threshold = static_cast<float>(atof(argv[++i]));
173 }
174 else if (std::string(argv[i]) == "--tag-hamming-distance-threshold" && i + 1 < argc) {
175 opt_tag_hamming_distance_threshold = atoi(argv[++i]);
176 }
177 else if (std::string(argv[i]) == "--tag-quad-decimate" && i + 1 < argc) {
178 opt_tag_quad_decimate = static_cast<float>(atof(argv[++i]));
179 }
180 else if (std::string(argv[i]) == "--tag-n-threads" && i + 1 < argc) {
181 opt_tag_nThreads = std::atoi(argv[++i]);
182 }
183#if defined(VISP_HAVE_PUGIXML)
184 else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
185 intrinsic_file = std::string(argv[++i]);
186 }
187 else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) {
188 camera_name = std::string(argv[++i]);
189 }
190#endif
191#if defined(VISP_HAVE_DISPLAY)
192 else if (std::string(argv[i]) == "--display-tag") {
193 display_tag = true;
194 }
195 else if (std::string(argv[i]) == "--display-on") {
196 display_on = true;
197 }
198 else if (std::string(argv[i]) == "--save-image") {
199 save_image = true;
200#endif
201 }
202 else if (std::string(argv[i]) == "--serial-off") {
203 serial_off = true;
204 }
205 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
206 usage(argv, 0);
207 return EXIT_SUCCESS;
208 }
209 else {
210 usage(argv, i);
211 return EXIT_FAILURE;
212 }
213 }
214
215 // Me Auriga led ring
216 // if serial com ok: led 1 green
217 // if exception: led 1 red
218 // if tag detected: led 2 green, else led 2 red
219 // if motor left: led 3 blue
220 // if motor right: led 4 blue
221
222 vpSerial *serial = nullptr;
223 if (!serial_off) {
224 serial = new vpSerial("/dev/ttyAMA0", 115200);
225
226 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
227 serial->write("LED_RING=1,0,10,0\n"); // Switch on led 1 to green: serial ok
228 }
229
230 try {
232
234 std::ostringstream device_name;
235 device_name << "/dev/video" << device;
236 g.setDevice(device_name.str());
237 g.setScale(1);
238 g.acquire(I);
239
240 vpDisplay *d = nullptr;
242#ifdef VISP_HAVE_DISPLAY
243 if (display_on) {
245 }
246#endif
247
249 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
250
251#if defined(VISP_HAVE_PUGIXML)
253 if (!intrinsic_file.empty() && !camera_name.empty()) {
254 parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
255 }
256#endif
257
258 std::cout << cam << std::endl;
259 std::cout << "Tag detector settings" << std::endl;
260 std::cout << " Tag size [m] : " << opt_tag_size << std::endl;
261 std::cout << " Tag family : " << opt_tag_family << std::endl;
262 std::cout << " Quad decimate : " << opt_tag_quad_decimate << std::endl;
263 std::cout << " Decision margin threshold : " << opt_tag_decision_margin_threshold << std::endl;
264 std::cout << " Hamming distance threshold: " << opt_tag_hamming_distance_threshold << std::endl;
265 std::cout << " Num threads : " << opt_tag_nThreads << std::endl;
266 std::cout << " Pose estimation : " << opt_tag_pose_estimation_method << std::endl;
267
268 vpDetectorAprilTag detector(opt_tag_family);
269
270 detector.setAprilTagQuadDecimate(opt_tag_quad_decimate);
271 detector.setAprilTagPoseEstimationMethod(opt_tag_pose_estimation_method);
272 detector.setAprilTagNbThreads(opt_tag_nThreads);
273 detector.setDisplayTag(display_tag);
274 detector.setAprilTagDecisionMarginThreshold(opt_tag_decision_margin_threshold);
275 detector.setAprilTagHammingDistanceThreshold(opt_tag_hamming_distance_threshold);
276
278 vpAdaptiveGain lambda;
279 if (display_on)
280 lambda.initStandard(2.5, 0.4, 30); // lambda(0)=2.5, lambda(oo)=0.4 and lambda'(0)=30
281 else
282 lambda.initStandard(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
283
284 vpUnicycle robot;
286 task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
287 task.setLambda(lambda);
289 cRe[0][0] = 0;
290 cRe[0][1] = -1;
291 cRe[0][2] = 0;
292 cRe[1][0] = 0;
293 cRe[1][1] = 0;
294 cRe[1][2] = -1;
295 cRe[2][0] = 1;
296 cRe[2][1] = 0;
297 cRe[2][2] = 0;
298
300 vpVelocityTwistMatrix cVe(cMe);
301 task.set_cVe(cVe);
302
303 vpMatrix eJe(6, 2, 0);
304 eJe[0][0] = eJe[5][1] = 1.0;
305
306 std::cout << "eJe: \n" << eJe << std::endl;
307
308 // Desired distance to the target
309 double Z_d = 0.4;
310 double X = 0, Y = 0, Z = Z_d;
311
312 // Create X_3D visual features
313 vpFeaturePoint3D s_XZ, s_XZ_d;
314 s_XZ.buildFrom(0, 0, Z_d);
315 s_XZ_d.buildFrom(0, 0, Z_d);
316
317 // Create Point 3D X, Z coordinates visual features
318 s_XZ.buildFrom(X, Y, Z);
319 s_XZ_d.buildFrom(0, 0, Z_d); // The value of s* is X=Y=0 and Z=Z_d meter
320
321 // Add the features
322 task.addFeature(s_XZ, s_XZ_d, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectZ());
323
324 std::vector<double> time_vec;
325 for (;;) {
326 g.acquire(I);
327
329
330 double t = vpTime::measureTimeMs();
331 std::vector<vpHomogeneousMatrix> cMo_vec;
332 detector.detect(I, opt_tag_size, cam, cMo_vec);
333
335 time_vec.push_back(t);
336
337 {
338 std::stringstream ss;
339 ss << "Detection time: " << t << " ms";
340 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
341 }
342
343 if (detector.getNbObjects() == 1) {
344 // Display visual features
345 vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
346 vpDisplay::displayFrame(I, cMo_vec[0], cam, opt_tag_size / 2, vpColor::none, 3);
347 vpDisplay::displayFrame(I, cdMo, cam, opt_tag_size / 3, vpColor::red, 3);
348
349 if (!serial_off) {
350 serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
351 }
352
353 X = cMo_vec[0][0][3];
354 Y = cMo_vec[0][1][3];
355 Z = cMo_vec[0][2][3];
356
357 // Update Point 3D feature
358 s_XZ.set_XYZ(X, Y, Z);
359
360 std::cout << "X: " << X << " Z: " << Z << std::endl;
361
362 task.set_cVe(cVe);
363 task.set_eJe(eJe);
364
365 // Compute the control law. Velocities are computed in the mobile robot reference frame
366 vpColVector v = task.computeControlLaw();
367
368 std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
369
370 task.print();
371 double radius = 0.0325;
372 double L = 0.0725;
373 double motor_left = (-v[0] - L * v[1]) / radius;
374 double motor_right = (v[0] - L * v[1]) / radius;
375 std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
376 if (!serial_off) {
377 // serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
378 // serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
379 }
380 std::stringstream ss;
381 double rpm_left = motor_left * 30. / M_PI;
382 double rpm_right = motor_right * 30. / M_PI;
383 ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
384 std::cout << "Send: " << ss.str() << std::endl;
385 if (!serial_off) {
386 serial->write(ss.str());
387 }
388 }
389 else {
390 // stop the robot
391 if (!serial_off) {
392 serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
393 // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
394 // serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
395 serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
396 }
397 }
398
399 vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
401 if (display_on && save_image) {
403 vpImageIo::write(O, "image.png");
404 }
405 if (vpDisplay::getClick(I, false)) {
406 break;
407 }
408 }
409
410 if (!serial_off) {
411 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
412 }
413
414 std::cout << "Benchmark computation time" << std::endl;
415 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
416 << " ; " << vpMath::getMedian(time_vec) << " ms"
417 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
418
419 if (display_on) {
420 delete d;
421 }
422 if (!serial_off) {
423 delete serial;
424 }
425 }
426 catch (const vpException &e) {
427 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
428 if (!serial_off) {
429 serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
430 }
431 }
432
433 return EXIT_SUCCESS;
434#else
435 (void)argc;
436 (void)argv;
437#ifndef VISP_HAVE_APRILTAG
438 std::cout << "ViSP is not build with Apriltag support" << std::endl;
439#endif
440#ifndef VISP_HAVE_V4L2
441 std::cout << "ViSP is not build with v4l2 support" << std::endl;
442#endif
443 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
444 return EXIT_SUCCESS;
445#endif
446}
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:198
static const vpColor none
Definition vpColor.h:210
@ TAG_36h11
AprilTag 36h11 pattern (recommended).
Class that defines generic functionalities for display.
Definition vpDisplay.h:171
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 getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
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:60
Class that defines the 3D point visual feature.
static unsigned int selectX()
void set_XYZ(double X, double Y, double Z)
static unsigned int selectZ()
vpFeaturePoint3D & buildFrom(const vpPoint &p)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition of the vpImage class member functions.
Definition vpImage.h:131
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:343
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:374
static int round(double x)
Definition vpMath.h:413
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:323
static double deg(double rad)
Definition vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:175
Implementation of a rotation matrix and operations on such kind of matrices.
void write(const std::string &s)
Definition vpSerial.cpp:332
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:183
@ PSEUDO_INVERSE
Definition vpServo.h:250
@ CURRENT
Definition vpServo.h:217
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
Definition vpUnicycle.h:52
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.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()