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/gui/vpDisplayGDI.h>
10 #include <visp3/gui/vpDisplayOpenCV.h>
11 #include <visp3/gui/vpDisplayX.h>
12 #include <visp3/core/vpXmlParserCamera.h>
13 #include <visp3/core/vpImageConvert.h>
14 #include <visp3/vision/vpPose.h>
15 #include <visp3/core/vpImageTools.h>
16 #include <visp3/core/vpMeterPixelConversion.h>
17 #include <visp3/core/vpPixelMeterConversion.h>
19 int 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") {
65 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
66 std::cout <<
"Usage: " << argv[0]
67 <<
" [--tag_size <tag_size in m> (default: 0.053)]"
68 " [--quad_decimate <quad_decimate> (default: 1)]"
69 " [--nthreads <nb> (default: 1)]"
70 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
71 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
72 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
73 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
74 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
75 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
76 " [--display_tag] [--z_aligned]";
77 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
78 std::cout <<
" [--display_off] [--color <color id>] [--thickness <line thickness>]";
80 std::cout <<
" [--help]" << std::endl;
87 std::cout <<
"Use Realsense 2 grabber" << std::endl;
90 unsigned int width = 848, height = 800;
91 config.disable_stream(RS2_STREAM_FISHEYE,1);
92 config.disable_stream(RS2_STREAM_FISHEYE,2);
93 config.enable_stream(RS2_STREAM_FISHEYE,1,RS2_FORMAT_Y8);
94 config.enable_stream(RS2_STREAM_FISHEYE,2,RS2_FORMAT_Y8);
100 g.
acquire(&I_left, NULL, NULL);
102 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(VISP_HAVE_OPENCV)
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++)
182 time_vec.push_back(t);
184 std::stringstream ss;
185 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
196 std::cout <<
"Benchmark loop processing time" << std::endl;
197 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
203 delete display_undistort;
207 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
214 #ifndef VISP_HAVE_APRILTAG
215 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
216 #elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
217 std::cout <<
"Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API "
218 << RS2_API_VERSION_STR <<
". You need to upgrade librealsense to use this example." << std::endl;
220 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
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 functionnalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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 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))
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 emited 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)
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()