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>
16 int main(
int argc,
const char **argv)
19 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2)
24 double tagSize = 0.053;
25 float quad_decimate = 1.0;
27 bool display_tag =
false;
29 unsigned int thickness = 2;
30 bool align_frame =
false;
32 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
33 bool display_off =
true;
34 std::cout <<
"Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
36 bool display_off =
false;
39 for (
int i = 1; i < argc; i++) {
40 if (std::string(argv[i]) ==
"--pose_method" && i + 1 < argc) {
42 }
else if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
43 tagSize = atof(argv[i + 1]);
44 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
45 quad_decimate = (float)atof(argv[i + 1]);
46 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
47 nThreads = atoi(argv[i + 1]);
48 }
else if (std::string(argv[i]) ==
"--display_tag") {
50 }
else if (std::string(argv[i]) ==
"--display_off") {
52 }
else if (std::string(argv[i]) ==
"--color" && i + 1 < argc) {
53 color_id = atoi(argv[i+1]);
54 }
else if (std::string(argv[i]) ==
"--thickness" && i + 1 < argc) {
55 thickness = (
unsigned int) atoi(argv[i+1]);
56 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
58 }
else if (std::string(argv[i]) ==
"--z_aligned") {
61 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
62 std::cout <<
"Usage: " << argv[0]
63 <<
" [--tag_size <tag_size in m> (default: 0.053)]"
64 " [--quad_decimate <quad_decimate> (default: 1)]"
65 " [--nthreads <nb> (default: 1)]"
66 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
67 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
68 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
69 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
70 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
71 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
72 " [--display_tag] [--z_aligned]";
73 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
74 std::cout <<
" [--display_off] [--color <color id>] [--thickness <line thickness>]";
76 std::cout <<
" [--help]" << std::endl;
83 std::cout <<
"Use Realsense 2 grabber" << std::endl;
86 unsigned int width = 640, height = 480;
87 config.enable_stream(RS2_STREAM_COLOR,
static_cast<int>(width),
static_cast<int>(height), RS2_FORMAT_RGBA8, 30);
88 config.enable_stream(RS2_STREAM_DEPTH,
static_cast<int>(width),
static_cast<int>(height), RS2_FORMAT_Z16, 30);
89 config.enable_stream(RS2_STREAM_INFRARED,
static_cast<int>(width),
static_cast<int>(height), RS2_FORMAT_Y8, 30);
98 std::cout <<
"I_color: " << I_color.getWidth() <<
" " << I_color.getHeight() << std::endl;
99 std::cout <<
"I_depth_raw: " << I_depth_raw.getWidth() <<
" " << I_depth_raw.getHeight() << std::endl;
101 rs2::align align_to_color = RS2_STREAM_COLOR;
102 g.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap),
103 NULL, NULL, &align_to_color);
105 std::cout <<
"Read camera parameters from Realsense device" << std::endl;
110 std::cout << cam << std::endl;
111 std::cout <<
"poseEstimationMethod: " << poseEstimationMethod << std::endl;
112 std::cout <<
"tagFamily: " << tagFamily << std::endl;
113 std::cout <<
"nThreads : " << nThreads << std::endl;
114 std::cout <<
"Z aligned: " << align_frame << std::endl;
125 d1 =
new vpDisplayX(I_color, 100, 30,
"Pose from Homography");
126 d2 =
new vpDisplayX(I_color2, I_color.getWidth()+120, 30,
"Pose from RGBD fusion");
127 d3 =
new vpDisplayX(I_depth, 100, I_color.getHeight()+70,
"Depth");
128 #elif defined(VISP_HAVE_GDI)
129 d1 =
new vpDisplayGDI(I_color, 100, 30,
"Pose from Homography");
130 d2 =
new vpDisplayGDI(I_color2, I_color.getWidth()+120, 30,
"Pose from RGBD fusion");
131 d3 =
new vpDisplayGDI(I_depth, 100, I_color.getHeight()+70,
"Depth");
132 #elif defined(VISP_HAVE_OPENCV)
134 d2 =
new vpDisplayOpenCV(I_color2, I_color.getWidth()+120, 30,
"Pose from RGBD fusion");
135 d3 =
new vpDisplayOpenCV(I_depth, 100, I_color.getHeight()+70,
"Depth");
144 detector.setAprilTagQuadDecimate(quad_decimate);
145 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
146 detector.setAprilTagNbThreads(nThreads);
148 detector.setZAlignedWithCameraAxis(align_frame);
150 std::vector<double> time_vec;
155 g.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap),
156 NULL, NULL, &align_to_color);
163 depthMap.
resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
164 #ifdef VISP_HAVE_OPENMP
165 #pragma omp parallel for
167 for (
int i = 0; i < static_cast<int>(I_depth_raw.getHeight()); i++) {
168 for (
int j = 0; j < static_cast<int>(I_depth_raw.getWidth()); j++) {
169 if (I_depth_raw[i][j]) {
170 float Z = I_depth_raw[i][j] * depth_scale;
182 std::vector<vpHomogeneousMatrix> cMo_vec;
183 detector.detect(I, tagSize, cam, cMo_vec);
186 for (
size_t i = 0; i < cMo_vec.size(); i++) {
191 std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
192 std::vector<int> tags_id = detector.getTagsId();
193 std::map<int, double> tags_size;
194 tags_size[-1] = tagSize;
195 std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
196 for (
size_t i = 0; i < tags_corners.size(); i++) {
198 double confidence_index;
200 if (confidence_index > 0.5) {
203 else if (confidence_index > 0.25) {
209 std::stringstream ss;
210 ss <<
"Tag id " << tags_id[i] <<
" confidence: " << confidence_index;
220 time_vec.push_back(t);
222 std::stringstream ss;
223 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
234 std::cout <<
"Benchmark loop processing time" << std::endl;
235 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms"
246 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
253 #ifndef VISP_HAVE_APRILTAG
254 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
256 std::cout <<
"Install librealsense 3rd party, configure and build ViSP again to use this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
static vpColor getColor(const unsigned int &i)
static const vpColor none
static const vpColor orange
@ 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 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 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
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)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
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)
static bool computePlanarObjectPoseFromRGBD(const vpImage< float > &depthMap, const std::vector< vpImagePoint > &corners, const vpCameraParameters &colorIntrinsics, const std::vector< vpPoint > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index=NULL)
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()