1 #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/gui/vpDisplayGDI.h>
14 #include <visp3/gui/vpDisplayOpenCV.h>
15 #include <visp3/gui/vpDisplayX.h>
16 #include <visp3/vision/vpPose.h>
18 int main(
int argc,
const char **argv)
22 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
27 double tagSize = 0.053;
28 float quad_decimate = 1.0;
30 bool display_tag =
false;
32 unsigned int thickness = 2;
33 bool align_frame =
false;
35 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
36 bool display_off =
true;
37 std::cout <<
"Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
39 bool display_off =
false;
42 for (
int i = 1; i < argc; i++) {
43 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]);
49 else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
50 quad_decimate = (float)atof(argv[i + 1]);
52 else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
53 nThreads = atoi(argv[i + 1]);
55 else if (std::string(argv[i]) ==
"--display_tag") {
58 else if (std::string(argv[i]) ==
"--display_off") {
61 else if (std::string(argv[i]) ==
"--color" && i + 1 < argc) {
62 color_id = atoi(argv[i + 1]);
64 else if (std::string(argv[i]) ==
"--thickness" && i + 1 < argc) {
65 thickness = (
unsigned int)atoi(argv[i + 1]);
67 else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
70 else if (std::string(argv[i]) ==
"--z_aligned") {
73 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
74 std::cout <<
"Usage: " << argv[0]
75 <<
" [--tag_size <tag_size in m> (default: 0.053)]"
76 " [--quad_decimate <quad_decimate> (default: 1)]"
77 " [--nthreads <nb> (default: 1)]"
78 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
79 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
80 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
81 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
82 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
83 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
84 " [--display_tag] [--z_aligned]";
85 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
86 std::cout <<
" [--display_off] [--color <color id>] [--thickness <line thickness>]";
88 std::cout <<
" [--help]" << std::endl;
95 std::cout <<
"Use Realsense 2 grabber" << std::endl;
98 unsigned int width = 848, height = 800;
99 config.disable_stream(RS2_STREAM_FISHEYE, 1);
100 config.disable_stream(RS2_STREAM_FISHEYE, 2);
101 config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
102 config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
108 g.
acquire(&I_left,
nullptr,
nullptr);
110 std::cout <<
"Read camera parameters from Realsense device" << std::endl;
117 std::cout << cam_left << std::endl;
118 std::cout <<
"poseEstimationMethod: " << poseEstimationMethod << std::endl;
119 std::cout <<
"tagFamily: " << tagFamily << std::endl;
120 std::cout <<
"nThreads : " << nThreads << std::endl;
121 std::cout <<
"Z aligned: " << align_frame << std::endl;
127 display_left =
new vpDisplayX(I_left, 100, 30,
"Left image");
128 display_undistort =
new vpDisplayX(I_undist, I_left.getWidth(), 30,
"Undistorted image");
129 #elif defined(VISP_HAVE_GDI)
130 display_left =
new vpDisplayGDI(I_left, 100, 30,
"Left image");
131 #elif defined(HAVE_OPENCV_HIGHGUI)
147 detector.setAprilTagQuadDecimate(quad_decimate);
148 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
149 detector.setAprilTagNbThreads(nThreads);
151 detector.setZAlignedWithCameraAxis(align_frame);
154 std::vector<double> time_vec;
156 std::vector<std::vector<vpImagePoint> > tag_corners;
162 g.
acquire(&I_left,
nullptr,
nullptr);
174 std::vector<vpHomogeneousMatrix> cMo_vec, cMo_vec1;
175 detector.detect(I_undist, tagSize, cam_undistort, cMo_vec);
179 for (
size_t i = 0; i < cMo_vec.size(); i++) {
180 tag_corners = detector.getTagsCorners();
181 for (
size_t j = 0; j < 4; j++) {
190 time_vec.push_back(t);
192 std::stringstream ss;
193 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
204 std::cout <<
"Benchmark loop processing time" << std::endl;
205 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
211 delete display_undistort;
216 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
223 #ifndef VISP_HAVE_APRILTAG
224 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
225 #elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
226 std::cout <<
"Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API "
227 << RS2_API_VERSION_STR <<
". You need to upgrade librealsense to use this example." << std::endl;
229 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
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)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()