Visual Servoing Platform  version 3.4.0
mbot-apriltag-ibvs.cpp
1 #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 
22 int 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 
97  vpV4l2Grabber g;
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;
105  vpImage<vpRGBa> O;
106 #ifdef VISP_HAVE_X11
107  if (display_on) {
108  d = new vpDisplayX(I);
109  }
110 #endif
111 
112  vpCameraParameters cam;
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);
139  vpRotationMatrix cRe;
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
284  vpColVector v = task.computeControlLaw();
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);
318  vpDisplay::flush(I);
319 
320  if (display_on && save_image) {
321  vpDisplay::getImage(I, O);
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:151
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)
Definition: vpImageIo.cpp:445
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:220
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:250
static int round(double x)
Definition: vpMath.h:245
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:200
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:497
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:499
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()