6 #include <visp3/gui/vpDisplayGDI.h>
7 #include <visp3/gui/vpDisplayOpenCV.h>
8 #include <visp3/gui/vpDisplayX.h>
9 #include <visp3/core/vpXmlParserCamera.h>
10 #include <visp3/sensor/vpRealSense2.h>
11 #include <visp3/detection/vpDetectorAprilTag.h>
12 #include <visp3/mbt/vpMbGenericTracker.h>
22 void 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;
79 #endif // #if defined(VISP_HAVE_APRILTAG)
92 return state_detection;
99 if (projection_error > projection_error_threshold) {
100 return state_detection;
108 return state_tracking;
113 std::map<std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr>mapOfPointclouds,
115 std::map<std::string,
const std::vector<vpColVector> *>mapOfPointclouds,
116 std::map<std::string, unsigned int> mapOfWidths,
117 std::map<std::string, unsigned int> mapOfHeights,
131 tracker.
track(mapOfImages, mapOfPointclouds);
133 tracker.
track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
137 return state_detection;
144 if (projection_error > projection_error_threshold) {
145 return state_detection;
149 tracker.
display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
153 return state_tracking;
156 int main(
int argc,
const char **argv)
159 #if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_MODULE_MBT)
163 double opt_tag_size = 0.08;
164 float opt_quad_decimate = 1.0;
165 int opt_nthreads = 1;
166 double opt_cube_size = 0.125;
167 #ifdef VISP_HAVE_OPENCV
168 bool opt_use_texture =
false;
170 bool opt_use_depth =
false;
171 double opt_projection_error_threshold = 40.;
173 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
174 bool display_off =
true;
176 bool display_off =
false;
179 for (
int i = 1; i < argc; i++) {
180 if (std::string(argv[i]) ==
"--tag_size" && i + 1 < argc) {
181 opt_tag_size = atof(argv[i + 1]);
182 }
else if (std::string(argv[i]) ==
"--quad_decimate" && i + 1 < argc) {
183 opt_quad_decimate = (float)atof(argv[i + 1]);
184 }
else if (std::string(argv[i]) ==
"--nthreads" && i + 1 < argc) {
185 opt_nthreads = atoi(argv[i + 1]);
186 }
else if (std::string(argv[i]) ==
"--display_off") {
188 }
else if (std::string(argv[i]) ==
"--tag_family" && i + 1 < argc) {
190 }
else if (std::string(argv[i]) ==
"--cube_size" && i + 1 < argc) {
191 opt_cube_size = atof(argv[i + 1]);
192 #ifdef VISP_HAVE_OPENCV
193 }
else if (std::string(argv[i]) ==
"--texture") {
194 opt_use_texture =
true;
196 }
else if (std::string(argv[i]) ==
"--depth") {
197 opt_use_depth =
true;
198 }
else if (std::string(argv[i]) ==
"--projection_error" && i + 1 < argc) {
199 opt_projection_error_threshold = atof(argv[i + 1]);
200 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
201 std::cout <<
"Usage: " << argv[0] <<
" [--cube_size <size in m>] [--tag_size <size in m>]"
202 " [--quad_decimate <decimation>] [--nthreads <nb>]"
203 " [--tag_family <0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT, "
204 " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5>]";
205 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
206 std::cout <<
" [--display_off]";
208 std::cout <<
" [--texture] [--depth] [--projection_error <30 - 100>] [--help]" << std::endl;
213 createCaoFile(opt_cube_size);
219 int width = 640, height = 480, stream_fps = 30;
220 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
221 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
222 config.disable_stream(RS2_STREAM_INFRARED);
223 realsense.
open(config);
237 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
238 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
240 std::map<std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr> mapOfPointclouds;
241 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
243 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
244 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
245 std::vector<vpColVector> pointcloud;
248 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
250 std::cout <<
"Cube size: " << opt_cube_size << std::endl;
251 std::cout <<
"AprilTag size: " << opt_tag_size << std::endl;
252 std::cout <<
"AprilTag family: " << opt_tag_family << std::endl;
253 std::cout <<
"Camera parameters:" << std::endl;
254 std::cout <<
" Color:\n" << cam_color << std::endl;
256 std::cout <<
" Depth:\n" << cam_depth << std::endl;
257 std::cout <<
"Detection: " << std::endl;
258 std::cout <<
" Quad decimate: " << opt_quad_decimate << std::endl;
259 std::cout <<
" Threads number: " << opt_nthreads << std::endl;
260 std::cout <<
"Tracker: " << std::endl;
261 std::cout <<
" Use edges : 1" << std::endl;
262 std::cout <<
" Use texture: "
263 #ifdef VISP_HAVE_OPENCV
264 << opt_use_texture << std::endl;
266 <<
" na" << std::endl;
268 std::cout <<
" Use depth : " << opt_use_depth << std::endl;
269 std::cout <<
" Projection error: " << opt_projection_error_threshold << std::endl;
277 d_gray =
new vpDisplayX(I_gray, 50, 50,
"Color stream");
279 d_depth =
new vpDisplayX(I_depth, 80+I_gray.getWidth(), 50,
"Depth stream");
280 #elif defined(VISP_HAVE_GDI)
284 #elif defined(VISP_HAVE_OPENCV)
297 std::vector<int> trackerTypes;
298 #ifdef VISP_HAVE_OPENCV
322 #ifdef VISP_HAVE_OPENCV
323 if (opt_use_texture) {
341 tracker.
loadModel(
"cube.cao",
"cube.cao");
342 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
358 state_t state = state_detection;
361 while (state != state_quit) {
364 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, NULL, pointcloud, NULL);
366 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, &pointcloud, NULL, NULL);
373 mapOfImages[
"Camera1"] = &I_gray;
374 mapOfImages[
"Camera2"] = &I_depth;
376 mapOfPointclouds[
"Camera2"] = pointcloud;
378 mapOfPointclouds[
"Camera2"] = &pointcloud;
379 mapOfWidths[
"Camera2"] = width;
380 mapOfHeights[
"Camera2"] = height;
388 if (state == state_detection) {
389 state = detectAprilTag(I_gray, detector, opt_tag_size, cam_color, cMo);
392 if (state == state_tracking) {
394 mapOfCameraPoses[
"Camera1"] = cMo;
395 mapOfCameraPoses[
"Camera2"] = depth_M_color * cMo;
404 if (state == state_tracking) {
407 state = track(mapOfImages, mapOfPointclouds,
408 I_gray, I_depth, depth_M_color, tracker, opt_projection_error_threshold, cMo);
410 state = track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights,
411 I_gray, I_depth, depth_M_color, tracker, opt_projection_error_threshold, cMo);
415 state = track(I_gray, tracker, opt_projection_error_threshold, cMo);
440 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
447 #ifndef VISP_HAVE_APRILTAG
448 std::cout <<
"ViSP is not build with Apriltag support" << std::endl;
450 #ifndef VISP_HAVE_REALSENSE2
451 std::cout <<
"ViSP is not build with librealsense2 support" << std::endl;
453 std::cout <<
"Install missing 3rd parties, configure and build ViSP to run this tutorial" << std::endl;