Example of eye-in-hand image-based control law. We control here a real robot, the FLIR PTU that has 2 degrees of freedom. The velocity is computed in the joint space. Visual features are the image coordinates of the center of gravity of an AprilTag. The goal is here to center the tag in the image acquired from a FLIR camera mounted on the PTU.
Camera extrinsic (eMc) parameters are set by default to a value that will not match Your configuration. Use –eMc command line option to read the values from a file. This file could be obtained following extrinsic camera calibration tutorial: https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-calibration-extrinsic.html
Camera intrinsic parameters are retrieved from the Realsense SDK.
The target is an AprilTag. It's size doesn't matter since we are using the center of gravity position.
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpRobotFlirPtu.h>
#include <visp3/sensor/vpFlyCaptureGrabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \
(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PUGIXML)
#ifdef ENABLE_VISP_NAMESPACE
#endif
std::vector<vpImagePoint> &traj_ip)
{
if (traj_ip.size()) {
traj_ip.push_back(ip);
}
}
else {
traj_ip.push_back(ip);
}
for (size_t j = 1; j < traj_ip.size(); j++) {
}
}
int main(int argc, char **argv)
{
std::string opt_portname;
int opt_baudrate = 9600;
bool opt_network = false;
std::string opt_extrinsic;
std::string opt_intrinsic;
std::string opt_camera_name;
bool display_tag = true;
int opt_quad_decimate = 2;
double opt_tag_size = 0.120;
bool opt_verbose = false;
bool opt_plot = false;
bool opt_adaptive_gain = false;
bool opt_task_sequencing = false;
double opt_constant_gain = 0.5;
bool opt_display_trajectory = true;
double convergence_threshold = 0.00005;
if (argc == 1) {
std::cout << "To see how to use this example, run: " << argv[0] << " --help" << std::endl;
return EXIT_SUCCESS;
}
for (int i = 1; i < argc; i++) {
if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) {
opt_portname = std::string(argv[i + 1]);
}
else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
opt_baudrate = std::atoi(argv[i + 1]);
}
else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
opt_network = true;
}
else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) {
opt_extrinsic = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
opt_intrinsic = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) {
opt_camera_name = std::string(argv[i + 1]);
}
else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
opt_verbose = true;
}
else if (std::string(argv[i]) == "--plot" || std::string(argv[i]) == "-p") {
opt_plot = true;
}
else if (std::string(argv[i]) == "--display-image-trajectory" || std::string(argv[i]) == "-traj") {
opt_display_trajectory = true;
}
else if (std::string(argv[i]) == "--adaptive-gain" || std::string(argv[i]) == "-a") {
opt_adaptive_gain = true;
}
else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") {
opt_constant_gain = std::stod(argv[i + 1]);
}
else if (std::string(argv[i]) == "--task-sequencing") {
opt_task_sequencing = true;
}
else if (std::string(argv[i]) == "--quad-decimate" && i + 1 < argc) {
opt_quad_decimate = std::stoi(argv[i + 1]);
}
if (std::string(argv[i]) == "--tag-size" && i + 1 < argc) {
opt_tag_size = std::stod(argv[i + 1]);
}
else if (std::string(argv[i]) == "--no-convergence-threshold") {
convergence_threshold = 0.;
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "SYNOPSIS" << std::endl
<< " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] "
<< "[--extrinsic <extrinsic.yaml>] [--intrinsic <intrinsic.xml>] [--camera-name <name>] "
<< "[--quad-decimate <decimation>] [--tag-size <size>] "
<< "[--adaptive-gain] [--constant-gain] [--display-image-trajectory] [--plot] [--task-sequencing] "
<< "[--no-convergence-threshold] [--verbose] [--help] [-h]" << std::endl
<< std::endl;
std::cout << "DESCRIPTION" << std::endl
<< " --portname, -p <portname>" << std::endl
<< " Set serial or tcp port name." << std::endl
<< std::endl
<< " --baudrate, -b <rate>" << std::endl
<< " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl
<< std::endl
<< " --network, -n" << std::endl
<< " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
<< std::endl
<< " --reset, -r" << std::endl
<< " Reset PTU axis and exit. " << std::endl
<< std::endl
<< " --extrinsic <extrinsic.yaml>" << std::endl
<< " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
<< " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
<< " and camera frame." << std::endl
<< std::endl
<< " --intrinsic <intrinsic.xml>" << std::endl
<< " Intrinsic camera parameters obtained after camera calibration." << std::endl
<< std::endl
<< " --camera-name <name>" << std::endl
<< " Name of the camera to consider in the xml file provided for intrinsic camera parameters."
<< std::endl
<< std::endl
<< " --quad-decimate <decimation>" << std::endl
<< " Decimation factor used to detect AprilTag. Default " << opt_quad_decimate << "." << std::endl
<< std::endl
<< " --tag-size <size>" << std::endl
<< " Width in meter or the black part of the AprilTag used as target. Default " << opt_tag_size
<< "." << std::endl
<< std::endl
<< " --adaptive-gain, -a" << std::endl
<< " Enable adaptive gain instead of constant gain to speed up convergence. " << std::endl
<< std::endl
<< " --constant-gain, -g" << std::endl
<< " Constant gain value. Default value: " << opt_constant_gain << std::endl
<< std::endl
<< " --display-image-trajectory, -traj" << std::endl
<< " Display the trajectory of the target cog in the image. " << std::endl
<< std::endl
<< " --plot, -p" << std::endl
<< " Enable curve plotter. " << std::endl
<< std::endl
<< " --task-sequencing" << std::endl
<< " Enable task sequencing that allows to smoothly control the velocity of the robot. " << std::endl
<< std::endl
<< " --no-convergence-threshold" << std::endl
<< " Disable ending servoing when it reaches the desired position." << std::endl
<< std::endl
<< " --verbose, -v" << std::endl
<< " Additional printings. " << std::endl
<< std::endl
<< " --help, -h" << std::endl
<< " Print this helper message. " << std::endl
<< std::endl;
std::cout << "EXAMPLE" << std::endl
<< " - How to get network IP" << std::endl
#ifdef _WIN32
<< " $ " << argv[0] << " --portname COM1 --network" << std::endl
<< " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
#else
<< " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl
<< " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
#endif
<< " PTU HostName: PTU-5" << std::endl
<< " PTU IP : 169.254.110.254" << std::endl
<< " PTU Gateway : 0.0.0.0" << std::endl
<< " - How to run this binary using network communication" << std::endl
<< " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
return EXIT_SUCCESS;
}
}
try {
std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
robot.connect(opt_portname, opt_baudrate);
if (opt_network) {
std::cout << "PTU HostName: " << robot.getNetworkHostName() << std::endl;
std::cout << "PTU IP : " << robot.getNetworkIP() << std::endl;
std::cout << "PTU Gateway : " << robot.getNetworkGateway() << std::endl;
return EXIT_SUCCESS;
}
eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
etc << -0.1, -0.123, 0.035;
if (!opt_extrinsic.empty()) {
eMc.buildFrom(ePc);
}
else {
std::cout << "***************************************************************" << std::endl;
std::cout << "Warning, use hard coded values for extrinsic camera parameters." << std::endl;
std::cout << "Create a yaml file that contains the extrinsic:" << std::endl
<< std::endl
<< "$ cat eMc.yaml" << std::endl
<< "rows: 4" << std::endl
<< "cols: 4" << std::endl
<< "data:" << std::endl
<< " - [0, 0, 1, -0.1]" << std::endl
<< " - [-1, 0, 0, -0.123]" << std::endl
<< " - [0, -1, 0, 0.035]" << std::endl
<< " - [0, 0, 0, 1]" << std::endl
<< std::endl
<< "and load this file with [--extrinsic <extrinsic.yaml] command line option, like:" << std::endl
<< std::endl
<< "$ " << argv[0] << "-p " << opt_portname << " --extrinsic eMc.yaml" << std::endl
<< std::endl;
std::cout << "***************************************************************" << std::endl;
}
std::cout << "Considered extrinsic transformation eMc:\n" << eMc << std::endl;
if (!opt_intrinsic.empty() && !opt_camera_name.empty()) {
}
else {
std::cout << "***************************************************************" << std::endl;
std::cout << "Warning, use hard coded values for intrinsic camera parameters." << std::endl;
std::cout << "Calibrate your camera and load the parameters from command line options, like:" << std::endl
<< std::endl
<< "$ " << argv[0] << "-p " << opt_portname << " --intrinsic camera.xml --camera-name \"Camera\""
<< std::endl
<< std::endl;
std::cout << "***************************************************************" << std::endl;
}
std::cout << "Considered intrinsic camera parameters:\n" << cam << "\n";
#if defined(VISP_HAVE_X11)
vpDisplayX dc(I, 10, 10, "Color image");
#elif defined(VISP_HAVE_GDI)
#endif
detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
detector.setDisplayTag(display_tag);
detector.setAprilTagQuadDecimate(opt_quad_decimate);
if (opt_adaptive_gain) {
}
else {
}
int iter_plot = 0;
if (opt_plot) {
plotter =
new vpPlot(2,
static_cast<int>(250 * 2), 500,
static_cast<int>(I.
getWidth()) + 80, 10,
"Real time curves plotter");
plotter->
setTitle(0,
"Visual features error");
plotter->
setTitle(1,
"Joint velocities");
}
bool final_quit = false;
bool has_converged = false;
bool send_velocities = false;
bool servo_started = false;
std::vector<vpImagePoint> traj_cog;
robot.set_eMc(eMc);
std::cout << cVe << std::endl;
while (!has_converged && !final_quit) {
std::vector<vpHomogeneousMatrix> cMo_vec;
detector.detect(I, opt_tag_size, cam, cMo_vec);
std::stringstream ss;
ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
if (detector.getNbObjects() == 1) {
double Z = cMo_vec[0][2][3];
double x = 0, y = 0;
if (opt_verbose) {
std::cout << "Z: " << Z << std::endl;
}
if (opt_task_sequencing) {
if (!servo_started) {
if (send_velocities) {
servo_started = true;
}
}
}
else {
}
if (opt_display_trajectory) {
display_point_trajectory(I, cog, traj_cog);
}
if (opt_plot) {
plotter->
plot(1, iter_plot, qdot);
iter_plot++;
}
if (opt_verbose) {
std::cout << "qdot: " << qdot.t() << std::endl;
}
ss.str("");
ss << "error: " << error;
if (opt_verbose)
std::cout << "error: " << error << std::endl;
if (error < convergence_threshold) {
has_converged = true;
std::cout << "Servo task has converged"
<< "\n";
}
}
else {
qdot = 0;
}
if (!send_velocities) {
qdot = 0;
}
ss.str("");
switch (button) {
send_velocities = !send_velocities;
break;
final_quit = true;
qdot = 0;
break;
default:
break;
}
}
}
std::cout << "Stop the robot " << std::endl;
if (opt_plot && plotter != nullptr) {
delete plotter;
plotter = nullptr;
}
if (!final_quit) {
while (!final_quit) {
final_quit = true;
}
}
}
}
std::cout <<
"Catch Flir Ptu signal exception: " << e.
getMessage() << std::endl;
}
std::cout <<
"ViSP exception: " << e.
what() << std::endl;
std::cout << "Stop the robot " << std::endl;
}
return EXIT_SUCCESS;
}
#else
int main()
{
#if !defined(VISP_HAVE_FLYCAPTURE)
std::cout << "Install FLIR Flycapture" << std::endl;
#endif
#if !defined(VISP_HAVE_FLIR_PTU_SDK)
std::cout << "Install FLIR PTU SDK." << std::endl;
#endif
#if !defined(VISP_HAVE_PUGIXML)
std::cout << "pugixml built-in 3rdparty is requested." << std::endl;
#endif
return EXIT_SUCCESS;
}
#endif
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor green
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
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 flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
const char * getMessage() const
const char * what() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void set_xyZ(double x, double y, double Z)
void open(vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setTitle(unsigned int graphNum, const std::string &title)
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
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 set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const
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)
VISP_EXPORT double measureTimeMs()