1 #include <visp3/core/vpConfig.h> 3 #ifdef VISP_HAVE_MODULE_SENSOR 4 #include <visp3/sensor/vpOccipitalStructure.h> 6 #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_OCCIPITAL_STRUCTURE) 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 Occipital Structure grabber" << std::endl;
85 ST::CaptureSessionSettings settings;
86 settings.source = ST::CaptureSessionSourceId::StructureCore;
87 settings.structureCore.visibleEnabled =
true;
88 settings.applyExpensiveCorrection =
true;
89 unsigned int width = 640, height = 480;
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 g.
acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
103 std::cout <<
"Read camera parameters from Structure core device" << std::endl;
108 std::cout << cam << 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;
123 d1 =
new vpDisplayX(I_color, 100, 30,
"Pose from Homography");
126 #elif defined(VISP_HAVE_GDI) 127 d1 =
new vpDisplayGDI(I_color, 100, 30,
"Pose from Homography");
130 #elif defined(VISP_HAVE_OPENCV) 148 std::vector<double> time_vec;
153 g.
acquire(reinterpret_cast<unsigned char *>(I_color.bitmap), reinterpret_cast<unsigned char *>(I_depth_raw.bitmap));
160 depthMap.
resize(I_depth_raw.getHeight(), I_depth_raw.getWidth());
161 #ifdef VISP_HAVE_OPENMP 162 #pragma omp parallel for 164 for (
unsigned int i = 0; i < I_depth_raw.getHeight(); i++) {
165 for (
unsigned int j = 0; j < I_depth_raw.getWidth(); j++) {
167 float Z = I_depth_raw[i][j] * 0.001;
179 std::vector<vpHomogeneousMatrix> cMo_vec;
180 detector.
detect(I, tagSize, cam, cMo_vec);
183 for (
size_t i = 0; i < cMo_vec.size(); i++) {
188 std::vector<std::vector<vpImagePoint> > tags_corners = detector.
getPolygon();
189 std::vector<int> tags_id = detector.
getTagsId();
190 std::map<int, double> tags_size;
191 tags_size[-1] = tagSize;
192 std::vector<std::vector<vpPoint> > tags_points3d = detector.
getTagsPoints3D(tags_id, tags_size);
193 for (
size_t i = 0; i < tags_corners.size(); i++) {
195 double confidence_index;
197 if (confidence_index > 0.5) {
200 else if (confidence_index > 0.25) {
206 std::stringstream ss;
207 ss <<
"Tag id " << tags_id[i] <<
" confidence: " << confidence_index;
217 time_vec.push_back(t);
219 std::stringstream ss;
220 ss <<
"Detection time: " << t <<
" ms for " << detector.
getNbObjects() <<
" tags";
231 std::cout <<
"Benchmark loop processing time" << std::endl;
232 std::cout <<
"Mean / Median / Std: " <<
vpMath::getMean(time_vec) <<
" ms" 243 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
250 #ifndef VISP_HAVE_APRILTAG 251 std::cout <<
"Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
253 std::cout <<
"Install Structure Core SDK, configure and build ViSP again to use this example" << std::endl;
Class that defines generic functionnalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
static double getMedian(const std::vector< double > &v)
Display for windows using GDI (available on any windows 32 platform).
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static const vpColor none
error that can be emited by ViSP classes.
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
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)
size_t getNbObjects() const
const char * getMessage() const
bool open(const ST::CaptureSessionSettings &settings)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
static const vpColor orange
void setAprilTagQuadDecimate(float quadDecimate)
static double getMean(const std::vector< double > &v)
void acquire(vpImage< unsigned char > &gray, bool undistorted=false, double *ts=NULL)
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setAprilTagNbThreads(int nThreads)
static bool isNaN(double value)
std::vector< int > getTagsId() const
unsigned int getHeight() const
vpCameraParameters getCameraParameters(const vpOccipitalStructureStream stream_type, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithoutDistortion)
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))
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
static vpColor getColor(const unsigned int &i)
unsigned int getWidth() const
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)