2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpRealSense2.h>
7#include <visp3/detection/vpDetectorAprilTag.h>
9#include <visp3/core/vpImageConvert.h>
10#include <visp3/core/vpImageTools.h>
11#include <visp3/core/vpMeterPixelConversion.h>
12#include <visp3/core/vpPixelMeterConversion.h>
13#include <visp3/core/vpXmlParserCamera.h>
14#include <visp3/gui/vpDisplayGDI.h>
15#include <visp3/gui/vpDisplayOpenCV.h>
16#include <visp3/gui/vpDisplayX.h>
17#include <visp3/vision/vpPose.h>
19int main(
int argc,
const char **argv)
23#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
28 double tagSize = 0.053;
29 float quad_decimate = 1.0;
31 bool display_tag =
false;
33 unsigned int thickness = 2;
34 bool align_frame =
false;
36#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
37 bool display_off =
true;
38 std::cout <<
"Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
40 bool display_off =
false;
43 for (
int i = 1; i < argc; i++) {
44 if (std::string(argv[i]) ==
"--pose_method" && i + 1 < argc) {
46 }
else if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
47 tagSize = atof(argv[i + 1]);
48 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
49 quad_decimate = (float)atof(argv[i + 1]);
50 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
51 nThreads = atoi(argv[i + 1]);
52 }
else if (std::string(argv[i]) ==
"--display_tag") {
54 }
else if (std::string(argv[i]) ==
"--display_off") {
56 }
else if (std::string(argv[i]) ==
"--color" && i + 1 < argc) {
57 color_id = atoi(argv[i + 1]);
58 }
else if (std::string(argv[i]) ==
"--thickness" && i + 1 < argc) {
59 thickness = (
unsigned int)atoi(argv[i + 1]);
60 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
62 }
else if (std::string(argv[i]) ==
"--z_aligned") {
64 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
65 std::cout <<
"Usage: " << argv[0]
66 <<
" [--tag_size <tag_size in m> (default: 0.053)]"
67 " [--quad_decimate <quad_decimate> (default: 1)]"
68 " [--nthreads <nb> (default: 1)]"
69 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
70 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
71 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
72 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
73 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
74 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
75 " [--display_tag] [--z_aligned]";
76#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
77 std::cout <<
" [--display_off] [--color <color id>] [--thickness <line thickness>]";
79 std::cout <<
" [--help]" << std::endl;
86 std::cout <<
"Use Realsense 2 grabber" << std::endl;
89 unsigned int width = 848, height = 800;
90 config.disable_stream(RS2_STREAM_FISHEYE, 1);
91 config.disable_stream(RS2_STREAM_FISHEYE, 2);
92 config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
93 config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
101 std::cout <<
"Read camera parameters from Realsense device" << std::endl;
108 std::cout << cam_left << std::endl;
109 std::cout <<
"poseEstimationMethod: " << poseEstimationMethod << std::endl;
110 std::cout <<
"tagFamily: " << tagFamily << std::endl;
111 std::cout <<
"nThreads : " << nThreads << std::endl;
112 std::cout <<
"Z aligned: " << align_frame << std::endl;
118 display_left =
new vpDisplayX(I_left, 100, 30,
"Left image");
119 display_undistort =
new vpDisplayX(I_undist, I_left.getWidth(), 30,
"Undistorted image");
120#elif defined(VISP_HAVE_GDI)
121 display_left =
new vpDisplayGDI(I_left, 100, 30,
"Left image");
122#elif defined(HAVE_OPENCV_HIGHGUI)
138 detector.setAprilTagQuadDecimate(quad_decimate);
139 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
140 detector.setAprilTagNbThreads(nThreads);
142 detector.setZAlignedWithCameraAxis(align_frame);
145 std::vector<double> time_vec;
147 std::vector<std::vector<vpImagePoint> > tag_corners;
153 g.
acquire(&I_left, NULL, NULL);
165 std::vector<vpHomogeneousMatrix> cMo_vec, cMo_vec1;
166 detector.detect(I_undist, tagSize, cam_undistort, cMo_vec);
170 for (
size_t i = 0; i < cMo_vec.size(); i++) {
171 tag_corners = detector.getTagsCorners();
172 for (
size_t j = 0; j < 4; j++) {
181 time_vec.push_back(t);
183 std::stringstream ss;
184 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
195 std::cout <<
"Benchmark loop processing time" << std::endl;
196 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
202 delete display_undistort;
206 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
213#ifndef VISP_HAVE_APRILTAG
214 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
215#elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
216 std::cout <<
"Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API "
217 << RS2_API_VERSION_STR <<
". You need to upgrade librealsense to use this example." << std::endl;
219 std::cout <<
"Install librealsense 3rd party, configure and build ViSP again to use this example." << std::endl;
Implementation of a generic 2D array used as base class for matrices and vectors.
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)
static const vpColor none
static const vpColor yellow
static const vpColor green
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
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...
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 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.
const char * getMessage() const
Definition of the vpImage class member functions.
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static double getMean(const std::vector< double > &v)
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()