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>
13void usage(
const char **argv,
int error)
15 std::cout <<
"Synopsis" << std::endl
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>]"
29#if defined(VISP_HAVE_DISPLAY)
35 <<
" [--help, -h]" << std::endl
37 std::cout <<
"Description" << std::endl
38 <<
" Position-based visual servoing using an Apriltag." << std::endl
40 <<
" --camera-device <id>" << std::endl
41 <<
" Camera device id." << std::endl
42 <<
" Default: 0" << std::endl
44 <<
" --tag-size <size>" << std::endl
45 <<
" Apriltag size in [m]." << std::endl
46 <<
" Default: 0.03" << 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
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
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
92 <<
" --tag-quad-decimate <factor>" << std::endl
93 <<
" Decimation factor used to detect a tag. " << std::endl
94 <<
" Default: 1" << 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
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
105 <<
" --camera-name <name>" << std::endl
106 <<
" Camera name in the intrinsic parameters file in xml format." << std::endl
107 <<
" Default: empty" << std::endl
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
115 <<
" --display-on" << std::endl
116 <<
" Flag used to turn display on." << std::endl
117 <<
" Default: disabled" << std::endl
119 <<
" --save-image" << std::endl
120 <<
" Flag used to save images with overlay drawings." << std::endl
121 <<
" Default: disabled" << std::endl
124 <<
" --serial-off" << std::endl
125 <<
" Flag used to disable serial link." << std::endl
126 <<
" Default: enabled" << std::endl
128 <<
" --help, -h" << std::endl
129 <<
" Print this helper message." << std::endl
133 std::cout <<
"Error" << std::endl
135 <<
"Unsupported parameter " << argv[
error] << std::endl;
139int main(
int argc,
const char **argv)
141#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
142#ifdef ENABLE_VISP_NAMESPACE
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 =
"";
156 bool display_tag =
false;
157 bool display_on =
false;
158 bool serial_off =
false;
159 bool save_image =
false;
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]);
165 else if (std::string(argv[i]) ==
"--tag-size" && i + 1 < argc) {
166 opt_tag_size = std::atof(argv[++i]);
168 else if (std::string(argv[i]) ==
"--tag-family" && i + 1 < argc) {
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]));
174 else if (std::string(argv[i]) ==
"--tag-hamming-distance-threshold" && i + 1 < argc) {
175 opt_tag_hamming_distance_threshold = atoi(argv[++i]);
177 else if (std::string(argv[i]) ==
"--tag-quad-decimate" && i + 1 < argc) {
178 opt_tag_quad_decimate =
static_cast<float>(atof(argv[++i]));
180 else if (std::string(argv[i]) ==
"--tag-n-threads" && i + 1 < argc) {
181 opt_tag_nThreads = std::atoi(argv[++i]);
183#if defined(VISP_HAVE_PUGIXML)
184 else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
185 intrinsic_file = std::string(argv[++i]);
187 else if (std::string(argv[i]) ==
"--camera-name" && i + 1 < argc) {
191#if defined(VISP_HAVE_DISPLAY)
192 else if (std::string(argv[i]) ==
"--display-tag") {
195 else if (std::string(argv[i]) ==
"--display-on") {
198 else if (std::string(argv[i]) ==
"--save-image") {
202 else if (std::string(argv[i]) ==
"--serial-off") {
205 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
224 serial =
new vpSerial(
"/dev/ttyAMA0", 115200);
226 serial->
write(
"LED_RING=0,0,0,0\n");
227 serial->
write(
"LED_RING=1,0,10,0\n");
234 std::ostringstream device_name;
235 device_name <<
"/dev/video" << device;
242#ifdef VISP_HAVE_DISPLAY
249 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
251#if defined(VISP_HAVE_PUGIXML)
253 if (!intrinsic_file.empty() && !
camera_name.empty()) {
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;
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);
287 task.setLambda(lambda);
304 eJe[0][0] = eJe[5][1] = 1.0;
306 std::cout <<
"eJe: \n" << eJe << std::endl;
310 double X = 0, Y = 0, Z = Z_d;
324 std::vector<double> time_vec;
331 std::vector<vpHomogeneousMatrix> cMo_vec;
332 detector.detect(I, opt_tag_size, cam, cMo_vec);
335 time_vec.push_back(t);
338 std::stringstream ss;
339 ss <<
"Detection time: " <<
t <<
" ms";
343 if (detector.getNbObjects() == 1) {
350 serial->
write(
"LED_RING=2,0,10,0\n");
353 X = cMo_vec[0][0][3];
354 Y = cMo_vec[0][1][3];
355 Z = cMo_vec[0][2][3];
360 std::cout <<
"X: " << X <<
" Z: " << Z << std::endl;
368 std::cout <<
"Send velocity to the mbot: " <<
v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
371 double radius = 0.0325;
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;
380 std::stringstream ss;
381 double rpm_left = motor_left * 30. / M_PI;
382 double rpm_right = motor_right * 30. / M_PI;
384 std::cout <<
"Send: " << ss.str() << std::endl;
386 serial->
write(ss.str());
392 serial->
write(
"LED_RING=2,10,0,0\n");
395 serial->
write(
"MOTOR_RPM=0,-0\n");
401 if (display_on && save_image) {
411 serial->
write(
"LED_RING=0,0,0,0\n");
414 std::cout <<
"Benchmark computation time" << std::endl;
415 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
427 std::cerr <<
"Catch an exception: " <<
e.getMessage() << std::endl;
429 serial->
write(
"LED_RING=1,10,0,0\n");
437#ifndef VISP_HAVE_APRILTAG
438 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
440#ifndef VISP_HAVE_V4L2
441 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
443 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
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 none
@ TAG_36h11
AprilTag 36h11 pattern (recommended).
Class that defines generic functionalities for display.
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.
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.
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static int round(double x)
static double getMean(const std::vector< double > &v)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Implementation of a rotation matrix and operations on such kind of matrices.
void write(const std::string &s)
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
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()