#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentGravityCenterNormalized.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpSerial.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayFactory.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpUnicycle.h>
#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
#include <visp3/vs/vpServo.h>
int main(int argc, const char **argv)
{
#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_V4L2)
#ifdef ENABLE_VISP_NAMESPACE
#endif
int device = 0;
double tagSize = 0.065;
float quad_decimate = 4.0;
int nThreads = 2;
std::string intrinsic_file = "";
std::string camera_name = "";
bool display_on = false;
bool serial_off = false;
#if defined(VISP_HAVE_DISPLAY)
bool display_tag = false;
bool save_image = false;
#endif
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--tag-size" && i + 1 < argc) {
tagSize = std::atof(argv[i + 1]);
}
else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
device = std::atoi(argv[i + 1]);
}
else if (std::string(argv[i]) == "--quad-decimate" && i + 1 < argc) {
quad_decimate = (float)atof(argv[i + 1]);
}
else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
nThreads = std::atoi(argv[i + 1]);
}
else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
intrinsic_file = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) {
camera_name = std::string(argv[i + 1]);
}
#if defined(VISP_HAVE_DISPLAY)
else if (std::string(argv[i]) == "--display-tag") {
display_tag = true;
}
else if (std::string(argv[i]) == "--display-on") {
display_on = true;
}
else if (std::string(argv[i]) == "--save-image") {
save_image = true;
}
#endif
else if (std::string(argv[i]) == "--serial-off") {
serial_off = true;
}
else if (std::string(argv[i]) == "--tag-family" && i + 1 < argc) {
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "Usage: " << argv[0]
<< " [--input <camera input>] [--tag-size <tag_size in m>]"
" [--quad-decimate <quad_decimate>] [--nthreads <nb>]"
" [--intrinsic <intrinsic file>] [--camera-name <camera name>]"
" [--tag-family <family> (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
" [--display-tag]";
#if defined(VISP_HAVE_DISPLAY)
std::cout << " [--display-on] [--save-image]";
#endif
std::cout << " [--serial-off] [--help]" << std::endl;
return EXIT_SUCCESS;
}
}
if (!serial_off) {
serial =
new vpSerial(
"/dev/ttyAMA0", 115200);
serial->
write(
"LED_RING=0,0,0,0\n");
serial->
write(
"LED_RING=1,0,10,0\n");
}
try {
std::ostringstream device_name;
device_name << "/dev/video" << device;
#ifdef VISP_HAVE_DISPLAY
if (display_on) {
}
#endif
#if defined(VISP_HAVE_PUGIXML)
if (!intrinsic_file.empty() && !camera_name.empty()) {
}
#endif
std::cout << "cam:\n" << cam << std::endl;
std::cout << "tagFamily: " << tagFamily << std::endl;
std::cout << "tagSize: " << tagSize << std::endl;
#ifdef VISP_HAVE_DISPLAY
#endif
if (display_on) {
}
else {
}
cRe[0][0] = 0;
cRe[0][1] = -1;
cRe[0][2] = 0;
cRe[1][0] = 0;
cRe[1][1] = 0;
cRe[1][2] = -1;
cRe[2][0] = 1;
cRe[2][1] = 0;
cRe[2][2] = 0;
eJe[0][0] = eJe[5][1] = 1.0;
std::cout << "eJe: \n" << eJe << std::endl;
double Z_d = 0.4;
double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. };
double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. };
std::vector<vpPoint> vec_P, vec_P_d;
for (int i = 0; i < 4; i++) {
vec_P_d.push_back(P_d);
}
man_d(0, Z_d);
double area = 0;
area = mb_d.
get(2, 0) + mb_d.
get(0, 2);
else
double A = 0.0;
double B = 0.0;
double C = 1.0 / Z_d;
std::vector<double> time_vec;
for (;;) {
#ifdef VISP_HAVE_DISPLAY
#endif
std::vector<vpHomogeneousMatrix> cMo_vec;
detector.
detect(I, tagSize, cam, cMo_vec);
time_vec.push_back(t);
{
std::stringstream ss;
ss << "Detection time: " << t << " ms";
#ifdef VISP_HAVE_DISPLAY
#endif
}
if (!serial_off) {
serial->
write(
"LED_RING=2,0,10,0\n");
}
std::vector<vpImagePoint> vec_ip = detector.
getPolygon(0);
vec_P.clear();
for (size_t i = 0; i < vec_ip.size(); i++) {
double x = 0, y = 0;
vec_P.push_back(P);
}
#ifdef VISP_HAVE_DISPLAY
3);
3);
#endif
m_obj.fromVector(vec_P);
man.linkTo(mdb);
man.setDesiredArea(
area);
man.compute();
s_mgn.update(A, B, C);
s_mgn.compute_interaction();
s_man.update(A, B, C);
s_man.compute_interaction();
std::cout <<
"Send velocity to the mbot: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
double radius = 0.0325;
double L = 0.0725;
double motor_left = (-v[0] - L * v[1]) / radius;
double motor_right = (v[0] - L * v[1]) / radius;
std::cout << "motor left vel: " << motor_left << " motor right vel: " << motor_right << std::endl;
if (!serial_off) {
}
std::stringstream ss;
double rpm_left = motor_left * 30. / M_PI;
double rpm_right = motor_right * 30. / M_PI;
std::cout << "Send: " << ss.str() << std::endl;
if (!serial_off) {
}
}
else {
if (!serial_off) {
serial->
write(
"LED_RING=2,10,0,0\n");
serial->
write(
"MOTOR_RPM=0,-0\n");
}
}
#ifdef VISP_HAVE_DISPLAY
if (display_on && save_image) {
}
break;
}
#endif
}
if (!serial_off) {
serial->
write(
"LED_RING=0,0,0,0\n");
}
std::cout << "Benchmark computation time" << std::endl;
if (display_on) {
delete d;
}
if (!serial_off) {
delete serial;
}
}
std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
if (!serial_off) {
serial->
write(
"LED_RING=1,10,0,0\n");
}
}
return EXIT_SUCCESS;
#else
(void)argc;
(void)argv;
#ifndef VISP_HAVE_APRILTAG
std::cout << "ViSP is not build with Apriltag support" << std::endl;
#endif
#ifndef VISP_HAVE_V4L2
std::cout << "ViSP is not build with v4l2 support" << std::endl;
#endif
std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
return EXIT_SUCCESS;
#endif
}
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
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor green
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
void setAprilTagQuadDecimate(float quadDecimate)
bool detect(const vpImage< unsigned char > &I) VP_OVERRIDE
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
void setAprilTagNbThreads(int nThreads)
std::vector< std::vector< vpImagePoint > > & getPolygon()
size_t getNbObjects() const
vpImagePoint getCog(size_t i) const
Class that defines generic functionalities 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 emitted by ViSP classes.
const char * getMessage() const
Functionality computation for normalized surface moment feature. Computes the interaction matrix asso...
void compute_interaction() VP_OVERRIDE
Functionality computation for centered and normalized moment feature. Computes the interaction matrix...
void compute_interaction() VP_OVERRIDE
static unsigned int selectXn()
void update(double A, double B, double C)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
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.
void setDesiredArea(double a_star)
This class defines the 2D basic moment . This class is a wrapper for vpMomentObject which allows to u...
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 setType(vpObjectType input_type)
vpObjectType getType() const
void fromVector(std::vector< vpPoint > &points)
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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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()
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, bool verbose=true)
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()