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;
115 if (!intrinsic_file.empty() && !camera_name.empty())
118 std::cout <<
"cam:\n" << cam << std::endl;
119 std::cout <<
"tagFamily: " << tagFamily << std::endl;
120 std::cout <<
"tagSize: " << tagSize << std::endl;
124 detector.setAprilTagQuadDecimate(quad_decimate);
125 detector.setAprilTagNbThreads(nThreads);
126 detector.setDisplayTag(display_tag);
140 cRe[0][0] = 0; cRe[0][1] = -1; cRe[0][2] = 0;
141 cRe[1][0] = 0; cRe[1][1] = 0; cRe[1][2] = -1;
142 cRe[2][0] = 1; cRe[2][1] = 0; cRe[2][2] = 0;
149 eJe[0][0] = eJe[5][1] = 1.0;
151 std::cout <<
"eJe: \n" << eJe << std::endl;
157 double X[4] = {tagSize/2., tagSize/2., -tagSize/2., -tagSize/2.};
158 double Y[4] = {tagSize/2., -tagSize/2., -tagSize/2., tagSize/2.};
159 std::vector<vpPoint> vec_P, vec_P_d;
161 for (
int i = 0; i < 4; i++) {
165 vec_P_d.push_back(P_d);
178 m_obj_d.fromVector(vec_P_d);
191 area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
193 area = mb_d.
get(0, 0);
195 man_d.setDesiredArea(area);
203 double C = 1.0 / Z_d;
214 s_mgn_d.update(A, B, C);
215 s_mgn_d.compute_interaction();
217 s_man_d.update(A, B, C);
218 s_man_d.compute_interaction();
220 std::vector<double> time_vec;
227 std::vector<vpHomogeneousMatrix> cMo_vec;
228 detector.detect(I, tagSize, cam, cMo_vec);
230 time_vec.push_back(t);
233 std::stringstream ss;
234 ss <<
"Detection time: " << t <<
" ms";
238 if (detector.getNbObjects() == 1) {
240 serial->
write(
"LED_RING=2,0,10,0\n");
244 std::vector< vpImagePoint > vec_ip = detector.getPolygon(0);
246 for (
size_t i = 0; i < vec_ip.size(); i++) {
262 m_obj.fromVector(vec_P);
271 man.setDesiredArea(area);
275 s_mgn.update(A, B, C);
276 s_mgn.compute_interaction();
277 s_man.update(A, B, C);
278 s_man.compute_interaction();
286 std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
289 double radius = 0.0325;
291 double motor_left = (-v[0] - L * v[1]) / radius;
292 double motor_right = ( v[0] - L * v[1]) / radius;
293 std::cout <<
"motor left vel: " << motor_left <<
" motor right vel: " << motor_right << std::endl;
298 std::stringstream ss;
299 double rpm_left = motor_left * 30. / M_PI;
300 double rpm_right = motor_right * 30. / M_PI;
302 std::cout <<
"Send: " << ss.str() << std::endl;
304 serial->
write(ss.str());
310 serial->
write(
"LED_RING=2,10,0,0\n");
313 serial->
write(
"MOTOR_RPM=0,-0\n");
320 if (display_on && save_image) {
329 serial->
write(
"LED_RING=0,0,0,0\n");
332 std::cout <<
"Benchmark computation time" << std::endl;
333 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
343 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
345 serial->
write(
"LED_RING=1,10,0,0\n");
353 #ifndef VISP_HAVE_APRILTAG
354 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
356 #ifndef VISP_HAVE_V4L2
357 std::cout <<
"ViSP is not build with v4l2 support" << std::endl;
359 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 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 displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
static void displayPolygon(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip, const vpColor &color, unsigned int thickness=1, bool closed=true)
error that can be emited by ViSP classes.
const char * getMessage() const
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
static unsigned int selectXn()
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void write(const vpImage< unsigned char > &I, const std::string &filename)
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.
Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
This class defines the 2D basic moment . This class is a wrapper for vpMomentObject wich allows to us...
const std::vector< double > & get() const
This class defines the double-indexed centered moment descriptor .
This class allows to register all vpMoments so they can access each other according to their dependen...
virtual void updateAll(vpMomentObject &object)
Class describing 2D normalized gravity center moment.
Class describing 2D gravity center moment.
Class for generic objects.
void linkTo(vpMomentDatabase &moments)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_x(double x)
Set the point x coordinate in the image plane.
void set_y(double y)
Set the point y coordinate in the image plane.
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()