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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void acquire(vpImage< unsigned char > &I)
Adaptive gain computation.
Class handling the normalized surface moment that is invariant in scale and used to estimate depth...
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:177
double get_u0() const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
This class defines the 2D basic moment . This class is a wrapper for vpMomentObject wich allows to us...
Definition: vpMomentBasic.h:74
unsigned int getWidth() const
Definition: vpImage.h:246
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
Class describing 2D normalized gravity center moment.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:220
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:250
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
void linkTo(vpMomentDatabase &moments)
Definition: vpMoment.cpp:98
void setDevice(const std::string &devname)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
const std::vector< double > & get() const
size_t getNbObjects() const
XML parser to load and save intrinsic camera parameters.
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
void write(const std::string &s)
Definition: vpSerial.cpp:343
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:497
Implementation of a rotation matrix and operations on such kind of matrices.
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:499
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
void setAprilTagQuadDecimate(float quadDecimate)
static void write(const vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:445
Generic functions for unicycle mobile robots.
Definition: vpUnicycle.h:56
vpImagePoint getCog(size_t i) const
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:200
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
This class allows to register all vpMoments so they can access each other according to their dependen...
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setAprilTagNbThreads(int nThreads)
void setLambda(double c)
Definition: vpServo.h:404
virtual void updateAll(vpMomentObject &object)
This class defines the double-indexed centered moment descriptor .
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
Class describing 2D gravity center moment.
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Definition: vpDisplay.cpp:144
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
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)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static int round(double x)
Definition: vpMath.h:245
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
static double deg(double rad)
Definition: vpMath.h:103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
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
unsigned int getHeight() const
Definition: vpImage.h:188
const char * getMessage() const
Definition: vpException.cpp:90
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that consider the case of a translation vector.
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)