 |
Visual Servoing Platform
version 3.3.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_PUGIXML
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;
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);
234 std::stringstream ss;
235 ss <<
"Detection time: " << t <<
" ms";
241 serial->
write(
"LED_RING=2,0,10,0\n");
245 std::vector< vpImagePoint > vec_ip = detector.
getPolygon(0);
247 for (
size_t i = 0; i < vec_ip.size(); i++) {
263 m_obj.fromVector(vec_P);
272 man.setDesiredArea(area);
276 s_mgn.update(A, B, C);
277 s_mgn.compute_interaction();
278 s_man.update(A, B, C);
279 s_man.compute_interaction();
287 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
290 double radius = 0.0325;
292 double motor_left = (-v[0] - L * v[1]) / radius;
293 double motor_right = ( v[0] - L * v[1]) / radius;
294 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
299 std::stringstream ss;
300 double rpm_left = motor_left * 30. / M_PI;
301 double rpm_right = motor_right * 30. / M_PI;
303 std::cout <<
"Send: " << ss.str() << std::endl;
305 serial->
write(ss.str());
311 serial->
write(
"LED_RING=2,10,0,0\n");
314 serial->
write(
"MOTOR_RPM=0,-0\n");
321 if (display_on && save_image) {
330 serial->
write(
"LED_RING=0,0,0,0\n");
333 std::cout <<
"Benchmark computation time" << std::endl;
334 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
344 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
346 serial->
write(
"LED_RING=1,10,0,0\n");
354 #ifndef VISP_HAVE_APRILTAG
355 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
357 #ifndef VISP_HAVE_V4L2
358 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
360 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)
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
This class defines the double-indexed centered moment descriptor .
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 setDevice(const std::string &devname)
void setAprilTagNbThreads(int nThreads)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
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)
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...
size_t getNbObjects() const
void set_cVe(const vpVelocityTwistMatrix &cVe_)
unsigned int getHeight() const
Class that consider the case of a translation vector.
static double getMean(const std::vector< double > &v)
void acquire(vpImage< unsigned char > &I)
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.
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
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()
unsigned int getWidth() const
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)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
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)
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
vpImagePoint getCog(size_t i) const
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.
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)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class describing 2D normalized gravity center moment.
void set_y(double y)
Set the point y coordinate in the image plane.
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 for generic objects.
const std::vector< double > & get() const
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)
static const vpColor green
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)
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 getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_x(double x)
Set the point x coordinate in the image plane.
Class that defines generic functionnalities for display.
Class describing 2D gravity center moment.
error that can be emited by ViSP classes.
static unsigned int selectXn()
Generic functions for unicycle mobile robots.