38#include <visp3/core/vpConfig.h>
40#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && defined(VISP_HAVE_REALSENSE2)
42#include <visp3/core/vpImageConvert.h>
43#include <visp3/core/vpMomentAreaNormalized.h>
44#include <visp3/core/vpMomentBasic.h>
45#include <visp3/core/vpMomentCentered.h>
46#include <visp3/core/vpMomentDatabase.h>
47#include <visp3/core/vpMomentGravityCenter.h>
48#include <visp3/core/vpMomentGravityCenterNormalized.h>
49#include <visp3/core/vpMomentObject.h>
50#include <visp3/core/vpPixelMeterConversion.h>
51#include <visp3/core/vpPoint.h>
52#include <visp3/core/vpTime.h>
53#include <visp3/core/vpXmlParserCamera.h>
54#include <visp3/detection/vpDetectorAprilTag.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpPlot.h>
58#include <visp3/robot/vpRobotMavsdk.h>
59#include <visp3/sensor/vpRealSense2.h>
60#include <visp3/visual_features/vpFeatureBuilder.h>
61#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
62#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
63#include <visp3/visual_features/vpFeatureVanishingPoint.h>
64#include <visp3/vs/vpServo.h>
65#include <visp3/vs/vpServoDisplay.h>
70bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
72 return (p1.second.get_v() < p2.second.get_v());
91int main(
int argc,
char **argv)
94 std::string opt_connecting_info =
"udp://:192.168.30.111:14552";
96 double opt_distance_to_tag = -1;
97 bool opt_has_distance_to_tag =
false;
98 int opt_display_fps = 10;
99 bool opt_verbose =
false;
103 if (argc >= 3 && std::string(argv[1]) ==
"--tag-size") {
104 tagSize = std::atof(argv[2]);
106 std::cout <<
"Error : invalid tag size." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
109 for (
int i = 3; i < argc; i++) {
110 if (std::string(argv[i]) ==
"--co" && i + 1 < argc) {
111 opt_connecting_info = std::string(argv[i + 1]);
113 }
else if (std::string(argv[i]) ==
"--distance-to-tag" && i + 1 < argc) {
114 opt_distance_to_tag = std::atof(argv[i + 1]);
115 if (opt_distance_to_tag <= 0) {
116 std::cout <<
"Error : invalid distance to tag." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
119 opt_has_distance_to_tag =
true;
122 }
else if (std::string(argv[i]) ==
"--display-fps" && i + 1 < argc) {
123 opt_display_fps = std::stoi(std::string(argv[i + 1]));
125 }
else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
128 std::cout <<
"Error : unknown parameter " << argv[i] << std::endl
129 <<
"See " << argv[0] <<
" --help" << std::endl;
133 }
else if (argc >= 2 && (std::string(argv[1]) ==
"--help" || std::string(argv[1]) ==
"-h")) {
134 std::cout <<
"\nUsage:\n"
136 <<
" [--tag-size <tag size [m]>] [--co <connection information>] [--distance-to-tag <distance>]"
137 <<
" [--display-fps <display fps>] [--verbose] [-v] [--help] [-h]\n"
140 <<
" --tag-size <size>\n"
141 <<
" The size of the tag to detect in meters, required.\n\n"
142 <<
" --co <connection information>\n"
143 <<
" - UDP: udp://[host][:port]\n"
144 <<
" - TCP: tcp://[host][:port]\n"
145 <<
" - serial: serial://[path][:baudrate]\n"
146 <<
" - Default: udp://192.168.30.111:14552).\n\n"
147 <<
" --distance-to-tag <distance>\n"
148 <<
" The desired distance to the tag in meters (default: 1 meter).\n\n"
149 <<
" --display-fps <display_fps>\n"
150 <<
" The desired fps rate for the video display (default: 10 fps).\n\n"
151 <<
" --verbose, -v\n"
152 <<
" Enables verbosity (drone information messages and velocity commands\n"
153 <<
" are then displayed).\n\n"
155 <<
" Print help message.\n"
160 std::cout <<
"Error : tag size parameter required." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
164 std::cout << std::endl
165 <<
"WARNING:" << std::endl
166 <<
" - This program does no sensing or avoiding of obstacles, " << std::endl
167 <<
" the drone WILL collide with any objects in the way! Make sure the " << std::endl
168 <<
" drone has approximately 3 meters of free space on all sides." << std::endl
169 <<
" - The drone uses a forward-facing camera for Apriltag detection," << std::endl
170 <<
" make sure the drone flies above a non-uniform flooring," << std::endl
171 <<
" or its movement will be inacurate and dangerous !" << std::endl
177 if (drone.isRunning()) {
182 std::cout <<
"Product line: " << product_line << std::endl;
185 if (product_line ==
"T200") {
186 std::cout <<
"This example doesn't support T200 product line family !" << std::endl;
191 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, acq_fps);
192 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, acq_fps);
193 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, acq_fps);
209#if defined(VISP_HAVE_X11)
211#elif defined(VISP_HAVE_GTK)
213#elif defined(VISP_HAVE_GDI)
215#elif defined(HAVE_OPENCV_HIGHGUI)
218 int orig_displayX = 100;
219 int orig_displayY = 100;
220 display.init(I, orig_displayX, orig_displayY,
"DRONE VIEW");
225 vpPlot plotter(1, 700, 700, orig_displayX +
static_cast<int>(I.
getWidth()) + 20, orig_displayY,
226 "Visual servoing tasks");
227 unsigned int iter = 0;
233 detector.setAprilTagQuadDecimate(4.0);
234 detector.setAprilTagNbThreads(4);
235 detector.setDisplayTag(
true);
281 double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
284 double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.};
285 double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.};
286 std::vector<vpPoint> vec_P, vec_P_d;
288 for (
int i = 0; i < 4; i++) {
292 vec_P_d.push_back(P_d);
304 m_obj_d.fromVector(vec_P_d);
317 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
319 area = mb_d.
get(0, 0);
321 man_d.setDesiredArea(area);
330 double C = 1.0 / Z_d;
342 plotter.initGraph(0, 4);
343 plotter.setLegend(0, 0,
"Xn");
344 plotter.setLegend(0, 1,
"Yn");
345 plotter.setLegend(0, 2,
"an");
346 plotter.setLegend(0, 3,
"atan(1/rho)");
349 s_mgn_d.update(A, B, C);
350 s_mgn_d.compute_interaction();
352 s_man_d.update(A, B, C);
353 s_man_d.compute_interaction();
361 bool vec_ip_has_been_sorted =
false;
362 bool send_velocities =
false;
363 std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
366 while (drone.isRunning() && runLoop) {
373 condition = (startTime - time_since_last_display) > 1000. / opt_display_fps ?
true : false;
379 std::vector<vpHomogeneousMatrix> cMo_vec;
380 detector.detect(I, tagSize, cam, cMo_vec);
384 std::stringstream ss;
385 ss <<
"Detection time: " << t <<
" ms";
389 if (detector.getNbObjects() != 0) {
392 std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
394 for (
size_t i = 0; i < vec_ip.size(); i++) {
405 m_obj.fromVector(vec_P);
414 man.setDesiredArea(area);
420 s_mgn.update(A, B, C);
421 s_mgn.compute_interaction();
422 s_man.update(A, B, C);
423 s_man.compute_interaction();
428 if (!vec_ip_has_been_sorted) {
429 for (
size_t i = 0; i < vec_ip.size(); i++) {
432 std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
433 vec_ip_sorted.push_back(index_pair);
437 std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
439 vec_ip_has_been_sorted =
true;
445 vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
454 if (!send_velocities) {
460 std::cout <<
"ve: " << ve.
t() << std::endl;
464 drone.setVelocity(ve);
468 for (
size_t i = 0; i < 4; i++) {
470 std::stringstream ss;
496 std::stringstream sserr;
497 sserr <<
"Failed to detect an Apriltag, or detected multiple ones";
502 std::cout << sserr.str() << std::endl;
511 std::stringstream ss;
512 ss <<
"Left click to " << (send_velocities ?
"stop the robot" :
"servo the robot")
513 <<
", right click to quit.";
518 plotter.plot(0, iter, task.
getError());
525 send_velocities = !send_velocities;
539 std::stringstream sstime;
540 sstime <<
"Total time: " << totalTime <<
" ms";
552 std::cout <<
"ERROR : failed to setup drone control." << std::endl;
556 std::cout <<
"Caught an exception: " << e << std::endl;
565#ifndef VISP_HAVE_MAVSDK
566 std::cout <<
"\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
569#ifndef VISP_HAVE_REALSENSE2
570 std::cout <<
"\nThis example requires librealsense2 library. You should install it, configure and rebuid ViSP.\n"
573#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
575 <<
"\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 ...
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.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
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 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.
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)