Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
mbot-apriltag-pbvs.cpp
1 #include <visp3/core/vpSerial.h>
3 #include <visp3/core/vpXmlParserCamera.h>
4 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/io/vpImageIo.h>
7 #include <visp3/robot/vpUnicycle.h>
8 #include <visp3/sensor/vpV4l2Grabber.h>
9 #include <visp3/visual_features/vpFeaturePoint3D.h>
10 #include <visp3/vs/vpServo.h>
11 
12 int main(int argc, const char **argv)
13 {
14 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
15  int device = 0;
18  double tagSize = 0.065;
19  float quad_decimate = 4.0;
20  int nThreads = 2;
21  std::string intrinsic_file = "";
22  std::string camera_name = "";
23  bool display_tag = false;
24  bool display_on = false;
25  bool serial_off = false;
26  bool save_image = false; // Only possible if display_on = true
27 
28  for (int i = 1; i < argc; i++) {
29  if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
30  tagSize = std::atof(argv[i + 1]);
31  }
32  else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
33  device = std::atoi(argv[i + 1]);
34  }
35  else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
36  quad_decimate = (float)atof(argv[i + 1]);
37  }
38  else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
39  nThreads = std::atoi(argv[i + 1]);
40  }
41  else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
42  intrinsic_file = std::string(argv[i + 1]);
43  }
44  else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
45  camera_name = std::string(argv[i + 1]);
46  }
47  else if (std::string(argv[i]) == "--display_tag") {
48  display_tag = true;
49 #if defined(VISP_HAVE_X11)
50  }
51  else if (std::string(argv[i]) == "--display_on") {
52  display_on = true;
53  }
54  else if (std::string(argv[i]) == "--save_image") {
55  save_image = true;
56 #endif
57  }
58  else if (std::string(argv[i]) == "--serial_off") {
59  serial_off = true;
60  }
61  else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
62  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
63  }
64  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
65  std::cout << "Usage: " << argv[0]
66  << " [--input <camera input>] [--tag_size <tag_size in m>]"
67  " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
68  " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
69  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT,"
70  " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
71  " [--display_tag]";
72 #if defined(VISP_HAVE_X11)
73  std::cout << " [--display_on] [--save_image]";
74 #endif
75  std::cout << " [--serial_off] [--help]" << std::endl;
76  return EXIT_SUCCESS;
77  }
78  }
79 
80  // Me Auriga led ring
81  // if serial com ok: led 1 green
82  // if exception: led 1 red
83  // if tag detected: led 2 green, else led 2 red
84  // if motor left: led 3 blue
85  // if motor right: led 4 blue
86 
87  vpSerial *serial = nullptr;
88  if (!serial_off) {
89  serial = new vpSerial("/dev/ttyAMA0", 115200);
90 
91  serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
92  serial->write("LED_RING=1,0,10,0\n"); // Switch on led 1 to green: serial ok
93  }
94 
95  try {
97 
98  vpV4l2Grabber g;
99  std::ostringstream device_name;
100  device_name << "/dev/video" << device;
101  g.setDevice(device_name.str());
102  g.setScale(1);
103  g.acquire(I);
104 
105  vpDisplay *d = nullptr;
106  vpImage<vpRGBa> O;
107 #ifdef VISP_HAVE_X11
108  if (display_on) {
109  d = new vpDisplayX(I);
110  }
111 #endif
112 
113  vpCameraParameters cam;
114  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.);
115 
116 #if defined(VISP_HAVE_PUGIXML)
117  vpXmlParserCamera parser;
118  if (!intrinsic_file.empty() && !camera_name.empty()) {
119  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
120  }
121 #endif
122 
123  std::cout << "cam:\n" << cam << std::endl;
124  std::cout << "tagFamily: " << tagFamily << std::endl;
125 
126  vpDetectorAprilTag detector(tagFamily);
127 
128  detector.setAprilTagQuadDecimate(quad_decimate);
129  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
130  detector.setAprilTagNbThreads(nThreads);
131  detector.setDisplayTag(display_tag);
132 
133  vpServo task;
134  vpAdaptiveGain lambda;
135  if (display_on)
136  lambda.initStandard(2.5, 0.4, 30); // lambda(0)=2.5, lambda(oo)=0.4 and lambda'(0)=30
137  else
138  lambda.initStandard(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
139 
140  vpUnicycle robot;
143  task.setLambda(lambda);
144  vpRotationMatrix cRe;
145  cRe[0][0] = 0;
146  cRe[0][1] = -1;
147  cRe[0][2] = 0;
148  cRe[1][0] = 0;
149  cRe[1][1] = 0;
150  cRe[1][2] = -1;
151  cRe[2][0] = 1;
152  cRe[2][1] = 0;
153  cRe[2][2] = 0;
154 
156  vpVelocityTwistMatrix cVe(cMe);
157  task.set_cVe(cVe);
158 
159  vpMatrix eJe(6, 2, 0);
160  eJe[0][0] = eJe[5][1] = 1.0;
161 
162  std::cout << "eJe: \n" << eJe << std::endl;
163 
164  // Desired distance to the target
165  double Z_d = 0.4;
166  double X = 0, Y = 0, Z = Z_d;
167 
168  // Create X_3D visual features
169  vpFeaturePoint3D s_XZ, s_XZ_d;
170  s_XZ.buildFrom(0, 0, Z_d);
171  s_XZ_d.buildFrom(0, 0, Z_d);
172 
173  // Create Point 3D X, Z coordinates visual features
174  s_XZ.buildFrom(X, Y, Z);
175  s_XZ_d.buildFrom(0, 0, Z_d); // The value of s* is X=Y=0 and Z=Z_d meter
176 
177  // Add the features
179 
180  std::vector<double> time_vec;
181  for (;;) {
182  g.acquire(I);
183 
185 
186  double t = vpTime::measureTimeMs();
187  std::vector<vpHomogeneousMatrix> cMo_vec;
188  detector.detect(I, tagSize, cam, cMo_vec);
189 
190  t = vpTime::measureTimeMs() - t;
191  time_vec.push_back(t);
192 
193  {
194  std::stringstream ss;
195  ss << "Detection time: " << t << " ms";
196  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
197  }
198 
199  if (detector.getNbObjects() == 1) {
200  // Display visual features
201  vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
202  vpDisplay::displayFrame(I, cMo_vec[0], cam, tagSize / 2, vpColor::none, 3);
203  vpDisplay::displayFrame(I, cdMo, cam, tagSize / 3, vpColor::red, 3);
204 
205  if (!serial_off) {
206  serial->write("LED_RING=2,0,10,0\n"); // Switch on led 2 to green: tag detected
207  }
208 
209  X = cMo_vec[0][0][3];
210  Y = cMo_vec[0][1][3];
211  Z = cMo_vec[0][2][3];
212 
213  // Update Point 3D feature
214  s_XZ.set_XYZ(X, Y, Z);
215 
216  std::cout << "X: " << X << " Z: " << Z << std::endl;
217 
218  task.set_cVe(cVe);
219  task.set_eJe(eJe);
220 
221  // Compute the control law. Velocities are computed in the mobile robot reference frame
222  vpColVector v = task.computeControlLaw();
223 
224  std::cout << "Send velocity to the mbot: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
225 
226  task.print();
227  double radius = 0.0325;
228  double L = 0.0725;
229  double motor_left = (-v[0] - L * v[1]) / radius;
230  double motor_right = (v[0] - L * v[1]) / radius;
231  std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
232  if (!serial_off) {
233  // serial->write("LED_RING=3,0,0,10\n"); // Switch on led 3 to blue: motor left servoed
234  // serial->write("LED_RING=4,0,0,10\n"); // Switch on led 4 to blue: motor right servoed
235  }
236  std::stringstream ss;
237  double rpm_left = motor_left * 30. / M_PI;
238  double rpm_right = motor_right * 30. / M_PI;
239  ss << "MOTOR_RPM=" << vpMath::round(rpm_left) << "," << vpMath::round(rpm_right) << "\n";
240  std::cout << "Send: " << ss.str() << std::endl;
241  if (!serial_off) {
242  serial->write(ss.str());
243  }
244  }
245  else {
246  // stop the robot
247  if (!serial_off) {
248  serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected
249  // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed
250  // serial->write("LED_RING=4,0,0,0\n"); // Switch on led 4 to blue: motor right not servoed
251  serial->write("MOTOR_RPM=0,-0\n"); // Stop the robot
252  }
253  }
254 
255  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
256  vpDisplay::flush(I);
257  if (display_on && save_image) {
258  vpDisplay::getImage(I, O);
259  vpImageIo::write(O, "image.png");
260  }
261  if (vpDisplay::getClick(I, false))
262  break;
263  }
264 
265  if (!serial_off) {
266  serial->write("LED_RING=0,0,0,0\n"); // Switch off all led
267  }
268 
269  std::cout << "Benchmark computation time" << std::endl;
270  std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
271  << " ; " << vpMath::getMedian(time_vec) << " ms"
272  << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
273 
274  if (display_on)
275  delete d;
276  if (!serial_off) {
277  delete serial;
278  }
279  }
280  catch (const vpException &e) {
281  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
282  if (!serial_off) {
283  serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red
284  }
285  }
286 
287  return EXIT_SUCCESS;
288 #else
289  (void)argc;
290  (void)argv;
291 #ifndef VISP_HAVE_APRILTAG
292  std::cout << "ViSP is not build with Apriltag support" << std::endl;
293 #endif
294 #ifndef VISP_HAVE_V4L2
295  std::cout << "ViSP is not build with v4l2 support" << std::endl;
296 #endif
297  std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
298  return EXIT_SUCCESS;
299 #endif
300 }
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.
Definition: vpColVector.h:163
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
@ 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:128
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 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)
Definition: vpDisplay.cpp:138
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
Definition: vpException.cpp:64
Class that defines the 3D point visual feature.
static unsigned int selectX()
void set_XYZ(double X, double Y, double Z)
static unsigned int selectZ()
void buildFrom(const vpPoint &p)
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:287
unsigned int getWidth() const
Definition: vpImage.h:245
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:403
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:294
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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:378
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:169
void setLambda(double c)
Definition: vpServo.h:976
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
@ PSEUDO_INVERSE
Definition: vpServo.h:229
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
@ CURRENT
Definition: vpServo.h:196
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
Definition: vpUnicycle.h:50
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()