1 #include <visp3/core/vpXmlParserCamera.h>
3 #include <visp3/core/vpSerial.h>
4 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/sensor/vpV4l2Grabber.h>
7 #include <visp3/io/vpImageIo.h>
8 #include <visp3/visual_features/vpFeatureBuilder.h>
9 #include <visp3/visual_features/vpFeatureDepth.h>
10 #include <visp3/visual_features/vpFeaturePoint.h>
11 #include <visp3/vs/vpServo.h>
12 #include <visp3/robot/vpUnicycle.h>
13 #include <visp3/core/vpPolygon.h>
15 int main(
int argc,
const char **argv)
17 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
21 double tagSize = 0.065;
22 float quad_decimate = 4.0;
24 std::string intrinsic_file =
"";
25 std::string camera_name =
"";
26 bool display_tag =
false;
27 bool display_on =
false;
28 bool serial_off =
false;
30 bool save_image =
false;
32 for (
int i = 1; i < argc; i++) {
33 if (std::string(argv[i]) ==
"--without_pose_computation") {
35 }
else if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
36 tagSize = std::atof(argv[i + 1]);
37 }
else if (std::string(argv[i]) ==
"--input" && i + 1 < argc) {
38 device = std::atoi(argv[i + 1]);
39 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
40 quad_decimate = (float)atof(argv[i + 1]);
41 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
42 nThreads = std::atoi(argv[i + 1]);
43 }
else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
44 intrinsic_file = std::string(argv[i + 1]);
45 }
else if (std::string(argv[i]) ==
"--camera_name" && i + 1 < argc) {
46 camera_name = std::string(argv[i + 1]);
47 }
else if (std::string(argv[i]) ==
"--display_tag") {
49 #if defined(VISP_HAVE_X11)
50 }
else if (std::string(argv[i]) ==
"--display_on") {
52 }
else if (std::string(argv[i]) ==
"--save_image") {
55 }
else if (std::string(argv[i]) ==
"--serial_off") {
57 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
59 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
60 std::cout <<
"Usage: " << argv[0]
61 <<
" [--input <camera input>] [--tag_size <tag_size in m>]"
62 " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
63 " [--intrinsic <intrinsic file>] [--camera_name <camera name>] [--without_pose_computation]"
64 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT,"
65 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
67 #if defined(VISP_HAVE_X11)
68 std::cout <<
" [--display_on] [--save_image]";
70 std::cout <<
" [--serial_off] [--help]" << std::endl;
84 serial =
new vpSerial(
"/dev/ttyAMA0", 115200);
86 serial->
write(
"LED_RING=0,0,0,0\n");
87 serial->
write(
"LED_RING=1,0,10,0\n");
94 std::ostringstream device_name;
95 device_name <<
"/dev/video" << device;
111 if (!intrinsic_file.empty() && !camera_name.empty())
114 std::cout <<
"cam:\n" << cam << std::endl;
115 std::cout <<
"use pose: " << use_pose << std::endl;
116 std::cout <<
"tagFamily: " << tagFamily << std::endl;
120 detector.setAprilTagQuadDecimate(quad_decimate);
122 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
123 detector.setAprilTagNbThreads(nThreads);
124 detector.setDisplayTag(display_tag);
138 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
139 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
140 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
147 eJe[0][0] = eJe[5][1] = 1.0;
149 std::cout <<
"eJe: \n" << eJe << std::endl;
169 std::cout <<
"Z " << Z << std::endl;
176 std::vector<double> time_vec;
183 std::vector<vpHomogeneousMatrix> cMo_vec;
185 detector.detect(I, tagSize, cam, cMo_vec);
190 time_vec.push_back(t);
193 std::stringstream ss;
194 ss <<
"Detection time: " << t <<
" ms";
198 if (detector.getNbObjects() == 1) {
210 serial->
write(
"LED_RING=2,0,10,0\n");
214 Z = cMo_vec[0][2][3];
217 vpPolygon polygon(detector.getPolygon(0));
218 double surface = polygon.getArea();
219 std::cout <<
"Surface: " << surface << std::endl;
222 Z = tagSize * cam.
get_px() / sqrt(surface);
231 std::cout <<
"cog: " << detector.getCog(0) <<
" Z: " << Z << std::endl;
239 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
242 double radius = 0.0325;
244 double motor_left = (-v[0] - L * v[1]) / radius;
245 double motor_right = ( v[0] - L * v[1]) / radius;
246 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
251 std::stringstream ss;
252 double rpm_left = motor_left * 30. / M_PI;
253 double rpm_right = motor_right * 30. / M_PI;
255 std::cout <<
"Send: " << ss.str() << std::endl;
257 serial->
write(ss.str());
263 serial->
write(
"LED_RING=2,10,0,0\n");
266 serial->
write(
"MOTOR_RPM=0,-0\n");
272 if (display_on && save_image) {
281 serial->
write(
"LED_RING=0,0,0,0\n");
284 std::cout <<
"Benchmark computation time" << std::endl;
285 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
295 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
297 serial->
write(
"LED_RING=1,10,0,0\n");
305 #ifndef VISP_HAVE_APRILTAG
306 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
308 #ifndef VISP_HAVE_V4L2
309 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
311 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Implementation of column vector and the associated operations.
static const vpColor none
static const vpColor green
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Class that defines generic functionnalities for display.
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 getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
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 displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
const char * getMessage() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void buildFrom(double x, double y, double Z, double LogZoverZstar)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
static unsigned int selectX()
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
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 getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static int round(double x)
static double getMean(const std::vector< double > &v)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Defines a generic 2D polygon.
Implementation of a rotation matrix and operations on such kind of matrices.
void write(const std::string &s)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that consider the case of a translation vector.
Generic functions for unicycle mobile robots.
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)
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)
VISP_EXPORT double measureTimeMs()