Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-mb-generic-tracker-apriltag-rs2.cpp
#include <fstream>
#include <ios>
#include <iostream>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/mbt/vpMbGenericTracker.h>
#include <visp3/sensor/vpRealSense2.h>
typedef enum { state_detection, state_tracking, state_quit } state_t;
// Creates a cube.cao file in your current directory
// cubeEdgeSize : size of cube edges in meters
void createCaoFile(double cubeEdgeSize)
{
std::ofstream fileStream;
fileStream.open("cube.cao", std::ofstream::out | std::ofstream::trunc);
fileStream << "V1\n";
fileStream << "# 3D Points\n";
fileStream << "8 # Number of points\n";
fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 0: (X, Y, Z)\n";
fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 1\n";
fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << 0 << " # Point 2\n";
fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << 0 << " # Point 3\n";
fileStream << -cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 4\n";
fileStream << -cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 5\n";
fileStream << cubeEdgeSize / 2 << " " << -cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 6\n";
fileStream << cubeEdgeSize / 2 << " " << cubeEdgeSize / 2 << " " << -cubeEdgeSize << " # Point 7\n";
fileStream << "# 3D Lines\n";
fileStream << "0 # Number of lines\n";
fileStream << "# Faces from 3D lines\n";
fileStream << "0 # Number of faces\n";
fileStream << "# Faces from 3D points\n";
fileStream << "6 # Number of faces\n";
fileStream << "4 0 3 2 1 # Face 0: [number of points] [index of the 3D points]...\n";
fileStream << "4 1 2 5 6\n";
fileStream << "4 4 7 6 5\n";
fileStream << "4 0 7 4 3\n";
fileStream << "4 5 2 3 4\n";
fileStream << "4 0 1 6 7 # Face 5\n";
fileStream << "# 3D cylinders\n";
fileStream << "0 # Number of cylinders\n";
fileStream << "# 3D circles\n";
fileStream << "0 # Number of circles\n";
fileStream.close();
}
#if defined(VISP_HAVE_APRILTAG)
state_t detectAprilTag(const vpImage<unsigned char> &I, vpDetectorAprilTag &detector, double tagSize,
{
std::vector<vpHomogeneousMatrix> cMo_vec;
// Detection
bool ret = detector.detect(I, tagSize, cam, cMo_vec);
// Display camera pose
for (size_t i = 0; i < cMo_vec.size(); i++) {
vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
}
vpDisplay::displayText(I, 40, 20, "State: waiting tag detection", vpColor::red);
if (ret && detector.getNbObjects() > 0) { // if tag detected, we pick the first one
cMo = cMo_vec[0];
return state_tracking;
}
return state_detection;
}
#endif // #if defined(VISP_HAVE_APRILTAG)
state_t track(const vpImage<unsigned char> &I, vpMbGenericTracker &tracker, double projection_error_threshold,
{
tracker.getCameraParameters(cam);
// Track the object
try {
tracker.track(I);
}
catch (...) {
return state_detection;
}
tracker.getPose(cMo);
// Detect tracking error
double projection_error = tracker.computeCurrentProjectionError(I, cMo, cam);
if (projection_error > projection_error_threshold) {
return state_detection;
}
// Display
tracker.display(I, cMo, cam, vpColor::red, 2);
vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
vpDisplay::displayText(I, 40, 20, "State: tracking in progress", vpColor::red);
{
std::stringstream ss;
ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt();
vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
}
return state_tracking;
}
state_t track(std::map<std::string, const vpImage<unsigned char> *> mapOfImages,
#ifdef VISP_HAVE_PCL
std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds,
#else
std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds,
std::map<std::string, unsigned int> mapOfWidths, std::map<std::string, unsigned int> mapOfHeights,
#endif
const vpImage<unsigned char> &I_gray, const vpImage<unsigned char> &I_depth,
const vpHomogeneousMatrix &depth_M_color, vpMbGenericTracker &tracker, double projection_error_threshold,
{
vpCameraParameters cam_color, cam_depth;
tracker.getCameraParameters(cam_color, cam_depth);
// Track the object
try {
#ifdef VISP_HAVE_PCL
tracker.track(mapOfImages, mapOfPointclouds);
#else
tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
#endif
}
catch (...) {
return state_detection;
}
tracker.getPose(cMo);
// Detect tracking error
double projection_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
if (projection_error > projection_error_threshold) {
return state_detection;
}
// Display
tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3);
vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3);
vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3);
return state_tracking;
}
int main(int argc, const char **argv)
{
#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_MODULE_MBT)
double opt_tag_size = 0.08;
float opt_quad_decimate = 1.0;
int opt_nthreads = 1;
double opt_cube_size = 0.125; // 12.5cm by default
#ifdef VISP_HAVE_OPENCV
bool opt_use_texture = false;
#endif
bool opt_use_depth = false;
double opt_projection_error_threshold = 40.;
#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
bool display_off = true;
#else
bool display_off = false;
#endif
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
opt_tag_size = atof(argv[i + 1]);
}
else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
opt_quad_decimate = (float)atof(argv[i + 1]);
}
else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
opt_nthreads = atoi(argv[i + 1]);
}
else if (std::string(argv[i]) == "--display_off") {
display_off = true;
}
else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
opt_tag_family = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
}
else if (std::string(argv[i]) == "--cube_size" && i + 1 < argc) {
opt_cube_size = atof(argv[i + 1]);
#ifdef VISP_HAVE_OPENCV
}
else if (std::string(argv[i]) == "--texture") {
opt_use_texture = true;
#endif
}
else if (std::string(argv[i]) == "--depth") {
opt_use_depth = true;
}
else if (std::string(argv[i]) == "--projection_error" && i + 1 < argc) {
opt_projection_error_threshold = atof(argv[i + 1]);
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "Usage: " << argv[0]
<< " [--cube_size <size in m>] [--tag_size <size in m>]"
" [--quad_decimate <decimation>] [--nthreads <nb>]"
" [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
" 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
std::cout << " [--display_off]";
#endif
std::cout << " [--texture] [--depth] [--projection_error <30 - 100>] [--help]" << std::endl;
return EXIT_SUCCESS;
}
}
createCaoFile(opt_cube_size);
try {
vpRealSense2 realsense;
rs2::config config;
int width = 640, height = 480, stream_fps = 30;
config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
config.disable_stream(RS2_STREAM_INFRARED);
realsense.open(config);
vpCameraParameters cam_color, cam_depth;
vpHomogeneousMatrix depth_M_color;
if (opt_use_depth) {
depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
}
vpImage<vpRGBa> I_color(height, width);
vpImage<unsigned char> I_gray(height, width);
vpImage<unsigned char> I_depth(height, width);
vpImage<uint16_t> I_depth_raw(height, width);
std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
#ifdef VISP_HAVE_PCL
std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
#else
std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
std::vector<vpColVector> pointcloud;
#endif
std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
std::cout << "Cube size: " << opt_cube_size << std::endl;
std::cout << "AprilTag size: " << opt_tag_size << std::endl;
std::cout << "AprilTag family: " << opt_tag_family << std::endl;
std::cout << "Camera parameters:" << std::endl;
std::cout << " Color:\n" << cam_color << std::endl;
if (opt_use_depth)
std::cout << " Depth:\n" << cam_depth << std::endl;
std::cout << "Detection: " << std::endl;
std::cout << " Quad decimate: " << opt_quad_decimate << std::endl;
std::cout << " Threads number: " << opt_nthreads << std::endl;
std::cout << "Tracker: " << std::endl;
std::cout << " Use edges : 1" << std::endl;
std::cout << " Use texture: "
#ifdef VISP_HAVE_OPENCV
<< opt_use_texture << std::endl;
#else
<< " na" << std::endl;
#endif
std::cout << " Use depth : " << opt_use_depth << std::endl;
std::cout << " Projection error: " << opt_projection_error_threshold << std::endl;
// Construct display
vpDisplay *d_gray = NULL;
vpDisplay *d_depth = NULL;
if (!display_off) {
#ifdef VISP_HAVE_X11
d_gray = new vpDisplayX(I_gray, 50, 50, "Color stream");
if (opt_use_depth)
d_depth = new vpDisplayX(I_depth, 80 + I_gray.getWidth(), 50, "Depth stream");
#elif defined(VISP_HAVE_GDI)
d_gray = new vpDisplayGDI(I_gray);
if (opt_use_depth)
d_depth = new vpDisplayGDI(I_depth);
#elif defined(HAVE_OPENCV_HIGHGUI)
d_gray = new vpDisplayOpenCV(I_gray);
if (opt_use_depth)
d_depth = new vpDisplayOpenCV(I_depth);
#endif
}
// Initialize AprilTag detector
vpDetectorAprilTag detector(opt_tag_family);
detector.setAprilTagQuadDecimate(opt_quad_decimate);
detector.setAprilTagNbThreads(opt_nthreads);
// Prepare MBT
std::vector<int> trackerTypes;
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
if (opt_use_texture)
else
#endif
trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER);
if (opt_use_depth)
trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
vpMbGenericTracker tracker(trackerTypes);
// edges
vpMe me;
me.setMaskSize(5);
me.setMaskNumber(180);
me.setRange(12);
me.setThreshold(20);
me.setMu1(0.5);
me.setMu2(0.5);
tracker.setMovingEdge(me);
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
if (opt_use_texture) {
vpKltOpencv klt_settings;
klt_settings.setMaxFeatures(300);
klt_settings.setWindowSize(5);
klt_settings.setQuality(0.015);
klt_settings.setMinDistance(8);
klt_settings.setHarrisFreeParameter(0.01);
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
tracker.setKltOpencv(klt_settings);
tracker.setKltMaskBorder(5);
}
#endif
if (opt_use_depth) {
// camera calibration params
tracker.setCameraParameters(cam_color, cam_depth);
// model definition
tracker.loadModel("cube.cao", "cube.cao");
mapOfCameraTransformations["Camera2"] = depth_M_color;
tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
}
else {
// camera calibration params
tracker.setCameraParameters(cam_color);
// model definition
tracker.loadModel("cube.cao");
}
tracker.setDisplayFeatures(true);
state_t state = state_detection;
// wait for a tag detection
while (state != state_quit) {
if (opt_use_depth) {
#ifdef VISP_HAVE_PCL
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, NULL, pointcloud, NULL);
#else
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointcloud, NULL,
NULL);
#endif
vpImageConvert::convert(I_color, I_gray);
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
mapOfImages["Camera1"] = &I_gray;
mapOfImages["Camera2"] = &I_depth;
#ifdef VISP_HAVE_PCL
mapOfPointclouds["Camera2"] = pointcloud;
#else
mapOfPointclouds["Camera2"] = &pointcloud;
mapOfWidths["Camera2"] = width;
mapOfHeights["Camera2"] = height;
#endif
}
else {
realsense.acquire(I_gray);
}
if (state == state_detection) {
state = detectAprilTag(I_gray, detector, opt_tag_size, cam_color, cMo);
// Initialize the tracker with the result of the detection
if (state == state_tracking) {
if (opt_use_depth) {
mapOfCameraPoses["Camera1"] = cMo;
mapOfCameraPoses["Camera2"] = depth_M_color * cMo;
tracker.initFromPose(mapOfImages, mapOfCameraPoses);
}
else {
tracker.initFromPose(I_gray, cMo);
}
}
}
if (state == state_tracking) {
if (opt_use_depth) {
#ifdef VISP_HAVE_PCL
state = track(mapOfImages, mapOfPointclouds, I_gray, I_depth, depth_M_color, tracker,
opt_projection_error_threshold, cMo);
#else
state = track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights, I_gray, I_depth, depth_M_color,
tracker, opt_projection_error_threshold, cMo);
#endif
}
else {
state = track(I_gray, tracker, opt_projection_error_threshold, cMo);
}
{
std::stringstream ss;
ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt()
<< ", depth " << tracker.getNbFeaturesDepthDense();
vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red);
}
}
vpDisplay::displayText(I_gray, 20, 20, "Click to quit...", vpColor::red);
if (vpDisplay::getClick(I_gray, false)) { // exit
state = state_quit;
}
if (opt_use_depth) {
vpDisplay::displayText(I_depth, 20, 20, "Click to quit...", vpColor::red);
if (vpDisplay::getClick(I_depth, false)) { // exit
state = state_quit;
}
vpDisplay::flush(I_depth);
}
}
if (!display_off) {
delete d_gray;
if (opt_use_depth)
delete d_depth;
}
}
catch (const vpException &e) {
std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
}
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_REALSENSE2
std::cout << "ViSP is not build with librealsense2 support" << std::endl;
#endif
std::cout << "Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
#endif
return EXIT_SUCCESS;
}
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
void setAprilTagQuadDecimate(float quadDecimate)
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
void setAprilTagNbThreads(int nThreads)
bool detect(const vpImage< unsigned char > &I)
size_t getNbObjects() const
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
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.
Definition vpException.h:59
const char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition vpKltOpencv.h:73
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition vpMath.h:116
Real-time 6D object pose tracking using its CAD model.
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setDisplayFeatures(bool displayF)
virtual void setKltMaskBorder(const unsigned int &e)
virtual unsigned int getNbFeaturesEdge() const
virtual void setAngleAppear(const double &a)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual unsigned int getNbFeaturesKlt() const
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void setAngleDisappear(const double &a)
virtual void setMovingEdge(const vpMe &me)
virtual unsigned int getNbFeaturesDepthDense() const
virtual void setKltOpencv(const vpKltOpencv &t)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
Definition vpMe.h:122
void setMu1(const double &mu_1)
Definition vpMe.h:353
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition vpMe.h:445
void setMaskSize(const unsigned int &a)
Definition vpMe.cpp:452
void setMu2(const double &mu_2)
Definition vpMe.h:360
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
void setMaskNumber(const unsigned int &a)
Definition vpMe.cpp:445
void setThreshold(const double &t)
Definition vpMe.h:435
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const