Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
mbot-apriltag-2D-half-vs.cpp
1
2#include <visp3/core/vpPolygon.h>
3#include <visp3/core/vpSerial.h>
4#include <visp3/core/vpXmlParserCamera.h>
5#include <visp3/detection/vpDetectorAprilTag.h>
6#include <visp3/gui/vpDisplayX.h>
7#include <visp3/io/vpImageIo.h>
8#include <visp3/robot/vpUnicycle.h>
9#include <visp3/sensor/vpV4l2Grabber.h>
10#include <visp3/visual_features/vpFeatureBuilder.h>
11#include <visp3/visual_features/vpFeatureDepth.h>
12#include <visp3/visual_features/vpFeaturePoint.h>
13#include <visp3/vs/vpServo.h>
14
15int main(int argc, const char **argv)
16{
17#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
18 int device = 0;
21 double tagSize = 0.065;
22 float quad_decimate = 4.0;
23 int nThreads = 2;
24 std::string intrinsic_file = "";
25 std::string camera_name = "";
26 bool display_tag = false;
27 bool display_on = false;
28 bool serial_off = false;
29 bool use_pose = true;
30 bool save_image = false; // Only possible if display_on = true
31
32 for (int i = 1; i < argc; i++) {
33 if (std::string(argv[i]) == "--without_pose_computation") {
34 use_pose = false;
35 } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
36 tagSize = std::atof(argv[i + 1]);
37 } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
38 device = std::atoi(argv[i + 1]);
39 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
40 quad_decimate = (float)atof(argv[i + 1]);
41 } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
42 nThreads = std::atoi(argv[i + 1]);
43 } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
44 intrinsic_file = std::string(argv[i + 1]);
45 } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
46 camera_name = std::string(argv[i + 1]);
47 } else if (std::string(argv[i]) == "--display_tag") {
48 display_tag = true;
49#if defined(VISP_HAVE_X11)
50 } else if (std::string(argv[i]) == "--display_on") {
51 display_on = true;
52 } else if (std::string(argv[i]) == "--save_image") {
53 save_image = true;
54#endif
55 } else if (std::string(argv[i]) == "--serial_off") {
56 serial_off = true;
57 } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
58 tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
59 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
60 std::cout << "Usage: " << argv[0]
61 << " [--input <camera input>] [--tag_size <tag_size in m>]"
62 " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
63 " [--intrinsic <intrinsic file>] [--camera_name <camera name>] [--without_pose_computation]"
64 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT,"
65 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
66 " [--display_tag]";
67#if defined(VISP_HAVE_X11)
68 std::cout << " [--display_on] [--save_image]";
69#endif
70 std::cout << " [--serial_off] [--help]" << std::endl;
71 return EXIT_SUCCESS;
72 }
73 }
74
75 // Me Auriga led ring
76 // if serial com ok: led 1 green
77 // if exception: led 1 red
78 // if tag detected: led 2 green, else led 2 red
79 // if motor left: led 3 blue
80 // if motor right: led 4 blue
81
82 vpSerial *serial = NULL;
83 if (!serial_off) {
84 serial = new vpSerial("/dev/ttyAMA0", 115200);
85
86 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
87 serial->write("LED_RING=1,0,10,0\n"); // Switch on led 1 to green: serial ok
88 }
89
90 try {
92
94 std::ostringstream device_name;
95 device_name << "/dev/video" << device;
96 g.setDevice(device_name.str());
97 g.setScale(1);
98 g.acquire(I);
99
100 vpDisplay *d = NULL;
102#ifdef VISP_HAVE_X11
103 if (display_on) {
104 d = new vpDisplayX(I);
105 }
106#endif
107
109 cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
110 vpXmlParserCamera parser;
111 if (!intrinsic_file.empty() && !camera_name.empty())
112 parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
113
114 std::cout << "cam:\n" << cam << std::endl;
115 std::cout << "use pose: " << use_pose << std::endl;
116 std::cout << "tagFamily: " << tagFamily << std::endl;
117
118 vpDetectorAprilTag detector(tagFamily);
119
120 detector.setAprilTagQuadDecimate(quad_decimate);
121 if (use_pose)
122 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
123 detector.setAprilTagNbThreads(nThreads);
124 detector.setDisplayTag(display_tag);
125
126 vpServo task;
127 vpAdaptiveGain lambda;
128 if (display_on)
129 lambda.initStandard(2.5, 0.4, 30); // lambda(0)=2.5, lambda(oo)=0.4 and lambda'(0)=30
130 else
131 lambda.initStandard(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
132
133 vpUnicycle robot;
136 task.setLambda(lambda);
138 cRe[0][0] = 0;
139 cRe[0][1] = -1;
140 cRe[0][2] = 0;
141 cRe[1][0] = 0;
142 cRe[1][1] = 0;
143 cRe[1][2] = -1;
144 cRe[2][0] = 1;
145 cRe[2][1] = 0;
146 cRe[2][2] = 0;
147
149 vpVelocityTwistMatrix cVe(cMe);
150 task.set_cVe(cVe);
151
152 vpMatrix eJe(6, 2, 0);
153 eJe[0][0] = eJe[5][1] = 1.0;
154
155 std::cout << "eJe: \n" << eJe << std::endl;
156
157 // Current and desired visual feature associated to the x coordinate of the point
158 vpFeaturePoint s_x, s_xd;
159 vpImagePoint cog;
160 double Z, Z_d;
161 Z = Z_d = 0.4;
162
163 // Create the current x visual feature
164 vpFeatureBuilder::create(s_x, cam, cog);
165
166 // Create the desired x* visual feature
167 s_xd.buildFrom(0, 0, Z_d);
168
169 // Add the point feature
170 task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());
171
172 // Create the log(Z/Z*) visual feature
173 vpFeatureDepth s_Z, s_Z_d;
174
175 std::cout << "Z " << Z << std::endl;
176 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, 0); // log(Z/Z*) = 0 that's why the last parameter is 0
177 s_Z_d.buildFrom(0, 0, Z_d, 0); // The value of s* is 0 with Z=1 meter
178
179 // Add the feature
180 task.addFeature(s_Z, s_Z_d);
181
182 std::vector<double> time_vec;
183 for (;;) {
184 g.acquire(I);
185
187
188 double t = vpTime::measureTimeMs();
189 std::vector<vpHomogeneousMatrix> cMo_vec;
190 if (use_pose)
191 detector.detect(I, tagSize, cam, cMo_vec);
192 else
193 detector.detect(I);
194
195 t = vpTime::measureTimeMs() - t;
196 time_vec.push_back(t);
197
198 {
199 std::stringstream ss;
200 ss << "Detection time: " << t << " ms";
201 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
202 }
203
204 if (detector.getNbObjects() == 1) {
205 // Display visual features
206 vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
207 vpDisplay::displayFrame(I, cdMo, cam, tagSize / 3, vpColor::red, 3);
208 vpDisplay::displayCross(I, detector.getCog(0), 15, vpColor::green,
209 3); // Current polygon used to compure an moment
210 vpDisplay::displayLine(I, 0, cam.get_u0(), I.getHeight() - 1, cam.get_u0(), vpColor::red,
211 3); // Vertical line as desired x position
212 if (use_pose) {
213 // Display visual features
214 vpDisplay::displayFrame(I, cMo_vec[0], cam, tagSize / 2, vpColor::none, 3);
215 }
216
217 if (!serial_off) {
218 serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
219 }
220
221 if (use_pose) {
222 Z = cMo_vec[0][2][3];
223 } else {
224 vpPolygon polygon(detector.getPolygon(0));
225 double surface = polygon.getArea();
226 std::cout << "Surface: " << surface << std::endl;
227
228 // Compute the distance from target surface and 3D size
229 Z = tagSize * cam.get_px() / sqrt(surface);
230 }
231
232 vpFeatureBuilder::create(s_x, cam, detector.getCog(0));
233 s_x.set_Z(Z);
234
235 // Update log(Z/Z*) feature
236 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Z_d));
237
238 std::cout << "cog: " << detector.getCog(0) << " Z: " << Z << std::endl;
239
240 task.set_cVe(cVe);
241 task.set_eJe(eJe);
242
243 // Compute the control law. Velocities are computed in the mobile robot reference frame
245
246 std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
247
248 task.print();
249 double radius = 0.0325;
250 double L = 0.0725;
251 double motor_left = (-v[0] - L * v[1]) / radius;
252 double motor_right = (v[0] - L * v[1]) / radius;
253 std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
254 if (!serial_off) {
255 // serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
256 // serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
257 }
258 std::stringstream ss;
259 double rpm_left = motor_left * 30. / M_PI;
260 double rpm_right = motor_right * 30. / M_PI;
261 ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
262 std::cout << "Send: " << ss.str() << std::endl;
263 if (!serial_off) {
264 serial->write(ss.str());
265 }
266 } else {
267 // stop the robot
268 if (!serial_off) {
269 serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
270 // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
271 // serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
272 serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
273 }
274 }
275
276 vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
278 if (display_on && save_image) {
280 vpImageIo::write(O, "image.png");
281 }
282 if (vpDisplay::getClick(I, false))
283 break;
284 }
285
286 if (!serial_off) {
287 serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
288 }
289
290 std::cout << "Benchmark computation time" << std::endl;
291 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
292 << " ; " << vpMath::getMedian(time_vec) << " ms"
293 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
294
295 if (display_on)
296 delete d;
297 if (!serial_off) {
298 delete serial;
299 }
300 } catch (const vpException &e) {
301 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
302 if (!serial_off) {
303 serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
304 }
305 }
306
307 return EXIT_SUCCESS;
308#else
309 (void)argc;
310 (void)argv;
311#ifndef VISP_HAVE_APRILTAG
312 std::cout << "ViSP is not build with Apriltag support" << std::endl;
313#endif
314#ifndef VISP_HAVE_V4L2
315 std::cout << "ViSP is not build with v4l2 support" << std::endl;
316#endif
317 std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
318 return EXIT_SUCCESS;
319#endif
320}
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 none
Definition vpColor.h:223
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 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 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)
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void buildFrom(double x, double y, double Z, double LogZoverZstar)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
static unsigned int selectX()
double get_y() const
double get_x() const
void set_Z(double Z)
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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
Defines a generic 2D polygon.
Definition vpPolygon.h:97
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
@ PSEUDO_INVERSE
Definition vpServo.h:199
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()