Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
mbot-apriltag-2D-half-vs.cpp
1 #include <visp3/core/vpXmlParserCamera.h>
3 #include <visp3/core/vpSerial.h>
4 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/sensor/vpV4l2Grabber.h>
7 #include <visp3/io/vpImageIo.h>
8 #include <visp3/visual_features/vpFeatureBuilder.h>
9 #include <visp3/visual_features/vpFeatureDepth.h>
10 #include <visp3/visual_features/vpFeaturePoint.h>
11 #include <visp3/vs/vpServo.h>
12 #include <visp3/robot/vpUnicycle.h>
13 #include <visp3/core/vpPolygon.h>
14 
15 int 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 
93  vpV4l2Grabber g;
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;
101  vpImage<vpRGBa> O;
102 #ifdef VISP_HAVE_X11
103  if (display_on) {
104  d = new vpDisplayX(I);
105  }
106 #endif
107 
108  vpCameraParameters cam;
109  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
110  #ifdef VISP_HAVE_XML2
111  vpXmlParserCamera parser;
112  if (!intrinsic_file.empty() && !camera_name.empty())
113  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
114  #endif
115  std::cout << "cam:\n" << cam << std::endl;
116  std::cout << "use pose: " << use_pose << std::endl;
117  std::cout << "tagFamily: " << tagFamily << std::endl;
118 
119  vpDetectorAprilTag detector(tagFamily);
120 
121  detector.setAprilTagQuadDecimate(quad_decimate);
122  if (use_pose)
123  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
124  detector.setAprilTagNbThreads(nThreads);
125  detector.setDisplayTag(display_tag);
126 
127  vpServo task;
128  vpAdaptiveGain lambda;
129  if (display_on)
130  lambda.initStandard(2.5, 0.4, 30); // lambda(0)=2.5, lambda(oo)=0.4 and lambda'(0)=30
131  else
132  lambda.initStandard(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
133 
134  vpUnicycle robot;
137  task.setLambda(lambda);
138  vpRotationMatrix cRe;
139  cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
140  cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
141  cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
142 
144  vpVelocityTwistMatrix cVe(cMe);
145  task.set_cVe(cVe);
146 
147  vpMatrix eJe(6, 2, 0);
148  eJe[0][0] = eJe[5][1] = 1.0;
149 
150  std::cout << "eJe: \n" << eJe << std::endl;
151 
152  // Current and desired visual feature associated to the x coordinate of the point
153  vpFeaturePoint s_x, s_xd;
154  vpImagePoint cog;
155  double Z, Z_d;
156  Z = Z_d = 0.4;
157 
158  // Create the current x visual feature
159  vpFeatureBuilder::create(s_x, cam, cog);
160 
161  // Create the desired x* visual feature
162  s_xd.buildFrom(0, 0, Z_d);
163 
164  // Add the point feature
165  task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());
166 
167  // Create the log(Z/Z*) visual feature
168  vpFeatureDepth s_Z, s_Z_d;
169 
170  std::cout << "Z " << Z << std::endl;
171  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
172  s_Z_d.buildFrom(0, 0, Z_d, 0); // The value of s* is 0 with Z=1 meter
173 
174  // Add the feature
175  task.addFeature(s_Z, s_Z_d);
176 
177  std::vector<double> time_vec;
178  for (;;) {
179  g.acquire(I);
180 
182 
183  double t = vpTime::measureTimeMs();
184  std::vector<vpHomogeneousMatrix> cMo_vec;
185  if (use_pose)
186  detector.detect(I, tagSize, cam, cMo_vec);
187  else
188  detector.detect(I);
189 
190  t = vpTime::measureTimeMs() - t;
191  time_vec.push_back(t);
192 
193  std::stringstream ss;
194  ss << "Detection time: " << t << " ms";
195  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
196 
197  if (detector.getNbObjects() == 1) {
198  // Display visual features
199  vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
200  vpDisplay::displayFrame(I, cdMo, cam, tagSize / 3, vpColor::red, 3);
201  vpDisplay::displayCross(I, detector.getCog(0), 15, vpColor::green, 3); // Current polygon used to compure an moment
202  vpDisplay::displayLine(I, 0, cam.get_u0(), I.getHeight()-1, cam.get_u0(), vpColor::red, 3); // Vertical line as desired x position
203  if (use_pose) {
204  // Display visual features
205  vpDisplay::displayFrame(I, cMo_vec[0], cam, tagSize / 2, vpColor::none, 3);
206  }
207 
208  if (! serial_off) {
209  serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
210  }
211 
212  if (use_pose) {
213  Z = cMo_vec[0][2][3];
214  }
215  else {
216  vpPolygon polygon(detector.getPolygon(0));
217  double surface = polygon.getArea();
218  std::cout << "Surface: " << surface << std::endl;
219 
220  // Compute the distance from target surface and 3D size
221  Z = tagSize * cam.get_px() / sqrt(surface);
222  }
223 
224  vpFeatureBuilder::create(s_x, cam, detector.getCog(0));
225  s_x.set_Z(Z);
226 
227  // Update log(Z/Z*) feature
228  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Z_d));
229 
230  std::cout << "cog: " << detector.getCog(0) << " Z: " << Z << std::endl;
231 
232  task.set_cVe(cVe);
233  task.set_eJe(eJe);
234 
235  // Compute the control law. Velocities are computed in the mobile robot reference frame
236  vpColVector v = task.computeControlLaw();
237 
238  std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
239 
240  task.print();
241  double radius = 0.0325;
242  double L = 0.0725;
243  double motor_left = (-v[0] - L * v[1]) / radius;
244  double motor_right = ( v[0] - L * v[1]) / radius;
245  std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
246  if (! serial_off) {
247 // serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
248 // serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
249  }
250  std::stringstream ss;
251  double rpm_left = motor_left * 30. / M_PI;
252  double rpm_right = motor_right * 30. / M_PI;
253  ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
254  std::cout << "Send: " << ss.str() << std::endl;
255  if (! serial_off) {
256  serial->write(ss.str());
257  }
258  }
259  else {
260  // stop the robot
261  if (! serial_off) {
262  serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
263 // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
264 // serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
265  serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
266  }
267  }
268 
269  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
270  vpDisplay::flush(I);
271  if (display_on && save_image) {
272  vpDisplay::getImage(I, O);
273  vpImageIo::write(O, "image.png");
274  }
275  if (vpDisplay::getClick(I, false))
276  break;
277  }
278 
279  if (! serial_off) {
280  serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
281  }
282 
283  std::cout << "Benchmark computation time" << std::endl;
284  std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
285  << " ; " << vpMath::getMedian(time_vec) << " ms"
286  << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
287 
288  if (display_on)
289  delete d;
290  if (! serial_off) {
291  delete serial;
292  }
293  } catch (const vpException &e) {
294  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
295  if (! serial_off) {
296  serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
297  }
298  }
299 
300  return EXIT_SUCCESS;
301 #else
302  (void)argc;
303  (void)argv;
304 #ifndef VISP_HAVE_APRILTAG
305  std::cout << "ViSP is not build with Apriltag support" << std::endl;
306 #endif
307 #ifndef VISP_HAVE_V4L2
308  std::cout << "ViSP is not build with v4l2 support" << std::endl;
309 #endif
310  std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
311  return EXIT_SUCCESS;
312 #endif
313 }
void setAprilTagQuadDecimate(const float quadDecimate)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void acquire(vpImage< unsigned char > &I)
Adaptive gain computation.
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:171
double get_u0() const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static double getStdev(const std::vector< double > &v, const bool useBesselCorrection=false)
Definition: vpMath.cpp:252
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
unsigned int getWidth() const
Definition: vpImage.h:239
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
void setDevice(const std::string &devname)
static const vpColor none
Definition: vpColor.h:192
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
size_t getNbObjects() const
XML parser to load and save intrinsic camera parameters.
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
void write(const std::string &s)
Definition: vpSerial.cpp:343
static int round(const double x)
Definition: vpMath.h:235
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
static const vpColor red
Definition: vpColor.h:180
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static void write(const vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:375
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:202
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
static unsigned int selectX()
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:406
const char * getMessage(void) const
Definition: vpException.cpp:90
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
double get_px() const
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:574
void buildFrom(const double x, const double y, const double Z)
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:95
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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))
double get_y() const
double get_x() const
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
unsigned int getHeight() const
Definition: vpImage.h:178
void set_Z(const double Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
Class that consider the case of a translation vector.
void setAprilTagNbThreads(const int nThreads)
std::vector< std::vector< vpImagePoint > > & getPolygon()
void setDisplayTag(const bool display, const vpColor &color=vpColor::none, const unsigned int thickness=2)
bool detect(const vpImage< unsigned char > &I)