 |
Visual Servoing Platform
version 3.3.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_PUGIXML
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;
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);
194 std::stringstream ss;
195 ss <<
"Detection time: " << t <<
" ms";
211 serial->
write(
"LED_RING=2,0,10,0\n");
215 Z = cMo_vec[0][2][3];
219 double surface = polygon.
getArea();
220 std::cout <<
"Surface: " << surface << std::endl;
223 Z = tagSize * cam.
get_px() / sqrt(surface);
232 std::cout <<
"cog: " << detector.
getCog(0) <<
" Z: " << Z << std::endl;
240 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
243 double radius = 0.0325;
245 double motor_left = (-v[0] - L * v[1]) / radius;
246 double motor_right = ( v[0] - L * v[1]) / radius;
247 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
252 std::stringstream ss;
253 double rpm_left = motor_left * 30. / M_PI;
254 double rpm_right = motor_right * 30. / M_PI;
256 std::cout <<
"Send: " << ss.str() << std::endl;
258 serial->
write(ss.str());
264 serial->
write(
"LED_RING=2,10,0,0\n");
267 serial->
write(
"MOTOR_RPM=0,-0\n");
273 if (display_on && save_image) {
282 serial->
write(
"LED_RING=0,0,0,0\n");
285 std::cout <<
"Benchmark computation time" << std::endl;
286 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
296 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
298 serial->
write(
"LED_RING=1,10,0,0\n");
306 #ifndef VISP_HAVE_APRILTAG
307 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
309 #ifndef VISP_HAVE_V4L2
310 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
312 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...
AprilTag 36h11 pattern (recommended)
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 double getMedian(const std::vector< double > &v)
void buildFrom(double x, double y, double Z, double LogZoverZstar)
void setDevice(const std::string &devname)
void setAprilTagNbThreads(int nThreads)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
void buildFrom(double x, double y, double Z)
Generic class defining intrinsic camera parameters.
std::vector< std::vector< vpImagePoint > > & getPolygon()
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)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
Defines a generic 2D polygon.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
size_t getNbObjects() const
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)
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))
bool detect(const vpImage< unsigned char > &I)
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)
void setAprilTagQuadDecimate(float quadDecimate)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
vpImagePoint getCog(size_t i) const
Implementation of a rotation matrix and operations on such kind of matrices.
Adaptive gain computation.
unsigned int getHeight() const
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)
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
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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)
static int round(double x)
vpColVector computeControlLaw()
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 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.
Generic functions for unicycle mobile robots.
unsigned int getWidth() const