Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-apriltag-detector-live-T265-realsense.cpp
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vpRealSense2.h>
#endif
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/vision/vpPose.h>
int main(int argc, const char **argv)
{
// Realsense T265 is only supported if realsense API > 2.31.0
#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
double tagSize = 0.053;
float quad_decimate = 1.0;
int nThreads = 1;
bool display_tag = false;
int color_id = -1;
unsigned int thickness = 2;
bool align_frame = false;
#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
bool display_off = true;
std::cout << "Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
#else
bool display_off = false;
#endif
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
tagSize = atof(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 = atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--display_tag") {
display_tag = true;
} else if (std::string(argv[i]) == "--display_off") {
display_off = true;
} else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
color_id = atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
thickness = (unsigned int)atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--z_aligned") {
align_frame = true;
} else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "Usage: " << argv[0]
<< " [--tag_size <tag_size in m> (default: 0.053)]"
" [--quad_decimate <quad_decimate> (default: 1)]"
" [--nthreads <nb> (default: 1)]"
" [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
" 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
" 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
" [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
" 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
" 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
" [--display_tag] [--z_aligned]";
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
#endif
std::cout << " [--help]" << std::endl;
return EXIT_SUCCESS;
}
}
try {
std::cout << "Use Realsense 2 grabber" << std::endl;
rs2::config config;
unsigned int width = 848, height = 800;
config.disable_stream(RS2_STREAM_FISHEYE, 1);
config.disable_stream(RS2_STREAM_FISHEYE, 2);
config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
vpImage<unsigned char> I_left(height, width);
vpImage<unsigned char> I_undist(height, width);
g.open(config);
g.acquire(&I_left, NULL, NULL);
std::cout << "Read camera parameters from Realsense device" << std::endl;
vpCameraParameters cam_left, cam_undistort;
cam_undistort.initPersProjWithoutDistortion(cam_left.get_px(), cam_left.get_py(), cam_left.get_u0(),
cam_left.get_v0());
std::cout << cam_left << std::endl;
std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
std::cout << "tagFamily: " << tagFamily << std::endl;
std::cout << "nThreads : " << nThreads << std::endl;
std::cout << "Z aligned: " << align_frame << std::endl;
vpDisplay *display_left = NULL;
vpDisplay *display_undistort = NULL;
if (!display_off) {
#ifdef VISP_HAVE_X11
display_left = new vpDisplayX(I_left, 100, 30, "Left image");
display_undistort = new vpDisplayX(I_undist, I_left.getWidth(), 30, "Undistorted image");
#elif defined(VISP_HAVE_GDI)
display_left = new vpDisplayGDI(I_left, 100, 30, "Left image");
#elif defined(HAVE_OPENCV_HIGHGUI)
display_left = new vpDisplayOpenCV(I_left, 100, 30, "Left image");
#endif
}
vpArray2D<int> mapU, mapV;
vpArray2D<float> mapDu, mapDv;
vpImageTools::initUndistortMap(cam_left, I_left.getWidth(), I_left.getHeight(), mapU, mapV, mapDu, mapDv);
vpDetectorAprilTag detector(tagFamily);
detector.setAprilTagQuadDecimate(quad_decimate);
detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
detector.setAprilTagNbThreads(nThreads);
detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
detector.setZAlignedWithCameraAxis(align_frame);
std::vector<double> time_vec;
std::vector<std::vector<vpImagePoint> > tag_corners;
for (;;) {
double t = vpTime::measureTimeMs();
g.acquire(&I_left, NULL, NULL);
vpImageTools::undistort(I_left, mapU, mapV, mapDu, mapDv, I_undist);
vpDisplay::display(I_undist);
std::vector<vpHomogeneousMatrix> cMo_vec, cMo_vec1;
detector.detect(I_undist, tagSize, cam_undistort, cMo_vec);
// Display tag corners, bounding box and pose
for (size_t i = 0; i < cMo_vec.size(); i++) {
tag_corners = detector.getTagsCorners();
for (size_t j = 0; j < 4; j++) {
vpDisplay::displayCross(I_undist, tag_corners[i][j], 20, vpColor::green, 2);
}
vpDisplay::displayRectangle(I_undist, detector.getBBox(i), vpColor::yellow, false, 3);
vpDisplay::displayFrame(I_undist, cMo_vec[i], cam_undistort, tagSize / 2, vpColor::red, 3);
}
time_vec.push_back(t);
std::stringstream ss;
ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
vpDisplay::displayText(I_left, 50, 20, ss.str(), vpColor::red);
vpDisplay::displayText(I_undist, 50, 20, ss.str(), vpColor::red);
if (vpDisplay::getClick(I_left, false) || vpDisplay::getClick(I_undist, false))
break;
vpDisplay::flush(I_undist);
}
std::cout << "Benchmark loop processing time" << std::endl;
std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
<< " ; " << vpMath::getMedian(time_vec) << " ms"
<< " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
if (!display_off) {
delete display_left;
delete display_undistort;
}
} 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 << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
#elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
std::cout << "Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API "
<< RS2_API_VERSION_STR << ". You need to upgrade librealsense to use this example." << std::endl;
#else
std::cout << "Install librealsense 3rd party, configure and build ViSP again to use this example." << std::endl;
#endif
#endif
return EXIT_SUCCESS;
}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
static vpColor getColor(const unsigned int &i)
Definition vpColor.h:307
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
void setAprilTagQuadDecimate(float quadDecimate)
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
void setAprilTagNbThreads(int nThreads)
bool detect(const vpImage< unsigned char > &I)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
vpRect getBBox(size_t i) const
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 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 displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
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
static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height, vpArray2D< int > &mapU, vpArray2D< int > &mapV, vpArray2D< float > &mapDu, vpArray2D< float > &mapDv)
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
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())
VISP_EXPORT double measureTimeMs()