Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
servoBebop2.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Example that shows how to do visual servoing with Parrot Bebop 2 drone in ViSP.
33  *
34  * Authors:
35  * Gatien Gaumerais
36  *
37 *****************************************************************************/
38 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpMomentAreaNormalized.h>
43 #include <visp3/core/vpMomentBasic.h>
44 #include <visp3/core/vpMomentCentered.h>
45 #include <visp3/core/vpMomentDatabase.h>
46 #include <visp3/core/vpMomentGravityCenter.h>
47 #include <visp3/core/vpMomentGravityCenterNormalized.h>
48 #include <visp3/core/vpMomentObject.h>
49 #include <visp3/core/vpPixelMeterConversion.h>
50 #include <visp3/core/vpPoint.h>
51 #include <visp3/core/vpTime.h>
52 #include <visp3/core/vpXmlParserCamera.h>
53 #include <visp3/detection/vpDetectorAprilTag.h>
54 #include <visp3/gui/vpDisplayX.h>
55 #include <visp3/gui/vpPlot.h>
56 #include <visp3/robot/vpRobotBebop2.h>
57 #include <visp3/visual_features/vpFeatureBuilder.h>
58 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
59 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
60 #include <visp3/visual_features/vpFeatureVanishingPoint.h>
61 #include <visp3/vs/vpServo.h>
62 #include <visp3/vs/vpServoDisplay.h>
63 
64 #if !defined(VISP_HAVE_ARSDK)
65 int main()
66 {
67  std::cout << "\nThis example requires Parrot ARSDK3 library. You should install it.\n" << std::endl;
68  return EXIT_SUCCESS;
69 }
70 #elif !defined(VISP_HAVE_FFMPEG)
71 int main()
72 {
73  std::cout << "\nThis example requires ffmpeg library. You should install it.\n" << std::endl;
74  return EXIT_SUCCESS;
75 }
76 #elif !defined(VISP_HAVE_PUGIXML)
77 int main()
78 {
79  std::cout << "\nThis example requires pugixml built-in 3rdparty library. You should enable it.\n" << std::endl;
80  return EXIT_SUCCESS;
81 }
82 
83 #else
84 
85 bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
86 {
87  return (p1.second.get_v() < p2.second.get_v());
88 };
89 
101 int main(int argc, char **argv)
102 {
103  try {
104 
105  std::string ip_address = "192.168.42.1";
106  std::string opt_cam_parameters;
107  bool opt_has_cam_parameters = false;
108 
109  double tagSize = -1;
110 
111  double opt_distance_to_tag = -1;
112  bool opt_has_distance_to_tag = false;
113 
114  int stream_res = 0; // Default 480p resolution
115 
116  bool verbose = false;
117 
118  if (argc >= 3 && std::string(argv[1]) == "--tag_size") {
119  tagSize = std::atof(argv[2]); // Tag size option is required
120  if (tagSize <= 0) {
121  std::cout << "Error : invalid tag size." << std::endl << "See " << argv[0] << " --help" << std::endl;
122  return EXIT_FAILURE;
123  }
124  for (int i = 3; i < argc; i++) {
125  if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
126  ip_address = std::string(argv[i + 1]);
127  i++;
128  }
129  else if (std::string(argv[i]) == "--distance_to_tag" && i + 1 < argc) {
130  opt_distance_to_tag = std::atof(argv[i + 1]);
131  if (opt_distance_to_tag <= 0) {
132  std::cout << "Error : invalid distance to tag." << std::endl << "See " << argv[0] << " --help" << std::endl;
133  return EXIT_FAILURE;
134  }
135  opt_has_distance_to_tag = true;
136  i++;
137  }
138  else if (std::string(argv[i]) == "--intrinsic") {
139 
140  opt_cam_parameters = std::string(argv[i + 1]);
141  opt_has_cam_parameters = true;
142  i++;
143  }
144  else if (std::string(argv[i]) == "--hd_stream") {
145  stream_res = 1;
146  }
147  else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
148  verbose = true;
149  }
150  else {
151  std::cout << "Error : unknown parameter " << argv[i] << std::endl
152  << "See " << argv[0] << " --help" << std::endl;
153  return EXIT_FAILURE;
154  }
155  }
156  }
157  else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) {
158  std::cout << "\nUsage:\n"
159  << " " << argv[0]
160  << " [--tag_size <size>] [--ip <drone ip>] [--distance_to_tag <distance>] [--intrinsic <xml file>] "
161  << "[--hd_stream] [--verbose] [-v] [--help] [-h]\n"
162  << std::endl
163  << "Description:\n"
164  << " --tag_size <size>\n"
165  << " The size of the tag to detect in meters, required.\n\n"
166  << " --ip <drone ip>\n"
167  << " Ip address of the drone to which you want to connect (default : 192.168.42.1).\n\n"
168  << " --distance_to_tag <distance>\n"
169  << " The desired distance to the tag in meters (default: 1 meter).\n\n"
170  << " --intrinsic <xml file>\n"
171  << " XML file containing computed intrinsic camera parameters (default: empty).\n\n"
172  << " --hd_stream\n"
173  << " Enables HD 720p streaming instead of default 480p.\n"
174  << " Allows to increase range and accuracy of the tag detection,\n"
175  << " but increases latency and computation time.\n"
176  << " Caution: camera calibration settings are different for the two resolutions.\n"
177  << " Make sure that if you pass custom intrinsic camera parameters,\n"
178  << " they were obtained with the correct resolution.\n\n"
179  << " --verbose, -v\n"
180  << " Enables verbose (drone information messages and velocity commands\n"
181  << " are then displayed).\n\n"
182  << " --help, -h\n"
183  << " Print help message.\n"
184  << std::endl;
185  return EXIT_SUCCESS;
186  }
187  else {
188  std::cout << "Error : tag size parameter required." << std::endl << "See " << argv[0] << " --help" << std::endl;
189  return EXIT_FAILURE;
190  }
191 
192  std::cout
193  << "\nWARNING: \n - This program does no sensing or avoiding of "
194  "obstacles, \n"
195  "the drone WILL collide with any objects in the way! Make sure "
196  "the \n"
197  "drone has approximately 3 meters of free space on all sides.\n"
198  " - The drone uses a downward-facing camera for horizontal speed estimation,\n make sure the drone flies "
199  "above a non-uniform flooring,\n or its movement will be inacurate and dangerous !\n"
200 
201  << std::endl;
202 
203  vpRobotBebop2 drone(
204  verbose, true, ip_address); // Create the drone with desired verbose level, settings reset, and corresponding IP
205 
206  if (drone.isRunning()) {
207 
208  drone.setVideoResolution(stream_res); // Set video resolution to 480p (default) or 720p
209 
210  drone.setStreamingMode(0); // Set lowest latency stream mode
211  drone.setVideoStabilisationMode(0); // Disable video stabilisation
212 
213  drone.doFlatTrim(); // Flat trim calibration
214 
215  drone.startStreaming(); // Start streaming and decoding video data
216 
217  drone.setExposure(1.5f); // Set exposure to max so that the aprilTag detection is more efficient
218 
219  drone.setCameraOrientation(-5., 0.,
220  true); // Set camera to look slightly down so that the drone is slightly above the tag
221 
222  drone.takeOff(true); // Take off
223 
225  drone.getGrayscaleImage(I);
226 
227 #if defined(VISP_HAVE_X11)
229 #elif defined(VISP_HAVE_GTK)
231 #elif defined(VISP_HAVE_GDI)
233 #elif defined(HAVE_OPENCV_HIGHGUI)
235 #endif
236  int orig_displayX = 100;
237  int orig_displayY = 100;
238  display.init(I, orig_displayX, orig_displayY, "DRONE VIEW");
240  vpDisplay::flush(I);
241 
242  vpPlot plotter(1, 700, 700, orig_displayX + static_cast<int>(I.getWidth()) + 20, orig_displayY,
243  "Visual servoing tasks");
244  unsigned int iter = 0;
245 
247  vpDetectorAprilTag detector(tagFamily); // The detector used to detect Apritags
248  detector.setAprilTagQuadDecimate(4.0);
249  detector.setAprilTagNbThreads(4);
250  detector.setDisplayTag(true);
251 
252  vpCameraParameters cam;
253 
254  if (opt_has_cam_parameters) {
255 
258 
259  if (p.parse(cam, opt_cam_parameters, "Camera", projModel, I.getWidth(), I.getHeight()) !=
261  std::cout << "Cannot find parameters in XML file " << opt_cam_parameters << std::endl;
262  if (drone.getVideoHeight() == 720) { // 720p streaming
263  cam.initPersProjWithoutDistortion(785.6412585, 785.3322447, 637.9049857, 359.7524531);
264  }
265  else { // 480p streaming
266  cam.initPersProjWithoutDistortion(531.9213063, 520.8495788, 429.133986, 240.9464457);
267  }
268  }
269  }
270  else {
271  std::cout << "Setting default camera parameters ... " << std::endl;
272  if (drone.getVideoHeight() == 720) { // 720p streaming
273  cam.initPersProjWithoutDistortion(785.6412585, 785.3322447, 637.9049857, 359.7524531);
274  }
275  else { // 480p streaming
276  cam.initPersProjWithoutDistortion(531.9213063, 520.8495788, 429.133986, 240.9464457);
277  }
278  }
279  cam.printParameters();
280 
281  vpServo task; // Visual servoing task
282 
283  // double lambda = 0.5;
284  vpAdaptiveGain lambda = vpAdaptiveGain(1.5, 0.7, 30);
287  task.setLambda(lambda);
288 
289  /*
290  In the following section, camera 1 refers to camera coordonates system of the drone, but without taking camera
291  pan and camera tilt into account.
292  Those values are taken into consideration in Camera 2.
293  E is the effective coordinate system of the drone, the one in which we need to convert every velocity command.
294 
295  We can easily compute homogeneous matrix between camera 1 and camera 2, and between camera 1
296  and effective coordonate system E of the drone.
297 
298  Using those matrices, we can in the end obtain the matrix between c2 and E
299  */
300  vpRxyzVector c1_rxyz_c2(vpMath::rad(drone.getCurrentCameraTilt()), vpMath::rad(drone.getCurrentCameraPan()), 0);
301  vpRotationMatrix c1Rc2(c1_rxyz_c2); // Rotation between camera 1 and 2
302  vpHomogeneousMatrix c1Mc2(vpTranslationVector(), c1Rc2); // Homogeneous matrix between c1 and c2
303 
304  vpRotationMatrix c1Re { 0, 1, 0, 0, 0, 1, 1, 0, 0 }; // Rotation between camera 1 and E
305  vpTranslationVector c1te(0, 0, -0.09); // Translation between camera 1 and E
306  vpHomogeneousMatrix c1Me(c1te, c1Re); // Homogeneous matrix between c1 and E
307 
308  vpHomogeneousMatrix c2Me = c1Mc2.inverse() * c1Me; // Homogeneous matrix between c2 and E
309 
310  vpVelocityTwistMatrix cVe(c2Me);
311  task.set_cVe(cVe);
312 
313  vpMatrix eJe(6, 4, 0);
314 
315  eJe[0][0] = 1;
316  eJe[1][1] = 1;
317  eJe[2][2] = 1;
318  eJe[5][3] = 1;
319 
320  // double Z_d = 1.; // Desired distance to the target
321  double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
322 
323  // Define the desired polygon corresponding the the AprilTag CLOCKWISE
324  double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. };
325  double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. };
326  std::vector<vpPoint> vec_P, vec_P_d;
327 
328  for (int i = 0; i < 4; i++) {
329  vpPoint P_d(X[i], Y[i], 0);
330  vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
331  P_d.track(cdMo); //
332  vec_P_d.push_back(P_d);
333  }
334  vpMomentObject m_obj(3), m_obj_d(3);
335  vpMomentDatabase mdb, mdb_d;
336  vpMomentBasic mb_d; // Here only to get the desired area m00
337  vpMomentGravityCenter mg, mg_d;
338  vpMomentCentered mc, mc_d;
339  vpMomentAreaNormalized man(0, Z_d), man_d(0, Z_d); // Declare normalized area updated below with m00
340  vpMomentGravityCenterNormalized mgn, mgn_d; // Declare normalized gravity center
341 
342  // Desired moments
343  m_obj_d.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
344  m_obj_d.fromVector(vec_P_d); // Initialize the object with the points coordinates
345 
346  mb_d.linkTo(mdb_d); // Add basic moments to database
347  mg_d.linkTo(mdb_d); // Add gravity center to database
348  mc_d.linkTo(mdb_d); // Add centered moments to database
349  man_d.linkTo(mdb_d); // Add area normalized to database
350  mgn_d.linkTo(mdb_d); // Add gravity center normalized to database
351  mdb_d.updateAll(m_obj_d); // All of the moments must be updated, not just an_d
352  mg_d.compute(); // Compute gravity center moment
353  mc_d.compute(); // Compute centered moments AFTER gravity center
354 
355  double area = 0;
356  if (m_obj_d.getType() == vpMomentObject::DISCRETE)
357  area = mb_d.get(2, 0) + mb_d.get(0, 2);
358  else
359  area = mb_d.get(0, 0);
360  // Update moment with the desired area
361  man_d.setDesiredArea(area);
362 
363  man_d.compute(); // Compute area normalized moment AFTER centered moments
364  mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment
365 
366  // Desired plane
367  double A = 0.0;
368  double B = 0.0;
369  double C = 1.0 / Z_d;
370 
371  // Construct area normalized features
372  vpFeatureMomentGravityCenterNormalized s_mgn(mdb, A, B, C), s_mgn_d(mdb_d, A, B, C);
373  vpFeatureMomentAreaNormalized s_man(mdb, A, B, C), s_man_d(mdb_d, A, B, C);
374  vpFeatureVanishingPoint s_vp, s_vp_d;
375 
376  // Add the features
377  task.addFeature(s_mgn, s_mgn_d);
378  task.addFeature(s_man, s_man_d);
380 
381  plotter.initGraph(0, 4);
382  plotter.setLegend(0, 0, "Xn"); // Distance from center on X axis feature
383  plotter.setLegend(0, 1, "Yn"); // Distance from center on Y axis feature
384  plotter.setLegend(0, 2, "an"); // Tag area feature
385  plotter.setLegend(0, 3, "atan(1/rho)"); // Vanishing point feature
386 
387  // Update desired gravity center normalized feature
388  s_mgn_d.update(A, B, C);
389  s_mgn_d.compute_interaction();
390  // Update desired area normalized feature
391  s_man_d.update(A, B, C);
392  s_man_d.compute_interaction();
393 
394  // Update desired vanishing point feature for the horizontal line
395  s_vp_d.setAtanOneOverRho(0);
396  s_vp_d.setAlpha(0);
397 
398  bool runLoop = true;
399  bool vec_ip_has_been_sorted = false;
400  std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
401 
402  //** Visual servoing loop **//
403  while (drone.isRunning() && drone.isStreaming() && runLoop) {
404 
405  double startTime = vpTime::measureTimeMs();
406 
407  drone.getGrayscaleImage(I);
409 
410  std::vector<vpHomogeneousMatrix> cMo_vec;
411  detector.detect(I, tagSize, cam, cMo_vec); // Detect AprilTags in current image
412  double t = vpTime::measureTimeMs() - startTime;
413 
414  {
415  std::stringstream ss;
416  ss << "Detection time: " << t << " ms";
417  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
418  }
419 
420  if (detector.getNbObjects() == 1) {
421 
422  // Update current points used to compute the moments
423  std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
424  vec_P.clear();
425  for (size_t i = 0; i < vec_ip.size(); i++) { // size = 4
426  double x = 0, y = 0;
427  vpPixelMeterConversion::convertPoint(cam, vec_ip[i], x, y);
428  vpPoint P;
429  P.set_x(x);
430  P.set_y(y);
431  vec_P.push_back(P);
432  }
433 
434  // Current moments
435  m_obj.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
436  m_obj.fromVector(vec_P); // Initialize the object with the points coordinates
437 
438  mg.linkTo(mdb); // Add gravity center to database
439  mc.linkTo(mdb); // Add centered moments to database
440  man.linkTo(mdb); // Add area normalized to database
441  mgn.linkTo(mdb); // Add gravity center normalized to database
442  mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d
443  mg.compute(); // Compute gravity center moment
444  mc.compute(); // Compute centered moments AFTER gravity center
445  man.setDesiredArea(area); // Desired area was init at 0 (unknow at construction), need to be updated here
446  man.compute(); // Compute area normalized moment AFTER centered moment
447  mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment
448 
449  s_mgn.update(A, B, C);
450  s_mgn.compute_interaction();
451  s_man.update(A, B, C);
452  s_man.compute_interaction();
453 
454  /* Sort points from their height in the image, and keep original indexes.
455  This is done once, in order to be independent from the orientation of the tag
456  when detecting vanishing points. */
457  if (!vec_ip_has_been_sorted) {
458  for (size_t i = 0; i < vec_ip.size(); i++) {
459 
460  // Add the points and their corresponding index
461  std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
462  vec_ip_sorted.push_back(index_pair);
463  }
464 
465  // Sort the points and indexes from the v value of the points
466  std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
467 
468  vec_ip_has_been_sorted = true;
469  }
470 
471  // Use the two highest points for the first line, and the two others for the second line.
472  vpFeatureBuilder::create(s_vp, cam, vec_ip[vec_ip_sorted[0].first], vec_ip[vec_ip_sorted[1].first],
473  vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
475 
476  task.set_cVe(cVe);
477  task.set_eJe(eJe);
478 
479  // Compute the control law. Velocities are computed in the mobile robot reference frame
480  vpColVector ve = task.computeControlLaw();
481 
482  // Sending the control law to the drone
483  if (verbose) {
484  std::cout << "ve: " << ve.t() << std::endl;
485  }
486  drone.setVelocity(ve, 1.0);
487 
488  for (size_t i = 0; i < 4; i++) {
489  vpDisplay::displayCross(I, vec_ip[i], 15, vpColor::red, 1);
490  std::stringstream ss;
491  ss << i;
492  vpDisplay::displayText(I, vec_ip[i] + vpImagePoint(15, 15), ss.str(), vpColor::green);
493  }
494 
495  // Display visual features
496  vpDisplay::displayPolygon(I, vec_ip, vpColor::green, 3); // Current polygon used to compure an moment
497  vpDisplay::displayCross(I, detector.getCog(0), 15, vpColor::green,
498  3); // Current polygon used to compute a moment
499  vpDisplay::displayLine(I, 0, static_cast<int>(cam.get_u0()), static_cast<int>(I.getHeight()) - 1,
500  static_cast<int>(cam.get_u0()), vpColor::red,
501  3); // Vertical line as desired x position
502  vpDisplay::displayLine(I, static_cast<int>(cam.get_v0()), 0, static_cast<int>(cam.get_v0()),
503  static_cast<int>(I.getWidth()) - 1, vpColor::red,
504  3); // Horizontal line as desired y position
505 
506  // Display lines corresponding to the vanishing point for the horizontal lines
507  vpDisplay::displayLine(I, vec_ip[vec_ip_sorted[0].first], vec_ip[vec_ip_sorted[1].first], vpColor::red, 1,
508  false);
509  vpDisplay::displayLine(I, vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first], vpColor::red, 1,
510  false);
511 
512  }
513  else {
514  std::stringstream sserr;
515  sserr << "Failed to detect an Apriltag, or detected multiple ones";
516  vpDisplay::displayText(I, 120, 20, sserr.str(), vpColor::red);
517  vpDisplay::flush(I);
518  drone.stopMoving(); // In this case, we stop the drone
519  }
520 
521  vpDisplay::displayText(I, 10, 10, "Click to exit", vpColor::red);
522  vpDisplay::flush(I);
523  if (vpDisplay::getClick(I, false)) {
524  drone.land();
525  runLoop = false;
526  }
527 
528  plotter.plot(0, iter, task.getError());
529 
530  double totalTime = vpTime::measureTimeMs() - startTime;
531  std::stringstream sstime;
532  sstime << "Total time: " << totalTime << " ms";
533  vpDisplay::displayText(I, 80, 20, sstime.str(), vpColor::red);
534  vpDisplay::flush(I);
535 
536  iter++;
537  vpTime::wait(startTime, 40.0); // We wait a total of 40 milliseconds
538  }
539 
540  return EXIT_SUCCESS;
541 
542  }
543  else {
544  std::cout << "ERROR : failed to setup drone control." << std::endl;
545  return EXIT_FAILURE;
546  }
547  }
548  catch (const vpException &e) {
549  std::cout << "Caught an exception: " << e << std::endl;
550  return EXIT_FAILURE;
551  }
552 }
553 
554 #endif // #elif !defined(VISP_HAVE_FFMPEG)
Adaptive gain computation.
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
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:211
static const vpColor green
Definition: vpColor.h:214
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
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 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 emitted by ViSP classes.
Definition: vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
static unsigned int selectAtanOneOverRho()
void setAlpha(double alpha)
Set vanishing point feature value.
void setAtanOneOverRho(double atan_one_over_rho)
Set vanishing point feature value.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
static double rad(double deg)
Definition: vpMath.h:127
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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:70
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:95
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:109
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:500
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:502
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
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 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
vpColVector getError() const
Definition: vpServo.h:504
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
@ CURRENT
Definition: vpServo.h:196
Class that consider the case of a translation vector.
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)
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()