Visual Servoing Platform version 3.5.0
mbot-apriltag-ibvs.cpp
1
2#include <visp3/core/vpSerial.h>
3#include <visp3/core/vpXmlParserCamera.h>
4#include <visp3/core/vpMomentObject.h>
5#include <visp3/core/vpPoint.h>
6#include <visp3/core/vpMomentBasic.h>
7#include <visp3/core/vpMomentGravityCenter.h>
8#include <visp3/core/vpMomentDatabase.h>
9#include <visp3/core/vpMomentCentered.h>
10#include <visp3/core/vpMomentAreaNormalized.h>
11#include <visp3/core/vpMomentGravityCenterNormalized.h>
12#include <visp3/core/vpPixelMeterConversion.h>
13#include <visp3/detection/vpDetectorAprilTag.h>
14#include <visp3/gui/vpDisplayX.h>
15#include <visp3/sensor/vpV4l2Grabber.h>
16#include <visp3/io/vpImageIo.h>
17#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
18#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
19#include <visp3/vs/vpServo.h>
20#include <visp3/robot/vpUnicycle.h>
21
22int main(int argc, const char **argv)
23{
24#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
25 int device = 0;
27 double tagSize = 0.065;
28 float quad_decimate = 4.0;
29 int nThreads = 2;
30 std::string intrinsic_file = "";
31 std::string camera_name = "";
32 bool display_tag = false;
33 bool display_on = false;
34 bool serial_off = false;
35 bool save_image = false; // Only possible if display_on = true
36
37 for (int i = 1; i < argc; i++) {
38 if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
39 tagSize = std::atof(argv[i + 1]);
40 } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
41 device = std::atoi(argv[i + 1]);
42 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
43 quad_decimate = (float)atof(argv[i + 1]);
44 } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
45 nThreads = std::atoi(argv[i + 1]);
46 } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
47 intrinsic_file = std::string(argv[i + 1]);
48 } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
49 camera_name = std::string(argv[i + 1]);
50 } else if (std::string(argv[i]) == "--display_tag") {
51 display_tag = true;
52#if defined(VISP_HAVE_X11)
53 } else if (std::string(argv[i]) == "--display_on") {
54 display_on = true;
55 } else if (std::string(argv[i]) == "--save_image") {
56 save_image = true;
57#endif
58 } else if (std::string(argv[i]) == "--serial_off") {
59 serial_off = true;
60 } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
61 tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)std::atoi(argv[i + 1]);
62 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
63 std::cout << "Usage: " << argv[0]
64 << " [--input <camera input>] [--tag_size <tag_size in m>]"
65 " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
66 " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
67 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: "
68 "TAG_36ARTOOLKIT,"
69 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
70 " [--display_tag]";
71#if defined(VISP_HAVE_X11)
72 std::cout << " [--display_on] [--save_image]";
73#endif
74 std::cout << " [--serial_off] [--help]" << std::endl;
75 return EXIT_SUCCESS;
76 }
77 }
78
79 // Me Auriga led ring
80 // if serial com ok: led 1 green
81 // if exception: led 1 red
82 // if tag detected: led 2 green, else led 2 red
83 // if motor left: led 3 blue
84 // if motor right: led 4 blue
85
86 vpSerial *serial = NULL;
87 if (! serial_off) {
88 serial = new vpSerial("/dev/ttyAMA0", 115200);
89
90 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
91 serial->write("LED_RING=1,0,10,0\n"); // Switch on led 1 to green: serial ok
92 }
93
94 try {
96
98 std::ostringstream device_name;
99 device_name << "/dev/video" << device;
100 g.setDevice(device_name.str());
101 g.setScale(1);
102 g.acquire(I);
103
104 vpDisplay *d = NULL;
106#ifdef VISP_HAVE_X11
107 if (display_on) {
108 d = new vpDisplayX(I);
109 }
110#endif
111
113 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
114 vpXmlParserCamera parser;
115 if (!intrinsic_file.empty() && !camera_name.empty())
116 parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
117
118 std::cout << "cam:\n" << cam << std::endl;
119 std::cout << "tagFamily: " << tagFamily << std::endl;
120 std::cout << "tagSize: " << tagSize << std::endl;
121
122 vpDetectorAprilTag detector(tagFamily);
123
124 detector.setAprilTagQuadDecimate(quad_decimate);
125 detector.setAprilTagNbThreads(nThreads);
126 detector.setDisplayTag(display_tag);
127
128 vpServo task;
129 vpAdaptiveGain lambda;
130 if (display_on)
131 lambda.initStandard(2.5, 0.4, 30); // lambda(0)=2.5, lambda(oo)=0.4 and lambda'(0)=30
132 else
133 lambda.initStandard(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
134
135 vpUnicycle robot;
138 task.setLambda(lambda);
140 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
141 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
142 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
143
145 vpVelocityTwistMatrix cVe(cMe);
146 task.set_cVe(cVe);
147
148 vpMatrix eJe(6, 2, 0);
149 eJe[0][0] = eJe[5][1] = 1.0;
150
151 std::cout << "eJe: \n" << eJe << std::endl;
152
153 // Desired distance to the target
154 double Z_d = 0.4;
155
156 // Define the desired polygon corresponding the the AprilTag CLOCKWISE
157 double X[4] = {tagSize/2., tagSize/2., -tagSize/2., -tagSize/2.};
158 double Y[4] = {tagSize/2., -tagSize/2., -tagSize/2., tagSize/2.};
159 std::vector<vpPoint> vec_P, vec_P_d;
160
161 for (int i = 0; i < 4; i++) {
162 vpPoint P_d(X[i], Y[i], 0);
163 vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
164 P_d.track(cdMo); //
165 vec_P_d.push_back(P_d);
166 }
167
168 vpMomentObject m_obj(3), m_obj_d(3);
169 vpMomentDatabase mdb, mdb_d;
170 vpMomentBasic mb_d; // Here only to get the desired area m00
171 vpMomentGravityCenter mg, mg_d;
172 vpMomentCentered mc, mc_d;
173 vpMomentAreaNormalized man(0, Z_d), man_d(0, Z_d); // Declare normalized area. Desired area parameter will be updated below with m00
174 vpMomentGravityCenterNormalized mgn, mgn_d; // Declare normalized gravity center
175
176 // Desired moments
177 m_obj_d.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
178 m_obj_d.fromVector(vec_P_d); // Initialize the object with the points coordinates
179
180 mb_d.linkTo(mdb_d); // Add basic moments to database
181 mg_d.linkTo(mdb_d); // Add gravity center to database
182 mc_d.linkTo(mdb_d); // Add centered moments to database
183 man_d.linkTo(mdb_d); // Add area normalized to database
184 mgn_d.linkTo(mdb_d); // Add gravity center normalized to database
185 mdb_d.updateAll(m_obj_d); // All of the moments must be updated, not just an_d
186 mg_d.compute(); // Compute gravity center moment
187 mc_d.compute(); // Compute centered moments AFTER gravity center
188
189 double area = 0;
190 if (m_obj_d.getType() == vpMomentObject::DISCRETE)
191 area = mb_d.get(2, 0) + mb_d.get(0, 2);
192 else
193 area = mb_d.get(0, 0);
194 // Update an moment with the desired area
195 man_d.setDesiredArea(area);
196
197 man_d.compute(); // Compute area normalized moment AFTER centered moments
198 mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment
199
200 // Desired plane
201 double A = 0.0;
202 double B = 0.0;
203 double C = 1.0 / Z_d;
204
205 // Construct area normalized features
206 vpFeatureMomentGravityCenterNormalized s_mgn(mdb, A, B, C), s_mgn_d(mdb_d, A, B, C);
207 vpFeatureMomentAreaNormalized s_man(mdb, A, B, C), s_man_d(mdb_d, A, B, C);
208
209 // Add the features
211 task.addFeature(s_man, s_man_d);
212
213 // Update desired gravity center normalized feature
214 s_mgn_d.update(A, B, C);
215 s_mgn_d.compute_interaction();
216 // Update desired area normalized feature
217 s_man_d.update(A, B, C);
218 s_man_d.compute_interaction();
219
220 std::vector<double> time_vec;
221 for (;;) {
222 g.acquire(I);
223
225
226 double t = vpTime::measureTimeMs();
227 std::vector<vpHomogeneousMatrix> cMo_vec;
228 detector.detect(I, tagSize, cam, cMo_vec);
229 t = vpTime::measureTimeMs() - t;
230 time_vec.push_back(t);
231
232 {
233 std::stringstream ss;
234 ss << "Detection time: " << t << " ms";
235 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
236 }
237
238 if (detector.getNbObjects() == 1) {
239 if (! serial_off) {
240 serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
241 }
242
243 // Update current points used to compute the moments
244 std::vector< vpImagePoint > vec_ip = detector.getPolygon(0);
245 vec_P.clear();
246 for (size_t i = 0; i < vec_ip.size(); i++) { // size = 4
247 double x = 0, y = 0;
248 vpPixelMeterConversion::convertPoint(cam, vec_ip[i], x, y);
249 vpPoint P;
250 P.set_x(x);
251 P.set_y(y);
252 vec_P.push_back(P);
253 }
254
255 // Display visual features
256 vpDisplay::displayPolygon(I, vec_ip, vpColor::green, 3); // Current polygon used to compure an moment
257 vpDisplay::displayCross(I, detector.getCog(0), 15, vpColor::green, 3); // Current polygon used to compure an moment
258 vpDisplay::displayLine(I, 0, cam.get_u0(), I.getHeight()-1, cam.get_u0(), vpColor::red, 3); // Vertical line as desired x position
259
260 // Current moments
261 m_obj.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
262 m_obj.fromVector(vec_P); // Initialize the object with the points coordinates
263
264 mg.linkTo(mdb); // Add gravity center to database
265 mc.linkTo(mdb); // Add centered moments to database
266 man.linkTo(mdb); // Add area normalized to database
267 mgn.linkTo(mdb); // Add gravity center normalized to database
268 mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d
269 mg.compute(); // Compute gravity center moment
270 mc.compute(); // Compute centered moments AFTER gravity center
271 man.setDesiredArea(area); // Since desired area was init at 0, because unknow at contruction, need to be updated here
272 man.compute(); // Compute area normalized moment AFTER centered moment
273 mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment
274
275 s_mgn.update(A, B, C);
276 s_mgn.compute_interaction();
277 s_man.update(A, B, C);
278 s_man.compute_interaction();
279
280 task.set_cVe(cVe);
281 task.set_eJe(eJe);
282
283 // Compute the control law. Velocities are computed in the mobile robot reference frame
285
286 std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
287
288 task.print();
289 double radius = 0.0325;
290 double L = 0.0725;
291 double motor_left = (-v[0] - L * v[1]) / radius;
292 double motor_right = ( v[0] - L * v[1]) / radius;
293 std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
294 if (! serial_off) {
295// serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
296// serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
297 }
298 std::stringstream ss;
299 double rpm_left = motor_left * 30. / M_PI;
300 double rpm_right = motor_right * 30. / M_PI;
301 ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
302 std::cout << "Send: " << ss.str() << std::endl;
303 if (! serial_off) {
304 serial->write(ss.str());
305 }
306 }
307 else {
308 // stop the robot
309 if (! serial_off) {
310 serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
311// serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
312// serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
313 serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
314 }
315 }
316
317 vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
319
320 if (display_on && save_image) {
322 vpImageIo::write(O, "image.png");
323 }
324 if (vpDisplay::getClick(I, false))
325 break;
326 }
327
328 if (! serial_off) {
329 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
330 }
331
332 std::cout << "Benchmark computation time" << std::endl;
333 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
334 << " ; " << vpMath::getMedian(time_vec) << " ms"
335 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
336
337 if (display_on)
338 delete d;
339 if (! serial_off) {
340 delete serial;
341 }
342 } catch (const vpException &e) {
343 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
344 if (! serial_off) {
345 serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
346 }
347 }
348
349 return EXIT_SUCCESS;
350#else
351 (void)argc;
352 (void)argv;
353#ifndef VISP_HAVE_APRILTAG
354 std::cout << "ViSP is not build with Apriltag support" << std::endl;
355#endif
356#ifndef VISP_HAVE_V4L2
357 std::cout << "ViSP is not build with v4l2 support" << std::endl;
358#endif
359 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
360 return EXIT_SUCCESS;
361#endif
362}
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:220
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
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 displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Definition: vpDisplay.cpp:144
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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)
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:90
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
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: vpImageIo.cpp:293
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
static int round(double x)
Definition: vpMath.h:247
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
This class defines the 2D basic moment . This class is a wrapper for vpMomentObject wich allows to us...
Definition: vpMomentBasic.h:75
const std::vector< double > & get() const
This class defines the double-indexed centered moment descriptor .
This class allows to register all vpMoments so they can access each other according to their dependen...
virtual void updateAll(vpMomentObject &object)
Class describing 2D normalized gravity center moment.
Class describing 2D gravity center moment.
Class for generic objects.
void linkTo(vpMomentDatabase &moments)
Definition: vpMoment.cpp:98
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
Implementation of a rotation matrix and operations on such kind of matrices.
void write(const std::string &s)
Definition: vpSerial.cpp:343
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ CURRENT
Definition: vpServo.h:182
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
Definition: vpUnicycle.h:57
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)
VISP_EXPORT double measureTimeMs()