 |
Visual Servoing Platform
version 3.2.0
|
1 #include <visp3/core/vpSerial.h>
3 #include <visp3/core/vpXmlParserCamera.h>
4 #include <visp3/core/vpMomentObject.h>
5 #include <visp3/core/vpPoint.h>
6 #include <visp3/core/vpMomentBasic.h>
7 #include <visp3/core/vpMomentGravityCenter.h>
8 #include <visp3/core/vpMomentDatabase.h>
9 #include <visp3/core/vpMomentCentered.h>
10 #include <visp3/core/vpMomentAreaNormalized.h>
11 #include <visp3/core/vpMomentGravityCenterNormalized.h>
12 #include <visp3/core/vpPixelMeterConversion.h>
13 #include <visp3/detection/vpDetectorAprilTag.h>
14 #include <visp3/gui/vpDisplayX.h>
15 #include <visp3/sensor/vpV4l2Grabber.h>
16 #include <visp3/io/vpImageIo.h>
17 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
18 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
19 #include <visp3/vs/vpServo.h>
20 #include <visp3/robot/vpUnicycle.h>
22 int main(
int argc,
const char **argv)
24 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
27 double tagSize = 0.065;
28 float quad_decimate = 4.0;
30 std::string intrinsic_file =
"";
31 std::string camera_name =
"";
32 bool display_tag =
false;
33 bool display_on =
false;
34 bool serial_off =
false;
35 bool save_image =
false;
37 for (
int i = 1; i < argc; i++) {
38 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
39 tagSize = std::atof(argv[i + 1]);
40 }
else if (std::string(argv[i]) ==
"--input" && i + 1 < argc) {
41 device = std::atoi(argv[i + 1]);
42 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
43 quad_decimate = (float)atof(argv[i + 1]);
44 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
45 nThreads = std::atoi(argv[i + 1]);
46 }
else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
47 intrinsic_file = std::string(argv[i + 1]);
48 }
else if (std::string(argv[i]) ==
"--camera_name" && i + 1 < argc) {
49 camera_name = std::string(argv[i + 1]);
50 }
else if (std::string(argv[i]) ==
"--display_tag") {
52 #if defined(VISP_HAVE_X11)
53 }
else if (std::string(argv[i]) ==
"--display_on") {
55 }
else if (std::string(argv[i]) ==
"--save_image") {
58 }
else if (std::string(argv[i]) ==
"--serial_off") {
60 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
62 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
63 std::cout <<
"Usage: " << argv[0]
64 <<
" [--input <camera input>] [--tag_size <tag_size in m>]"
65 " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
66 " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
67 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: "
69 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
71 #if defined(VISP_HAVE_X11)
72 std::cout <<
" [--display_on] [--save_image]";
74 std::cout <<
" [--serial_off] [--help]" << std::endl;
88 serial =
new vpSerial(
"/dev/ttyAMA0", 115200);
90 serial->
write(
"LED_RING=0,0,0,0\n");
91 serial->
write(
"LED_RING=1,0,10,0\n");
98 std::ostringstream device_name;
99 device_name <<
"/dev/video" << device;
114 #ifdef VISP_HAVE_XML2
116 if (!intrinsic_file.empty() && !camera_name.empty())
119 std::cout <<
"cam:\n" << cam << std::endl;
120 std::cout <<
"tagFamily: " << tagFamily << std::endl;
121 std::cout <<
"tagSize: " << tagSize << std::endl;
125 detector.setAprilTagQuadDecimate(quad_decimate);
126 detector.setAprilTagNbThreads(nThreads);
127 detector.setDisplayTag(display_tag);
141 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
142 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
143 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
150 eJe[0][0] = eJe[5][1] = 1.0;
152 std::cout <<
"eJe: \n" << eJe << std::endl;
158 double X[4] = {tagSize/2., tagSize/2., -tagSize/2., -tagSize/2.};
159 double Y[4] = {tagSize/2., -tagSize/2., -tagSize/2., tagSize/2.};
160 std::vector<vpPoint> vec_P, vec_P_d;
162 for (
int i = 0; i < 4; i++) {
166 vec_P_d.push_back(P_d);
179 m_obj_d.fromVector(vec_P_d);
192 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
194 area = mb_d.
get(0, 0);
196 man_d.setDesiredArea(area);
204 double C = 1.0 / Z_d;
215 s_mgn_d.update(A, B, C);
216 s_mgn_d.compute_interaction();
218 s_man_d.update(A, B, C);
219 s_man_d.compute_interaction();
221 std::vector<double> time_vec;
228 std::vector<vpHomogeneousMatrix> cMo_vec;
229 detector.detect(I, tagSize, cam, cMo_vec);
231 time_vec.push_back(t);
233 std::stringstream ss;
234 ss <<
"Detection time: " << t <<
" ms";
237 if (detector.getNbObjects() == 1) {
239 serial->
write(
"LED_RING=2,0,10,0\n");
243 std::vector< vpImagePoint > vec_ip = detector.getPolygon(0);
245 for (
size_t i = 0; i < vec_ip.size(); i++) {
261 m_obj.fromVector(vec_P);
270 man.setDesiredArea(area);
274 s_mgn.update(A, B, C);
275 s_mgn.compute_interaction();
276 s_man.update(A, B, C);
277 s_man.compute_interaction();
285 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
288 double radius = 0.0325;
290 double motor_left = (-v[0] - L * v[1]) / radius;
291 double motor_right = ( v[0] - L * v[1]) / radius;
292 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
297 std::stringstream ss;
298 double rpm_left = motor_left * 30. / M_PI;
299 double rpm_right = motor_right * 30. / M_PI;
301 std::cout <<
"Send: " << ss.str() << std::endl;
303 serial->
write(ss.str());
309 serial->
write(
"LED_RING=2,10,0,0\n");
312 serial->
write(
"MOTOR_RPM=0,-0\n");
319 if (display_on && save_image) {
328 serial->
write(
"LED_RING=0,0,0,0\n");
331 std::cout <<
"Benchmark computation time" << std::endl;
332 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
342 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
344 serial->
write(
"LED_RING=1,10,0,0\n");
352 #ifndef VISP_HAVE_APRILTAG
353 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
355 #ifndef VISP_HAVE_V4L2
356 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
358 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...
void linkTo(vpMomentDatabase &moments)
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)
This class defines the double-indexed centered moment descriptor .
static double getMedian(const std::vector< double > &v)
void set_x(const double x)
Set the point x coordinate in the image plane.
void setDevice(const std::string &devname)
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)
virtual void updateAll(vpMomentObject &object)
Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
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)
This class allows to register all vpMoments so they can access each other according to their dependen...
Implementation of column vector and the associated operations.
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 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)
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
This class defines the 2D basic moment . This class is a wrapper for vpMomentObject wich allows to us...
Implementation of a rotation matrix and operations on such kind of matrices.
Adaptive gain computation.
unsigned int getHeight() const
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class describing 2D normalized gravity center moment.
const char * getMessage(void) const
Class for generic objects.
const std::vector< double > & get() const
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void set_y(const double y)
Set the point y coordinate in the image plane.
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)
Class that defines what is a point.
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.
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1)
Class that defines generic functionnalities for display.
Class describing 2D gravity center moment.
error that can be emited by ViSP classes.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static unsigned int selectXn()
Generic functions for unicycle mobile robots.
unsigned int getWidth() const