 |
Visual Servoing Platform
version 3.2.0
|
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;
110 #ifdef VISP_HAVE_XML2
112 if (!intrinsic_file.empty() && !camera_name.empty())
115 std::cout <<
"cam:\n" << cam << std::endl;
116 std::cout <<
"use pose: " << use_pose << std::endl;
117 std::cout <<
"tagFamily: " << tagFamily << std::endl;
121 detector.setAprilTagQuadDecimate(quad_decimate);
123 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
124 detector.setAprilTagNbThreads(nThreads);
125 detector.setDisplayTag(display_tag);
139 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
140 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
141 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
148 eJe[0][0] = eJe[5][1] = 1.0;
150 std::cout <<
"eJe: \n" << eJe << std::endl;
170 std::cout <<
"Z " << Z << std::endl;
177 std::vector<double> time_vec;
184 std::vector<vpHomogeneousMatrix> cMo_vec;
186 detector.detect(I, tagSize, cam, cMo_vec);
191 time_vec.push_back(t);
193 std::stringstream ss;
194 ss <<
"Detection time: " << t <<
" ms";
197 if (detector.getNbObjects() == 1) {
209 serial->
write(
"LED_RING=2,0,10,0\n");
213 Z = cMo_vec[0][2][3];
216 vpPolygon polygon(detector.getPolygon(0));
217 double surface = polygon.
getArea();
218 std::cout <<
"Surface: " << surface << std::endl;
221 Z = tagSize * cam.
get_px() / sqrt(surface);
230 std::cout <<
"cog: " << detector.getCog(0) <<
" Z: " << Z << std::endl;
238 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
241 double radius = 0.0325;
243 double motor_left = (-v[0] - L * v[1]) / radius;
244 double motor_right = ( v[0] - L * v[1]) / radius;
245 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
250 std::stringstream ss;
251 double rpm_left = motor_left * 30. / M_PI;
252 double rpm_right = motor_right * 30. / M_PI;
254 std::cout <<
"Send: " << ss.str() << std::endl;
256 serial->
write(ss.str());
262 serial->
write(
"LED_RING=2,10,0,0\n");
265 serial->
write(
"MOTOR_RPM=0,-0\n");
271 if (display_on && save_image) {
280 serial->
write(
"LED_RING=0,0,0,0\n");
283 std::cout <<
"Benchmark computation time" << std::endl;
284 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
294 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
296 serial->
write(
"LED_RING=1,10,0,0\n");
304 #ifndef VISP_HAVE_APRILTAG
305 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
307 #ifndef VISP_HAVE_V4L2
308 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
310 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
static double getMedian(const std::vector< double > &v)
void setDevice(const std::string &devname)
void buildFrom(const double x, const double y, const double Z)
Generic class defining intrinsic camera parameters.
XML parser to load and save intrinsic camera parameters.
void set_eJe(const vpMatrix &eJe_)
void write(const std::string &s)
static double deg(double rad)
Defines a generic 2D polygon.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Class that consider the case of a translation vector.
static double getMean(const std::vector< double > &v)
void acquire(vpImage< unsigned char > &I)
static int round(const double x)
void set_Z(const double Z)
Implementation of column vector and the associated operations.
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))
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
static void write(const vpImage< unsigned char > &I, const std::string &filename)
static const vpColor green
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static unsigned int selectX()
static void display(const vpImage< unsigned char > &I)
static double getStdev(const std::vector< double > &v, const bool useBesselCorrection=false)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of a rotation matrix and operations on such kind of matrices.
Adaptive gain computation.
unsigned int getHeight() const
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static const vpColor none
const char * getMessage(void) const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines generic functionnalities for display.
error that can be emited by ViSP classes.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Generic functions for unicycle mobile robots.
unsigned int getWidth() const