Visual Servoing Platform  version 3.5.1 under development (2022-11-26)
mbot-apriltag-ibvs.cpp
1 #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 
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;
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
294  vpColVector v = task.computeControlLaw();
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);
327  vpDisplay::flush(I);
328 
329  if (display_on && save_image) {
330  vpDisplay::getImage(I, O);
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)
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:87
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:292
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:262
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:292
static int round(double x)
Definition: vpMath.h:318
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:242
static double deg(double rad)
Definition: vpMath.h:110
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:316
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:564
@ 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:303
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:215
vpColVector computeControlLaw()
Definition: vpServo.cpp:926
@ CURRENT
Definition: vpServo.h:182
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: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()