4 #include <visp3/core/vpConfig.h>
6 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV)
7 #include <visp3/core/vpDisplay.h>
8 #include <visp3/core/vpIoTools.h>
9 #include <visp3/core/vpXmlParserCamera.h>
10 #include <visp3/gui/vpDisplayX.h>
11 #include <visp3/gui/vpDisplayGDI.h>
12 #include <visp3/gui/vpDisplayOpenCV.h>
13 #include <visp3/mbt/vpMbGenericTracker.h>
14 #include <visp3/sensor/vpRealSense2.h>
15 #include <visp3/vision/vpKeyPoint.h>
17 int main(
int argc,
char *argv[])
19 std::string config_color =
"", config_depth =
"";
20 std::string model_color =
"", model_depth =
"";
21 std::string init_file =
"";
22 bool use_ogre =
false;
23 bool use_scanline =
false;
24 bool use_edges =
true;
26 bool use_depth =
true;
28 bool auto_init =
false;
29 double proj_error_threshold = 25;
30 std::string learning_data =
"learning/data-learned.bin";
31 bool display_projection_error =
false;
33 for (
int i = 1; i < argc; i++) {
34 if (std::string(argv[i]) ==
"--config_color" && i+1 < argc) {
35 config_color = std::string(argv[i+1]);
36 }
else if (std::string(argv[i]) ==
"--config_depth" && i+1 < argc) {
37 config_depth = std::string(argv[i+1]);
38 }
else if (std::string(argv[i]) ==
"--model_color" && i+1 < argc) {
39 model_color = std::string(argv[i+1]);
40 }
else if (std::string(argv[i]) ==
"--model_depth" && i+1 < argc) {
41 model_depth = std::string(argv[i+1]);
42 }
else if (std::string(argv[i]) ==
"--init_file" && i+1 < argc) {
43 init_file = std::string(argv[i+1]);
44 }
else if (std::string(argv[i]) ==
"--proj_error_threshold" && i+1 < argc) {
45 proj_error_threshold = std::atof(argv[i+1]);
46 }
else if (std::string(argv[i]) ==
"--use_ogre") {
48 }
else if (std::string(argv[i]) ==
"--use_scanline") {
50 }
else if (std::string(argv[i]) ==
"--use_edges" && i+1 < argc) {
51 use_edges = (std::atoi(argv[i+1]) == 0 ? false :
true);
52 }
else if (std::string(argv[i]) ==
"--use_klt" && i+1 < argc) {
53 use_klt = (std::atoi(argv[i+1]) == 0 ? false :
true);
54 }
else if (std::string(argv[i]) ==
"--use_depth" && i+1 < argc) {
55 use_depth = (std::atoi(argv[i+1]) == 0 ? false :
true);
56 }
else if (std::string(argv[i]) ==
"--learn") {
58 }
else if (std::string(argv[i]) ==
"--learning_data" && i+1 < argc) {
59 learning_data = argv[i+1];
60 }
else if (std::string(argv[i]) ==
"--auto_init") {
62 }
else if (std::string(argv[i]) ==
"--display_proj_error") {
63 display_projection_error =
true;
64 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
65 std::cout <<
"Usage: \n" << argv[0]
66 <<
" [--config_color <object.xml>] [--config_depth <object.xml>] [--model_color <object.cao>] [--model_depth <object.cao>]"
67 " [--init_file <object.init>] [--use_ogre] [--use_scanline]"
68 " [--proj_error_threshold <threshold between 0 and 90> (default: "<< proj_error_threshold <<
")]"
69 " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 0)] [--use_depth <0|1> (default: 1)]"
70 " [--learn] [--auto_init] [--learning_data <path to .bin> (default: learning/data-learned.bin)]"
71 " [--display_proj_error]" << std::endl;
72 std::cout <<
"\nExample to track a 4.2 cm width cube with manual initialization:\n" << argv[0]
73 <<
" --config_color cube.xml --config_depth cube_depth.xml"
74 <<
" --model_color cube.cao --model_depth cube.cao"
75 <<
" --init_file cube.init"
76 <<
" --use_edges 1 --use_klt 1 --use_depth 1"
78 std::cout <<
"\nExample to learn the 4.2 cm width cube:\n" << argv[0]
79 <<
" --config_color cube.xml --config_depth cube_depth.xml"
80 <<
" --model_color cube.cao --model_depth cube.cao"
81 <<
" --init_file cube.init"
82 <<
" --use_edges 1 --use_klt 1 --use_depth 1"
85 std::cout <<
"\nExample to track a 4.2 cm width cube with initialization from learning database:\n" << argv[0]
86 <<
" --config_color cube.xml --config_depth cube_depth.xml"
87 <<
" --model_color cube.cao --model_depth cube.cao"
88 <<
" --init_file cube.init"
89 <<
" --use_edges 1 --use_klt 1 --use_depth 1"
96 std::cout <<
"Tracked features: " << std::endl;
97 std::cout <<
" Use edges : " << use_edges << std::endl;
98 std::cout <<
" Use klt : " << use_klt << std::endl;
99 std::cout <<
" Use depth : " << use_depth << std::endl;
100 std::cout <<
"Tracker options: " << std::endl;
101 std::cout <<
" Use ogre : " << use_ogre << std::endl;
102 std::cout <<
" Use scanline: " << use_scanline << std::endl;
103 std::cout <<
" Proj. error : " << proj_error_threshold << std::endl;
104 std::cout <<
" Display proj. error: " << display_projection_error << std::endl;
105 std::cout <<
"Config files: " << std::endl;
106 std::cout <<
" Config color: " <<
"\"" << config_color <<
"\"" << std::endl;
107 std::cout <<
" Config depth: " <<
"\"" << config_depth <<
"\"" << std::endl;
108 std::cout <<
" Model color : " <<
"\"" << model_color <<
"\"" << std::endl;
109 std::cout <<
" Model depth : " <<
"\"" << model_depth <<
"\"" << std::endl;
110 std::cout <<
" Init file : " <<
"\"" << init_file <<
"\"" << std::endl;
111 std::cout <<
"Learning options : " << std::endl;
112 std::cout <<
" Learn : " << learn << std::endl;
113 std::cout <<
" Auto init : " << auto_init << std::endl;
114 std::cout <<
" Learning data: " << learning_data << std::endl;
116 if (!use_edges && !use_klt && !use_depth) {
117 std::cout <<
"You must choose at least one visual features between edge, KLT and depth." << std::endl;
121 if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) {
122 std::cout <<
"config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()" << std::endl;
127 int width = 640, height = 480;
130 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
131 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
134 realsense.
open(config);
137 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
138 std::cout <<
"Check if the Realsense camera is connected..." << std::endl;
145 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
146 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
153 unsigned int _posx = 100, _posy = 50;
157 #elif defined(VISP_HAVE_GDI)
159 #elif defined(VISP_HAVE_OPENCV)
162 if (use_edges || use_klt)
163 d1.
init(I_gray, _posx, _posy,
"Color stream");
165 d2.
init(I_depth, _posx + I_gray.getWidth()+10, _posy,
"Depth stream");
168 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, NULL, NULL);
170 if (use_edges || use_klt) {
193 std::vector<int> trackerTypes;
194 if (use_edges && use_klt)
205 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
206 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
207 std::map<std::string, std::string> mapOfInitFiles;
208 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
209 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
210 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
212 std::vector<vpColVector> pointcloud;
216 if ((use_edges || use_klt) && use_depth) {
217 tracker.loadConfigFile(config_color, config_depth);
218 tracker.loadModel(model_color, model_depth);
219 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
220 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
221 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
222 mapOfImages[
"Camera1"] = &I_gray;
223 mapOfImages[
"Camera2"] = &I_depth;
224 mapOfInitFiles[
"Camera1"] = init_file;
225 tracker.setCameraParameters(cam_color, cam_depth);
227 else if (use_edges || use_klt) {
228 tracker.loadConfigFile(config_color);
229 tracker.loadModel(model_color);
230 tracker.setCameraParameters(cam_color);
232 else if (use_depth) {
233 tracker.loadConfigFile(config_depth);
234 tracker.loadModel(model_depth);
235 tracker.setCameraParameters(cam_depth);
238 tracker.setDisplayFeatures(
true);
239 tracker.setOgreVisibilityTest(use_ogre);
240 tracker.setScanLineVisibilityTest(use_scanline);
241 tracker.setProjectionErrorComputation(
true);
242 tracker.setProjectionErrorDisplay(display_projection_error);
244 #if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
245 std::string detectorName =
"SIFT";
246 std::string extractorName =
"SIFT";
247 std::string matcherName =
"BruteForce";
249 std::string detectorName =
"FAST";
250 std::string extractorName =
"ORB";
251 std::string matcherName =
"BruteForce-Hamming";
254 if (learn || auto_init) {
258 #if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
259 # if (VISP_HAVE_OPENCV_VERSION < 0x030000)
260 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
262 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
264 orb_detector->setNLevels(1);
272 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
277 if ((use_edges || use_klt) && use_depth)
278 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
279 else if (use_edges || use_klt)
280 tracker.initClick(I_gray, init_file,
true);
282 tracker.initClick(I_depth, init_file,
true);
289 bool run_auto_init =
false;
291 run_auto_init =
true;
293 std::vector<double> times_vec;
299 bool learn_position =
false;
305 bool tracking_failed =
false;
308 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, &pointcloud, NULL, NULL);
310 if (use_edges || use_klt || run_auto_init) {
319 if ((use_edges || use_klt) && use_depth) {
320 mapOfImages[
"Camera1"] = &I_gray;
321 mapOfPointclouds[
"Camera2"] = &pointcloud;
322 mapOfWidths[
"Camera2"] = width;
323 mapOfHeights[
"Camera2"] = height;
324 }
else if (use_edges || use_klt) {
325 mapOfImages[
"Camera"] = &I_gray;
326 }
else if (use_depth) {
327 mapOfPointclouds[
"Camera"] = &pointcloud;
328 mapOfWidths[
"Camera"] = width;
329 mapOfHeights[
"Camera"] = height;
334 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
335 std::cout <<
"Auto init succeed" << std::endl;
336 if ((use_edges || use_klt) && use_depth) {
337 mapOfCameraPoses[
"Camera1"] = cMo;
338 mapOfCameraPoses[
"Camera2"] = depth_M_color *cMo;
339 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
340 }
else if (use_edges || use_klt) {
341 tracker.initFromPose(I_gray, cMo);
342 }
else if (use_depth) {
343 tracker.initFromPose(I_depth, depth_M_color*cMo);
346 if (use_edges || use_klt) {
360 tracker.setDisplayFeatures(
false);
362 run_auto_init =
false;
364 if ((use_edges || use_klt) && use_depth) {
365 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
366 }
else if (use_edges || use_klt) {
367 tracker.track(I_gray);
368 }
else if (use_depth) {
369 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
373 tracking_failed =
true;
375 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
376 run_auto_init =
true;
381 cMo = tracker.getPose();
384 double proj_error = 0;
387 proj_error = tracker.getProjectionError();
390 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
393 if (auto_init && proj_error > proj_error_threshold) {
394 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
395 run_auto_init =
true;
396 tracking_failed =
true;
400 if (!tracking_failed) {
402 tracker.setDisplayFeatures(
true);
404 if ((use_edges || use_klt) && use_depth) {
405 tracker.display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
408 }
else if (use_edges || use_klt) {
409 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
411 }
else if (use_depth) {
412 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
416 if (use_edges || use_klt) {
417 std::stringstream ss;
418 ss <<
"Nb features: " << tracker.getError().getRows();
420 }
else if (use_depth) {
421 std::stringstream ss;
422 ss <<
"Nb features: " << tracker.getError().getRows();
427 std::stringstream ss;
428 ss <<
"Loop time: " << loop_t <<
" ms";
431 if (use_edges || use_klt) {
446 learn_position =
true;
448 run_auto_init =
true;
462 if (learn_position) {
464 std::vector<cv::KeyPoint> trainKeyPoints;
465 keypoint.
detect(I_gray, trainKeyPoints);
468 std::vector<vpPolygon> polygons;
469 std::vector<std::vector<vpPoint> > roisPt;
470 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
471 polygons = pair.first;
472 roisPt = pair.second;
475 std::vector<cv::Point3f> points3f;
479 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
482 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
485 learn_position =
false;
486 std::cout <<
"Data learned" << std::endl;
489 times_vec.push_back(loop_t);
492 std::cout <<
"Save learning file: " << learning_data << std::endl;
496 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
499 if (!times_vec.empty()) {
506 #elif defined(VISP_HAVE_REALSENSE2)
508 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
513 std::cout <<
"Install librealsense2 3rd party, configure and build ViSP again to use this example" << std::endl;