42 #include <visp3/core/vpConfig.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/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 #ifdef VISP_HAVE_PUGIXML
65 #include <visp3/core/vpXmlParserCamera.h>
68 #if !defined(VISP_HAVE_ARSDK)
71 std::cout <<
"\nThis example requires Parrot ARSDK3 library. You should install it.\n" << std::endl;
74 #elif !defined(VISP_HAVE_FFMPEG)
77 std::cout <<
"\nThis example requires ffmpeg library. You should install it.\n" << std::endl;
82 bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
84 return (p1.second.get_v() < p2.second.get_v());
98 int main(
int argc,
char **argv)
102 std::string ip_address =
"192.168.42.1";
103 std::string opt_cam_parameters;
104 bool opt_has_cam_parameters =
false;
108 double opt_distance_to_tag = -1;
109 bool opt_has_distance_to_tag =
false;
113 bool verbose =
false;
115 if (argc >= 3 && std::string(argv[1]) ==
"--tag_size") {
116 tagSize = std::atof(argv[2]);
118 std::cout <<
"Error : invalid tag size." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
121 for (
int i = 3; i < argc; i++) {
122 if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
123 ip_address = std::string(argv[i + 1]);
125 }
else if (std::string(argv[i]) ==
"--distance_to_tag" && i + 1 < argc) {
126 opt_distance_to_tag = std::atof(argv[i + 1]);
127 if (opt_distance_to_tag <= 0) {
128 std::cout <<
"Error : invalid distance to tag." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
131 opt_has_distance_to_tag =
true;
133 }
else if (std::string(argv[i]) ==
"--intrinsic") {
135 #ifdef VISP_HAVE_PUGIXML
136 opt_cam_parameters = std::string(argv[i + 1]);
137 opt_has_cam_parameters =
true;
140 std::cout <<
"PUGIXML is required for custom camera parameters input." << std::endl;
143 }
else if (std::string(argv[i]) ==
"--hd_stream") {
145 }
else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
148 std::cout <<
"Error : unknown parameter " << argv[i] << std::endl
149 <<
"See " << argv[0] <<
" --help" << std::endl;
153 }
else if (argc >= 2 && (std::string(argv[1]) ==
"--help" || std::string(argv[1]) ==
"-h")) {
154 std::cout <<
"\nUsage:\n"
156 <<
" [--tag_size <size>] [--ip <drone ip>] [--distance_to_tag <distance>] [--intrinsic <xml file>] "
157 <<
"[--hd_stream] [--verbose] [-v] [--help] [-h]\n"
160 <<
" --tag_size <size>\n"
161 <<
" The size of the tag to detect in meters, required.\n\n"
162 <<
" --ip <drone ip>\n"
163 <<
" Ip address of the drone to which you want to connect (default : 192.168.42.1).\n\n"
164 <<
" --distance_to_tag <distance>\n"
165 <<
" The desired distance to the tag in meters (default: 1 meter).\n\n"
166 <<
" --intrinsic <xml file>\n"
167 <<
" XML file containing computed intrinsic camera parameters (default: empty).\n\n"
169 <<
" Enables HD 720p streaming instead of default 480p.\n"
170 <<
" Allows to increase range and accuracy of the tag detection,\n"
171 <<
" but increases latency and computation time.\n"
172 <<
" Caution: camera calibration settings are different for the two resolutions.\n"
173 <<
" Make sure that if you pass custom intrinsic camera parameters,\n"
174 <<
" they were obtained with the correct resolution.\n\n"
175 <<
" --verbose, -v\n"
176 <<
" Enables verbose (drone information messages and velocity commands\n"
177 <<
" are then displayed).\n\n"
179 <<
" Print help message.\n"
184 std::cout <<
"Error : tag size parameter required." << std::endl <<
"See " << argv[0] <<
" --help" << std::endl;
189 <<
"\nWARNING: \n - This program does no sensing or avoiding of "
191 "the drone WILL collide with any objects in the way! Make sure "
193 "drone has approximately 3 meters of free space on all sides.\n"
194 " - The drone uses a downward-facing camera for horizontal speed estimation,\n make sure the drone flies "
195 "above a non-uniform flooring,\n or its movement will be inacurate and dangerous !\n"
200 verbose,
true, ip_address);
202 if (drone.isRunning()) {
204 drone.setVideoResolution(stream_res);
206 drone.setStreamingMode(0);
207 drone.setVideoStabilisationMode(0);
211 drone.startStreaming();
213 drone.setExposure(1.5f);
215 drone.setCameraOrientation(-5., 0.,
221 drone.getGrayscaleImage(I);
223 #if defined VISP_HAVE_X11
225 #elif defined VISP_HAVE_GTK
227 #elif defined VISP_HAVE_GDI
229 #elif defined VISP_HAVE_OPENCV
232 int orig_displayX = 100;
233 int orig_displayY = 100;
234 display.init(I, orig_displayX, orig_displayY,
"DRONE VIEW");
238 vpPlot plotter(1, 700, 700, orig_displayX + static_cast<int>(I.
getWidth()) + 20, orig_displayY,
239 "Visual servoing tasks");
240 unsigned int iter = 0;
250 if (opt_has_cam_parameters) {
257 std::cout <<
"Cannot find parameters in XML file " << opt_cam_parameters << std::endl;
258 if (drone.getVideoHeight() == 720) {
265 std::cout <<
"Setting default camera parameters ... " << std::endl;
266 if (drone.getVideoHeight() == 720) {
314 double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
317 double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.};
318 double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.};
319 std::vector<vpPoint> vec_P, vec_P_d;
321 for (
int i = 0; i < 4; i++) {
325 vec_P_d.push_back(P_d);
337 m_obj_d.fromVector(vec_P_d);
350 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
352 area = mb_d.
get(0, 0);
354 man_d.setDesiredArea(area);
362 double C = 1.0 / Z_d;
374 plotter.initGraph(0, 4);
375 plotter.setLegend(0, 0,
"Xn");
376 plotter.setLegend(0, 1,
"Yn");
377 plotter.setLegend(0, 2,
"an");
378 plotter.setLegend(0, 3,
"atan(1/rho)");
381 s_mgn_d.update(A, B, C);
382 s_mgn_d.compute_interaction();
384 s_man_d.update(A, B, C);
385 s_man_d.compute_interaction();
392 bool vec_ip_has_been_sorted =
false;
393 std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
396 while (drone.isRunning() && drone.isStreaming() && runLoop) {
400 drone.getGrayscaleImage(I);
403 std::vector<vpHomogeneousMatrix> cMo_vec;
404 detector.
detect(I, tagSize, cam, cMo_vec);
408 std::stringstream ss;
409 ss <<
"Detection time: " << t <<
" ms";
416 std::vector<vpImagePoint> vec_ip = detector.
getPolygon(0);
418 for (
size_t i = 0; i < vec_ip.size(); i++) {
429 m_obj.fromVector(vec_P);
438 man.setDesiredArea(area);
442 s_mgn.update(A, B, C);
443 s_mgn.compute_interaction();
444 s_man.update(A, B, C);
445 s_man.compute_interaction();
450 if (!vec_ip_has_been_sorted) {
451 for (
size_t i = 0; i < vec_ip.size(); i++) {
454 std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
455 vec_ip_sorted.push_back(index_pair);
459 std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
461 vec_ip_has_been_sorted =
true;
466 vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
477 std::cout <<
"ve: " << ve.
t() << std::endl;
479 drone.setVelocity(ve, 1.0);
481 for (
size_t i = 0; i < 4; i++) {
483 std::stringstream ss;
506 std::stringstream sserr;
507 sserr <<
"Failed to detect an Apriltag, or detected multiple ones";
520 plotter.plot(0, iter, task.
getError());
523 std::stringstream sstime;
524 sstime <<
"Total time: " << totalTime <<
" ms";
535 std::cout <<
"ERROR : failed to setup drone control." << std::endl;
539 std::cout <<
"Caught an exception: " << e << std::endl;
544 #endif // #elif !defined(VISP_HAVE_FFMPEG)