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>
64 #if !defined(VISP_HAVE_ARSDK)
67 std::cout <<
"\nThis example requires Parrot ARSDK3 library. You should install it.\n" << std::endl;
70 #elif !defined(VISP_HAVE_FFMPEG)
73 std::cout <<
"\nThis example requires ffmpeg library. You should install it.\n" << std::endl;
76 #elif !defined(VISP_HAVE_PUGIXML)
79 std::cout <<
"\nThis example requires pugixml built-in 3rdparty library. You should enable it.\n" << std::endl;
85 bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
87 return (p1.second.get_v() < p2.second.get_v());
101 int main(
int argc,
char **argv)
105 std::string ip_address =
"192.168.42.1";
106 std::string opt_cam_parameters;
107 bool opt_has_cam_parameters =
false;
111 double opt_distance_to_tag = -1;
112 bool opt_has_distance_to_tag =
false;
116 bool verbose =
false;
118 if (argc >= 3 && std::string(argv[1]) ==
"--tag_size") {
119 tagSize = std::atof(argv[2]);
121 std::cout <<
"Error : invalid tag size." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
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]);
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;
135 opt_has_distance_to_tag =
true;
138 else if (std::string(argv[i]) ==
"--intrinsic") {
140 opt_cam_parameters = std::string(argv[i + 1]);
141 opt_has_cam_parameters =
true;
144 else if (std::string(argv[i]) ==
"--hd_stream") {
147 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
151 std::cout <<
"Error : unknown parameter " << argv[i] << std::endl
152 <<
"See " << argv[0] <<
" --help" << std::endl;
157 else if (argc >= 2 && (std::string(argv[1]) ==
"--help" || std::string(argv[1]) ==
"-h")) {
158 std::cout <<
"\nUsage:\n"
160 <<
" [--tag_size <size>] [--ip <drone ip>] [--distance_to_tag <distance>] [--intrinsic <xml file>] "
161 <<
"[--hd_stream] [--verbose] [-v] [--help] [-h]\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"
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"
183 <<
" Print help message.\n"
188 std::cout <<
"Error : tag size parameter required." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
193 <<
"\nWARNING: \n - This program does no sensing or avoiding of "
195 "the drone WILL collide with any objects in the way! Make sure "
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"
204 verbose,
true, ip_address);
206 if (drone.isRunning()) {
208 drone.setVideoResolution(stream_res);
210 drone.setStreamingMode(0);
211 drone.setVideoStabilisationMode(0);
215 drone.startStreaming();
217 drone.setExposure(1.5f);
219 drone.setCameraOrientation(-5., 0.,
225 drone.getGrayscaleImage(I);
227 #if defined(VISP_HAVE_X11)
229 #elif defined(VISP_HAVE_GTK)
231 #elif defined(VISP_HAVE_GDI)
233 #elif defined(HAVE_OPENCV_HIGHGUI)
236 int orig_displayX = 100;
237 int orig_displayY = 100;
238 display.init(I, orig_displayX, orig_displayY,
"DRONE VIEW");
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;
248 detector.setAprilTagQuadDecimate(4.0);
249 detector.setAprilTagNbThreads(4);
250 detector.setDisplayTag(
true);
254 if (opt_has_cam_parameters) {
261 std::cout <<
"Cannot find parameters in XML file " << opt_cam_parameters << std::endl;
262 if (drone.getVideoHeight() == 720) {
271 std::cout <<
"Setting default camera parameters ... " << std::endl;
272 if (drone.getVideoHeight() == 720) {
321 double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
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;
328 for (
int i = 0; i < 4; i++) {
332 vec_P_d.push_back(P_d);
344 m_obj_d.fromVector(vec_P_d);
357 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
359 area = mb_d.
get(0, 0);
361 man_d.setDesiredArea(area);
369 double C = 1.0 / Z_d;
381 plotter.initGraph(0, 4);
382 plotter.setLegend(0, 0,
"Xn");
383 plotter.setLegend(0, 1,
"Yn");
384 plotter.setLegend(0, 2,
"an");
385 plotter.setLegend(0, 3,
"atan(1/rho)");
388 s_mgn_d.update(A, B, C);
389 s_mgn_d.compute_interaction();
391 s_man_d.update(A, B, C);
392 s_man_d.compute_interaction();
399 bool vec_ip_has_been_sorted =
false;
400 std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
403 while (drone.isRunning() && drone.isStreaming() && runLoop) {
407 drone.getGrayscaleImage(I);
410 std::vector<vpHomogeneousMatrix> cMo_vec;
411 detector.detect(I, tagSize, cam, cMo_vec);
415 std::stringstream ss;
416 ss <<
"Detection time: " << t <<
" ms";
420 if (detector.getNbObjects() == 1) {
423 std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
425 for (
size_t i = 0; i < vec_ip.size(); i++) {
436 m_obj.fromVector(vec_P);
445 man.setDesiredArea(area);
449 s_mgn.update(A, B, C);
450 s_mgn.compute_interaction();
451 s_man.update(A, B, C);
452 s_man.compute_interaction();
457 if (!vec_ip_has_been_sorted) {
458 for (
size_t i = 0; i < vec_ip.size(); i++) {
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);
466 std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
468 vec_ip_has_been_sorted =
true;
473 vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
484 std::cout <<
"ve: " << ve.
t() << std::endl;
486 drone.setVelocity(ve, 1.0);
488 for (
size_t i = 0; i < 4; i++) {
490 std::stringstream ss;
514 std::stringstream sserr;
515 sserr <<
"Failed to detect an Apriltag, or detected multiple ones";
528 plotter.plot(0, iter, task.
getError());
531 std::stringstream sstime;
532 sstime <<
"Total time: " << totalTime <<
" ms";
544 std::cout <<
"ERROR : failed to setup drone control." << std::endl;
549 std::cout <<
"Caught an exception: " << e << std::endl;
Adaptive gain computation.
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
vpCameraParametersProjType
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor green
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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...
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.
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 ...
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
Implementation of a matrix and operations on matrices.
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...
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)
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...
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_x(double x)
Set the point x coordinate in the image plane.
void set_y(double y)
Set the point y coordinate in the image plane.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
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()