38 #include <visp3/core/vpConfig.h>
41 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \
42 defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PUGIXML)
44 #include <visp3/core/vpImageConvert.h>
45 #include <visp3/core/vpMomentAreaNormalized.h>
46 #include <visp3/core/vpMomentBasic.h>
47 #include <visp3/core/vpMomentCentered.h>
48 #include <visp3/core/vpMomentDatabase.h>
49 #include <visp3/core/vpMomentGravityCenter.h>
50 #include <visp3/core/vpMomentGravityCenterNormalized.h>
51 #include <visp3/core/vpMomentObject.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 #include <visp3/core/vpPoint.h>
54 #include <visp3/core/vpTime.h>
55 #include <visp3/core/vpXmlParserCamera.h>
56 #include <visp3/detection/vpDetectorAprilTag.h>
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/gui/vpPlot.h>
60 #include <visp3/robot/vpRobotMavsdk.h>
61 #include <visp3/sensor/vpRealSense2.h>
62 #include <visp3/visual_features/vpFeatureBuilder.h>
63 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
64 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
65 #include <visp3/visual_features/vpFeatureVanishingPoint.h>
66 #include <visp3/vs/vpServo.h>
67 #include <visp3/vs/vpServoDisplay.h>
72 bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
74 return (p1.second.get_v() < p2.second.get_v());
93 int main(
int argc,
char **argv)
96 std::string opt_connecting_info =
"udp://:192.168.30.111:14552";
98 double opt_distance_to_tag = -1;
99 bool opt_has_distance_to_tag =
false;
100 int opt_display_fps = 10;
101 bool opt_verbose =
false;
105 if (argc >= 3 && std::string(argv[1]) ==
"--tag-size") {
106 tagSize = std::atof(argv[2]);
108 std::cout <<
"Error : invalid tag size." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
111 for (
int i = 3; i < argc; i++) {
112 if (std::string(argv[i]) ==
"--co" && i + 1 < argc) {
113 opt_connecting_info = std::string(argv[i + 1]);
116 else if (std::string(argv[i]) ==
"--distance-to-tag" && i + 1 < argc) {
117 opt_distance_to_tag = std::atof(argv[i + 1]);
118 if (opt_distance_to_tag <= 0) {
119 std::cout <<
"Error : invalid distance to tag." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
122 opt_has_distance_to_tag =
true;
126 else if (std::string(argv[i]) ==
"--display-fps" && i + 1 < argc) {
127 opt_display_fps = std::stoi(std::string(argv[i + 1]));
130 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
134 std::cout <<
"Error : unknown parameter " << argv[i] << std::endl
135 <<
"See " << argv[0] <<
" --help" << std::endl;
140 else if (argc >= 2 && (std::string(argv[1]) ==
"--help" || std::string(argv[1]) ==
"-h")) {
141 std::cout <<
"\nUsage:\n"
143 <<
" [--tag-size <tag size [m]>] [--co <connection information>] [--distance-to-tag <distance>]"
144 <<
" [--display-fps <display fps>] [--verbose] [-v] [--help] [-h]\n"
147 <<
" --tag-size <size>\n"
148 <<
" The size of the tag to detect in meters, required.\n\n"
149 <<
" --co <connection information>\n"
150 <<
" - UDP: udp://[host][:port]\n"
151 <<
" - TCP: tcp://[host][:port]\n"
152 <<
" - serial: serial://[path][:baudrate]\n"
153 <<
" - Default: udp://192.168.30.111:14552).\n\n"
154 <<
" --distance-to-tag <distance>\n"
155 <<
" The desired distance to the tag in meters (default: 1 meter).\n\n"
156 <<
" --display-fps <display_fps>\n"
157 <<
" The desired fps rate for the video display (default: 10 fps).\n\n"
158 <<
" --verbose, -v\n"
159 <<
" Enables verbosity (drone information messages and velocity commands\n"
160 <<
" are then displayed).\n\n"
162 <<
" Print help message.\n"
168 std::cout <<
"Error : tag size parameter required." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
172 std::cout << std::endl
173 <<
"WARNING:" << std::endl
174 <<
" - This program does no sensing or avoiding of obstacles, " << std::endl
175 <<
" the drone WILL collide with any objects in the way! Make sure the " << std::endl
176 <<
" drone has approximately 3 meters of free space on all sides." << std::endl
177 <<
" - The drone uses a forward-facing camera for Apriltag detection," << std::endl
178 <<
" make sure the drone flies above a non-uniform flooring," << std::endl
179 <<
" or its movement will be inacurate and dangerous !" << std::endl
183 vpRobotMavsdk drone(opt_connecting_info);
185 if (drone.isRunning()) {
190 std::cout <<
"Product line: " << product_line << std::endl;
193 if (product_line ==
"T200") {
194 std::cout <<
"This example doesn't support T200 product line family !" << std::endl;
199 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, acq_fps);
200 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, acq_fps);
201 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, acq_fps);
217 #if defined(VISP_HAVE_X11)
219 #elif defined(VISP_HAVE_GTK)
221 #elif defined(VISP_HAVE_GDI)
223 #elif defined(HAVE_OPENCV_HIGHGUI)
226 int orig_displayX = 100;
227 int orig_displayY = 100;
228 display.init(I, orig_displayX, orig_displayY,
"DRONE VIEW");
233 vpPlot plotter(1, 700, 700, orig_displayX +
static_cast<int>(I.
getWidth()) + 20, orig_displayY,
234 "Visual servoing tasks");
235 unsigned int iter = 0;
241 detector.setAprilTagQuadDecimate(4.0);
242 detector.setAprilTagNbThreads(4);
243 detector.setDisplayTag(
true);
289 double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
292 double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. };
293 double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. };
294 std::vector<vpPoint> vec_P, vec_P_d;
296 for (
int i = 0; i < 4; i++) {
300 vec_P_d.push_back(P_d);
312 m_obj_d.fromVector(vec_P_d);
325 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
327 area = mb_d.
get(0, 0);
329 man_d.setDesiredArea(area);
338 double C = 1.0 / Z_d;
350 plotter.initGraph(0, 4);
351 plotter.setLegend(0, 0,
"Xn");
352 plotter.setLegend(0, 1,
"Yn");
353 plotter.setLegend(0, 2,
"an");
354 plotter.setLegend(0, 3,
"atan(1/rho)");
357 s_mgn_d.update(A, B, C);
358 s_mgn_d.compute_interaction();
360 s_man_d.update(A, B, C);
361 s_man_d.compute_interaction();
369 bool vec_ip_has_been_sorted =
false;
370 bool send_velocities =
false;
371 std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
374 while (drone.isRunning() && runLoop) {
381 condition = (startTime - time_since_last_display) > 1000. / opt_display_fps ?
true :
false;
387 std::vector<vpHomogeneousMatrix> cMo_vec;
388 detector.detect(I, tagSize, cam, cMo_vec);
392 std::stringstream ss;
393 ss <<
"Detection time: " << t <<
" ms";
397 if (detector.getNbObjects() != 0) {
400 std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
402 for (
size_t i = 0; i < vec_ip.size(); i++) {
413 m_obj.fromVector(vec_P);
422 man.setDesiredArea(area);
428 s_mgn.update(A, B, C);
429 s_mgn.compute_interaction();
430 s_man.update(A, B, C);
431 s_man.compute_interaction();
436 if (!vec_ip_has_been_sorted) {
437 for (
size_t i = 0; i < vec_ip.size(); i++) {
440 std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
441 vec_ip_sorted.push_back(index_pair);
445 std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
447 vec_ip_has_been_sorted =
true;
453 vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
462 if (!send_velocities) {
468 std::cout <<
"ve: " << ve.
t() << std::endl;
472 drone.setVelocity(ve);
476 for (
size_t i = 0; i < 4; i++) {
478 std::stringstream ss;
505 std::stringstream sserr;
506 sserr <<
"Failed to detect an Apriltag, or detected multiple ones";
512 std::cout << sserr.str() << std::endl;
521 std::stringstream ss;
522 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot")
523 <<
", right click to quit.";
528 plotter.plot(0, iter, task.
getError());
535 send_velocities = !send_velocities;
549 std::stringstream sstime;
550 sstime <<
"Total time: " << totalTime <<
" ms";
563 std::cout <<
"ERROR : failed to setup drone control." << std::endl;
568 std::cout <<
"Caught an exception: " << e << std::endl;
577 #ifndef VISP_HAVE_MAVSDK
578 std::cout <<
"\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" << std::endl;
580 #ifndef VISP_HAVE_REALSENSE2
581 std::cout <<
"\nThis example requires librealsense2 library. You should install it, configure and rebuid ViSP.\n" << std::endl;
583 #if !defined(VISP_HAVE_PUGIXML)
584 std::cout <<
"\nThis example requires pugixml built-in 3rdparty." << std::endl;
586 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
588 <<
"\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
Adaptive gain computation.
Generic class defining intrinsic camera parameters.
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.
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
std::string getProductLine()
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.
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()