6#include <visp3/core/vpXmlParserCamera.h>
7#include <visp3/detection/vpDetectorAprilTag.h>
8#include <visp3/gui/vpDisplayGDI.h>
9#include <visp3/gui/vpDisplayOpenCV.h>
10#include <visp3/gui/vpDisplayX.h>
11#include <visp3/mbt/vpMbGenericTracker.h>
12#include <visp3/sensor/vpV4l2Grabber.h>
14#if defined(HAVE_OPENCV_VIDEOIO)
15#include <opencv2/videoio.hpp>
18typedef enum { state_detection, state_tracking, state_quit } state_t;
22void createCaoFile(
double cubeEdgeSize)
24 std::ofstream fileStream;
25 fileStream.open(
"cube.cao", std::ofstream::out | std::ofstream::trunc);
27 fileStream <<
"# 3D Points\n";
28 fileStream <<
"8 # Number of points\n";
29 fileStream << cubeEdgeSize / 2 <<
" " << cubeEdgeSize / 2 <<
" " << 0 <<
" # Point 0: (X, Y, Z)\n";
30 fileStream << cubeEdgeSize / 2 <<
" " << -cubeEdgeSize / 2 <<
" " << 0 <<
" # Point 1\n";
31 fileStream << -cubeEdgeSize / 2 <<
" " << -cubeEdgeSize / 2 <<
" " << 0 <<
" # Point 2\n";
32 fileStream << -cubeEdgeSize / 2 <<
" " << cubeEdgeSize / 2 <<
" " << 0 <<
" # Point 3\n";
33 fileStream << -cubeEdgeSize / 2 <<
" " << cubeEdgeSize / 2 <<
" " << -cubeEdgeSize <<
" # Point 4\n";
34 fileStream << -cubeEdgeSize / 2 <<
" " << -cubeEdgeSize / 2 <<
" " << -cubeEdgeSize <<
" # Point 5\n";
35 fileStream << cubeEdgeSize / 2 <<
" " << -cubeEdgeSize / 2 <<
" " << -cubeEdgeSize <<
" # Point 6\n";
36 fileStream << cubeEdgeSize / 2 <<
" " << cubeEdgeSize / 2 <<
" " << -cubeEdgeSize <<
" # Point 7\n";
37 fileStream <<
"# 3D Lines\n";
38 fileStream <<
"0 # Number of lines\n";
39 fileStream <<
"# Faces from 3D lines\n";
40 fileStream <<
"0 # Number of faces\n";
41 fileStream <<
"# Faces from 3D points\n";
42 fileStream <<
"6 # Number of faces\n";
43 fileStream <<
"4 0 3 2 1 # Face 0: [number of points] [index of the 3D points]...\n";
44 fileStream <<
"4 1 2 5 6\n";
45 fileStream <<
"4 4 7 6 5\n";
46 fileStream <<
"4 0 7 4 3\n";
47 fileStream <<
"4 5 2 3 4\n";
48 fileStream <<
"4 0 1 6 7 # Face 5\n";
49 fileStream <<
"# 3D cylinders\n";
50 fileStream <<
"0 # Number of cylinders\n";
51 fileStream <<
"# 3D circles\n";
52 fileStream <<
"0 # Number of circles\n";
56#if defined(VISP_HAVE_APRILTAG)
60 std::vector<vpHomogeneousMatrix> cMo_vec;
63 bool ret = detector.
detect(I, tagSize, cam, cMo_vec);
66 for (
size_t i = 0; i < cMo_vec.size(); i++) {
74 return state_tracking;
77 return state_detection;
92 return state_detection;
99 if (projection_error > projection_error_threshold) {
100 return state_detection;
108 std::stringstream ss;
113 return state_tracking;
116int main(
int argc,
const char **argv)
119#if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_V4L2) || defined(HAVE_OPENCV_VIDEOIO)) && defined(VISP_HAVE_MODULE_MBT)
124 double opt_tag_size = 0.08;
125 float opt_quad_decimate = 1.0;
126 int opt_nthreads = 1;
127 std::string opt_intrinsic_file =
"";
128 std::string opt_camera_name =
"";
129 double opt_cube_size = 0.125;
130#ifdef VISP_HAVE_OPENCV
131 bool opt_use_texture =
false;
133 double opt_projection_error_threshold = 40.;
135#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
136 bool display_off =
true;
138 bool display_off =
false;
141 for (
int i = 1; i < argc; i++) {
142 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
143 opt_tag_size = atof(argv[i + 1]);
145 else if (std::string(argv[i]) ==
"--input" && i + 1 < argc) {
146 opt_device = atoi(argv[i + 1]);
148 else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
149 opt_quad_decimate = (float)atof(argv[i + 1]);
151 else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
152 opt_nthreads = atoi(argv[i + 1]);
154 else if (std::string(argv[i]) ==
"--intrinsic" && i + 1 < argc) {
155 opt_intrinsic_file = std::string(argv[i + 1]);
157 else if (std::string(argv[i]) ==
"--camera_name" && i + 1 < argc) {
158 opt_camera_name = std::string(argv[i + 1]);
160 else if (std::string(argv[i]) ==
"--display_off") {
163 else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
166 else if (std::string(argv[i]) ==
"--cube_size" && i + 1 < argc) {
167 opt_cube_size = atof(argv[i + 1]);
168#ifdef VISP_HAVE_OPENCV
170 else if (std::string(argv[i]) ==
"--texture") {
171 opt_use_texture =
true;
174 else if (std::string(argv[i]) ==
"--projection_error" && i + 1 < argc) {
175 opt_projection_error_threshold = atof(argv[i + 1]);
177 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
178 std::cout <<
"Usage: " << argv[0]
179 <<
" [--input <camera id>] [--cube_size <size in m>] [--tag_size <size in m>]"
180 " [--quad_decimate <decimation>] [--nthreads <nb>]"
181 " [--intrinsic <xml intrinsic file>] [--camera_name <camera name in xml file>]"
182 " [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
183 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
184#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
185 std::cout <<
" [--display_off]";
187 std::cout <<
" [--texture] [--projection_error <30 - 100>] [--help]" << std::endl;
192 createCaoFile(opt_cube_size);
195 bool camIsInit =
false;
197 if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
206#if defined(VISP_HAVE_V4L2)
208 std::ostringstream device;
209 device <<
"/dev/video" << opt_device;
210 std::cout <<
"Use device " << device.str() <<
" (v4l2 grabber)" << std::endl;
214#elif defined(HAVE_OPENCV_VIDEOIO)
215 std::cout <<
"Use device " << opt_device <<
" (OpenCV grabber)" << std::endl;
216 cv::VideoCapture cap(opt_device);
217 if (!cap.isOpened()) {
218 std::cout <<
"Failed to open the camera" << std::endl;
229 std::cout <<
"Cube size: " << opt_cube_size << std::endl;
230 std::cout <<
"AprilTag size: " << opt_tag_size << std::endl;
231 std::cout <<
"AprilTag family: " << opt_tag_family << std::endl;
232 std::cout <<
"Camera parameters:\n" << cam << std::endl;
233 std::cout <<
"Detection: " << std::endl;
234 std::cout <<
" Quad decimate: " << opt_quad_decimate << std::endl;
235 std::cout <<
" Threads number: " << opt_nthreads << std::endl;
236 std::cout <<
"Tracker: " << std::endl;
237 std::cout <<
" Use edges : 1" << std::endl;
238 std::cout <<
" Use texture: "
239#ifdef VISP_HAVE_OPENCV
240 << opt_use_texture << std::endl;
242 <<
" na" << std::endl;
244 std::cout <<
" Projection error: " << opt_projection_error_threshold << std::endl;
251#elif defined(VISP_HAVE_GDI)
253#elif defined(HAVE_OPENCV_HIGHGUI)
265#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
283#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
284 if (opt_use_texture) {
307 state_t state = state_detection;
310 while (state != state_quit) {
312#if defined(VISP_HAVE_V4L2)
314#elif defined(HAVE_OPENCV_VIDEOIO)
321 if (state == state_detection) {
322 state = detectAprilTag(I, detector, opt_tag_size, cam, cMo);
325 if (state == state_tracking) {
332 if (state == state_tracking) {
333 state = track(I, tracker, opt_projection_error_threshold, cMo);
348 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
355#ifndef VISP_HAVE_APRILTAG
356 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
358#if !(defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_OPENCV))
359 std::cout <<
"ViSP is not build with v4l2 or OpenCV support" << std::endl;
361 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor none
void setAprilTagQuadDecimate(float quadDecimate)
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
void setAprilTagNbThreads(int nThreads)
bool detect(const vpImage< unsigned char > &I)
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...
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 flush(const vpImage< unsigned char > &I)
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
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Real-time 6D object pose tracking using its CAD model.
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setDisplayFeatures(bool displayF)
virtual void setKltMaskBorder(const unsigned int &e)
virtual unsigned int getNbFeaturesEdge() const
virtual void setAngleAppear(const double &a)
virtual void initFromPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual unsigned int getNbFeaturesKlt() const
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual void setAngleDisappear(const double &a)
virtual void setMovingEdge(const vpMe &me)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void setTrackerType(int type)
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void track(const vpImage< unsigned char > &I)
void setMu1(const double &mu_1)
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskSize(const unsigned int &a)
void setMu2(const double &mu_2)
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
void setMaskNumber(const unsigned int &a)
void setThreshold(const double &t)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)