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 (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
79 std::cout <<
"\nThis example requires cxx11 standard or higher. Turn it on using cmake -DUSE_CXX_STANDARD=11.\n"
85bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
87 return (p1.second.get_v() < p2.second.get_v());
101int 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]);
128 }
else if (std::string(argv[i]) ==
"--distance_to_tag" && i + 1 < argc) {
129 opt_distance_to_tag = std::atof(argv[i + 1]);
130 if (opt_distance_to_tag <= 0) {
131 std::cout <<
"Error : invalid distance to tag." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
134 opt_has_distance_to_tag =
true;
136 }
else if (std::string(argv[i]) ==
"--intrinsic") {
138 opt_cam_parameters = std::string(argv[i + 1]);
139 opt_has_cam_parameters =
true;
141 }
else if (std::string(argv[i]) ==
"--hd_stream") {
143 }
else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
146 std::cout <<
"Error : unknown parameter " << argv[i] << std::endl
147 <<
"See " << argv[0] <<
" --help" << std::endl;
151 }
else if (argc >= 2 && (std::string(argv[1]) ==
"--help" || std::string(argv[1]) ==
"-h")) {
152 std::cout <<
"\nUsage:\n"
154 <<
" [--tag_size <size>] [--ip <drone ip>] [--distance_to_tag <distance>] [--intrinsic <xml file>] "
155 <<
"[--hd_stream] [--verbose] [-v] [--help] [-h]\n"
158 <<
" --tag_size <size>\n"
159 <<
" The size of the tag to detect in meters, required.\n\n"
160 <<
" --ip <drone ip>\n"
161 <<
" Ip address of the drone to which you want to connect (default : 192.168.42.1).\n\n"
162 <<
" --distance_to_tag <distance>\n"
163 <<
" The desired distance to the tag in meters (default: 1 meter).\n\n"
164 <<
" --intrinsic <xml file>\n"
165 <<
" XML file containing computed intrinsic camera parameters (default: empty).\n\n"
167 <<
" Enables HD 720p streaming instead of default 480p.\n"
168 <<
" Allows to increase range and accuracy of the tag detection,\n"
169 <<
" but increases latency and computation time.\n"
170 <<
" Caution: camera calibration settings are different for the two resolutions.\n"
171 <<
" Make sure that if you pass custom intrinsic camera parameters,\n"
172 <<
" they were obtained with the correct resolution.\n\n"
173 <<
" --verbose, -v\n"
174 <<
" Enables verbose (drone information messages and velocity commands\n"
175 <<
" are then displayed).\n\n"
177 <<
" Print help message.\n"
181 std::cout <<
"Error : tag size parameter required." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
186 <<
"\nWARNING: \n - This program does no sensing or avoiding of "
188 "the drone WILL collide with any objects in the way! Make sure "
190 "drone has approximately 3 meters of free space on all sides.\n"
191 " - The drone uses a downward-facing camera for horizontal speed estimation,\n make sure the drone flies "
192 "above a non-uniform flooring,\n or its movement will be inacurate and dangerous !\n"
197 verbose,
true, ip_address);
199 if (drone.isRunning()) {
201 drone.setVideoResolution(stream_res);
203 drone.setStreamingMode(0);
204 drone.setVideoStabilisationMode(0);
208 drone.startStreaming();
210 drone.setExposure(1.5f);
212 drone.setCameraOrientation(-5., 0.,
218 drone.getGrayscaleImage(I);
220#if defined(VISP_HAVE_X11)
222#elif defined(VISP_HAVE_GTK)
224#elif defined(VISP_HAVE_GDI)
226#elif defined(HAVE_OPENCV_HIGHGUI)
229 int orig_displayX = 100;
230 int orig_displayY = 100;
231 display.init(I, orig_displayX, orig_displayY,
"DRONE VIEW");
235 vpPlot plotter(1, 700, 700, orig_displayX +
static_cast<int>(I.
getWidth()) + 20, orig_displayY,
236 "Visual servoing tasks");
237 unsigned int iter = 0;
241 detector.setAprilTagQuadDecimate(4.0);
242 detector.setAprilTagNbThreads(4);
243 detector.setDisplayTag(
true);
247 if (opt_has_cam_parameters) {
254 std::cout <<
"Cannot find parameters in XML file " << opt_cam_parameters << std::endl;
255 if (drone.getVideoHeight() == 720) {
262 std::cout <<
"Setting default camera parameters ... " << std::endl;
263 if (drone.getVideoHeight() == 720) {
311 double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
314 double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.};
315 double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.};
316 std::vector<vpPoint> vec_P, vec_P_d;
318 for (
int i = 0; i < 4; i++) {
322 vec_P_d.push_back(P_d);
334 m_obj_d.fromVector(vec_P_d);
347 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
349 area = mb_d.
get(0, 0);
351 man_d.setDesiredArea(area);
359 double C = 1.0 / Z_d;
371 plotter.initGraph(0, 4);
372 plotter.setLegend(0, 0,
"Xn");
373 plotter.setLegend(0, 1,
"Yn");
374 plotter.setLegend(0, 2,
"an");
375 plotter.setLegend(0, 3,
"atan(1/rho)");
378 s_mgn_d.update(A, B, C);
379 s_mgn_d.compute_interaction();
381 s_man_d.update(A, B, C);
382 s_man_d.compute_interaction();
389 bool vec_ip_has_been_sorted =
false;
390 std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
393 while (drone.isRunning() && drone.isStreaming() && runLoop) {
397 drone.getGrayscaleImage(I);
400 std::vector<vpHomogeneousMatrix> cMo_vec;
401 detector.detect(I, tagSize, cam, cMo_vec);
405 std::stringstream ss;
406 ss <<
"Detection time: " << t <<
" ms";
410 if (detector.getNbObjects() == 1) {
413 std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
415 for (
size_t i = 0; i < vec_ip.size(); i++) {
426 m_obj.fromVector(vec_P);
435 man.setDesiredArea(area);
439 s_mgn.update(A, B, C);
440 s_mgn.compute_interaction();
441 s_man.update(A, B, C);
442 s_man.compute_interaction();
447 if (!vec_ip_has_been_sorted) {
448 for (
size_t i = 0; i < vec_ip.size(); i++) {
451 std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
452 vec_ip_sorted.push_back(index_pair);
456 std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
458 vec_ip_has_been_sorted =
true;
463 vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
474 std::cout <<
"ve: " << ve.
t() << std::endl;
476 drone.setVelocity(ve, 1.0);
478 for (
size_t i = 0; i < 4; i++) {
480 std::stringstream ss;
503 std::stringstream sserr;
504 sserr <<
"Failed to detect an Apriltag, or detected multiple ones";
517 plotter.plot(0, iter, task.
getError());
520 std::stringstream sstime;
521 sstime <<
"Total time: " << totalTime <<
" ms";
532 std::cout <<
"ERROR : failed to setup drone control." << std::endl;
536 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 ...
Definition of the vpImage class member functions.
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 set_cVe(const vpVelocityTwistMatrix &cVe_)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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)
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)