Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
mbot-apriltag-ibvs.cpp
1
2#include <visp3/core/vpMomentAreaNormalized.h>
3#include <visp3/core/vpMomentBasic.h>
4#include <visp3/core/vpMomentCentered.h>
5#include <visp3/core/vpMomentDatabase.h>
6#include <visp3/core/vpMomentGravityCenter.h>
7#include <visp3/core/vpMomentGravityCenterNormalized.h>
8#include <visp3/core/vpMomentObject.h>
9#include <visp3/core/vpPixelMeterConversion.h>
10#include <visp3/core/vpPoint.h>
11#include <visp3/core/vpSerial.h>
12#include <visp3/core/vpXmlParserCamera.h>
13#include <visp3/detection/vpDetectorAprilTag.h>
14#include <visp3/gui/vpDisplayX.h>
15#include <visp3/io/vpImageIo.h>
16#include <visp3/robot/vpUnicycle.h>
17#include <visp3/sensor/vpV4l2Grabber.h>
18#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
19#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
20#include <visp3/vs/vpServo.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;
141 cRe[0][1] = -1;
142 cRe[0][2] = 0;
143 cRe[1][0] = 0;
144 cRe[1][1] = 0;
145 cRe[1][2] = -1;
146 cRe[2][0] = 1;
147 cRe[2][1] = 0;
148 cRe[2][2] = 0;
149
151 vpVelocityTwistMatrix cVe(cMe);
152 task.set_cVe(cVe);
153
154 vpMatrix eJe(6, 2, 0);
155 eJe[0][0] = eJe[5][1] = 1.0;
156
157 std::cout << "eJe: \n" << eJe << std::endl;
158
159 // Desired distance to the target
160 double Z_d = 0.4;
161
162 // Define the desired polygon corresponding the the AprilTag CLOCKWISE
163 double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.};
164 double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.};
165 std::vector<vpPoint> vec_P, vec_P_d;
166
167 for (int i = 0; i < 4; i++) {
168 vpPoint P_d(X[i], Y[i], 0);
169 vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
170 P_d.track(cdMo); //
171 vec_P_d.push_back(P_d);
172 }
173
174 vpMomentObject m_obj(3), m_obj_d(3);
175 vpMomentDatabase mdb, mdb_d;
176 vpMomentBasic mb_d; // Here only to get the desired area m00
177 vpMomentGravityCenter mg, mg_d;
178 vpMomentCentered mc, mc_d;
179 vpMomentAreaNormalized man(0, Z_d),
180 man_d(0, Z_d); // Declare normalized area. Desired area parameter will be updated below with m00
181 vpMomentGravityCenterNormalized mgn, mgn_d; // Declare normalized gravity center
182
183 // Desired moments
184 m_obj_d.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
185 m_obj_d.fromVector(vec_P_d); // Initialize the object with the points coordinates
186
187 mb_d.linkTo(mdb_d); // Add basic moments to database
188 mg_d.linkTo(mdb_d); // Add gravity center to database
189 mc_d.linkTo(mdb_d); // Add centered moments to database
190 man_d.linkTo(mdb_d); // Add area normalized to database
191 mgn_d.linkTo(mdb_d); // Add gravity center normalized to database
192 mdb_d.updateAll(m_obj_d); // All of the moments must be updated, not just an_d
193 mg_d.compute(); // Compute gravity center moment
194 mc_d.compute(); // Compute centered moments AFTER gravity center
195
196 double area = 0;
197 if (m_obj_d.getType() == vpMomentObject::DISCRETE)
198 area = mb_d.get(2, 0) + mb_d.get(0, 2);
199 else
200 area = mb_d.get(0, 0);
201 // Update an moment with the desired area
202 man_d.setDesiredArea(area);
203
204 man_d.compute(); // Compute area normalized moment AFTER centered moments
205 mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment
206
207 // Desired plane
208 double A = 0.0;
209 double B = 0.0;
210 double C = 1.0 / Z_d;
211
212 // Construct area normalized features
213 vpFeatureMomentGravityCenterNormalized s_mgn(mdb, A, B, C), s_mgn_d(mdb_d, A, B, C);
214 vpFeatureMomentAreaNormalized s_man(mdb, A, B, C), s_man_d(mdb_d, A, B, C);
215
216 // Add the features
218 task.addFeature(s_man, s_man_d);
219
220 // Update desired gravity center normalized feature
221 s_mgn_d.update(A, B, C);
222 s_mgn_d.compute_interaction();
223 // Update desired area normalized feature
224 s_man_d.update(A, B, C);
225 s_man_d.compute_interaction();
226
227 std::vector<double> time_vec;
228 for (;;) {
229 g.acquire(I);
230
232
233 double t = vpTime::measureTimeMs();
234 std::vector<vpHomogeneousMatrix> cMo_vec;
235 detector.detect(I, tagSize, cam, cMo_vec);
236 t = vpTime::measureTimeMs() - t;
237 time_vec.push_back(t);
238
239 {
240 std::stringstream ss;
241 ss << "Detection time: " << t << " ms";
242 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
243 }
244
245 if (detector.getNbObjects() == 1) {
246 if (!serial_off) {
247 serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
248 }
249
250 // Update current points used to compute the moments
251 std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
252 vec_P.clear();
253 for (size_t i = 0; i < vec_ip.size(); i++) { // size = 4
254 double x = 0, y = 0;
255 vpPixelMeterConversion::convertPoint(cam, vec_ip[i], x, y);
256 vpPoint P;
257 P.set_x(x);
258 P.set_y(y);
259 vec_P.push_back(P);
260 }
261
262 // Display visual features
263 vpDisplay::displayPolygon(I, vec_ip, vpColor::green, 3); // Current polygon used to compure an moment
264 vpDisplay::displayCross(I, detector.getCog(0), 15, vpColor::green,
265 3); // Current polygon used to compure an moment
266 vpDisplay::displayLine(I, 0, cam.get_u0(), I.getHeight() - 1, cam.get_u0(), vpColor::red,
267 3); // Vertical line as desired x position
268
269 // Current moments
270 m_obj.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
271 m_obj.fromVector(vec_P); // Initialize the object with the points coordinates
272
273 mg.linkTo(mdb); // Add gravity center to database
274 mc.linkTo(mdb); // Add centered moments to database
275 man.linkTo(mdb); // Add area normalized to database
276 mgn.linkTo(mdb); // Add gravity center normalized to database
277 mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d
278 mg.compute(); // Compute gravity center moment
279 mc.compute(); // Compute centered moments AFTER gravity center
280 man.setDesiredArea(
281 area); // Since desired area was init at 0, because unknow at contruction, need to be updated here
282 man.compute(); // Compute area normalized moment AFTER centered moment
283 mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment
284
285 s_mgn.update(A, B, C);
286 s_mgn.compute_interaction();
287 s_man.update(A, B, C);
288 s_man.compute_interaction();
289
290 task.set_cVe(cVe);
291 task.set_eJe(eJe);
292
293 // Compute the control law. Velocities are computed in the mobile robot reference frame
295
296 std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
297
298 task.print();
299 double radius = 0.0325;
300 double L = 0.0725;
301 double motor_left = (-v[0] - L * v[1]) / radius;
302 double motor_right = (v[0] - L * v[1]) / radius;
303 std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
304 if (!serial_off) {
305 // serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
306 // serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
307 }
308 std::stringstream ss;
309 double rpm_left = motor_left * 30. / M_PI;
310 double rpm_right = motor_right * 30. / M_PI;
311 ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
312 std::cout << "Send: " << ss.str() << std::endl;
313 if (!serial_off) {
314 serial->write(ss.str());
315 }
316 } else {
317 // stop the robot
318 if (!serial_off) {
319 serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
320 // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
321 // serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
322 serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
323 }
324 }
325
326 vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
328
329 if (display_on && save_image) {
331 vpImageIo::write(O, "image.png");
332 }
333 if (vpDisplay::getClick(I, false))
334 break;
335 }
336
337 if (!serial_off) {
338 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
339 }
340
341 std::cout << "Benchmark computation time" << std::endl;
342 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
343 << " ; " << vpMath::getMedian(time_vec) << " ms"
344 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
345
346 if (display_on)
347 delete d;
348 if (!serial_off) {
349 delete serial;
350 }
351 } catch (const vpException &e) {
352 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
353 if (!serial_off) {
354 serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
355 }
356 }
357
358 return EXIT_SUCCESS;
359#else
360 (void)argc;
361 (void)argv;
362#ifndef VISP_HAVE_APRILTAG
363 std::cout << "ViSP is not build with Apriltag support" << std::endl;
364#endif
365#ifndef VISP_HAVE_V4L2
366 std::cout << "ViSP is not build with v4l2 support" << std::endl;
367#endif
368 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
369 return EXIT_SUCCESS;
370#endif
371}
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)
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
@ 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: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 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)
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 emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
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 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
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static int round(double x)
Definition vpMath.h:323
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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...
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:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Implementation of a rotation matrix and operations on such kind of matrices.
void write(const std::string &s)
Definition vpSerial.cpp:313
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:155
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:299
void setLambda(double c)
Definition vpServo.h:403
void set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
Definition vpUnicycle.h:54
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)
VISP_EXPORT double measureTimeMs()