4 #include <visp3/core/vpConfig.h>
6 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_PUGIXML)
7 #include <visp3/core/vpDisplay.h>
8 #include <visp3/core/vpIoTools.h>
9 #include <visp3/gui/vpDisplayGDI.h>
10 #include <visp3/gui/vpDisplayOpenCV.h>
11 #include <visp3/gui/vpDisplayX.h>
12 #include <visp3/mbt/vpMbGenericTracker.h>
13 #include <visp3/sensor/vpRealSense2.h>
14 #include <visp3/vision/vpKeyPoint.h>
16 int main(
int argc,
char *argv[])
18 std::string config_color =
"", config_depth =
"";
19 std::string model_color =
"", model_depth =
"";
20 std::string init_file =
"";
21 bool use_ogre =
false;
22 bool use_scanline =
false;
23 bool use_edges =
true;
25 bool use_depth =
true;
27 bool auto_init =
false;
28 double proj_error_threshold = 25;
29 std::string learning_data =
"learning/data-learned.bin";
30 bool display_projection_error =
false;
32 for (
int i = 1; i < argc; i++) {
33 if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
34 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]);
39 else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
40 model_color = std::string(argv[i + 1]);
42 else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
43 model_depth = std::string(argv[i + 1]);
45 else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
46 init_file = std::string(argv[i + 1]);
48 else if (std::string(argv[i]) ==
"--proj_error_threshold" && i + 1 < argc) {
49 proj_error_threshold = std::atof(argv[i + 1]);
51 else if (std::string(argv[i]) ==
"--use_ogre") {
54 else if (std::string(argv[i]) ==
"--use_scanline") {
57 else if (std::string(argv[i]) ==
"--use_edges" && i + 1 < argc) {
58 use_edges = (std::atoi(argv[i + 1]) == 0 ? false :
true);
60 else if (std::string(argv[i]) ==
"--use_klt" && i + 1 < argc) {
61 use_klt = (std::atoi(argv[i + 1]) == 0 ? false :
true);
63 else if (std::string(argv[i]) ==
"--use_depth" && i + 1 < argc) {
64 use_depth = (std::atoi(argv[i + 1]) == 0 ? false :
true);
66 else if (std::string(argv[i]) ==
"--learn") {
69 else if (std::string(argv[i]) ==
"--learning_data" && i + 1 < argc) {
70 learning_data = argv[i + 1];
72 else if (std::string(argv[i]) ==
"--auto_init") {
75 else if (std::string(argv[i]) ==
"--display_proj_error") {
76 display_projection_error =
true;
78 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
79 std::cout <<
"Usage: \n"
81 <<
" [--model_color <object.cao>] [--model_depth <object.cao>]"
82 " [--config_color <object.xml>] [--config_depth <object.xml>]"
83 " [--init_file <object.init>] [--use_ogre] [--use_scanline]"
84 " [--proj_error_threshold <threshold between 0 and 90> (default: "
85 << proj_error_threshold
87 " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]"
88 " [--learn] [--auto_init] [--learning_data <path to .bin> (default: learning/data-learned.bin)]"
89 " [--display_proj_error]"
92 std::cout <<
"\n** How to track a 4.2 cm width cube with manual initialization:\n"
93 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl;
94 std::cout <<
"\n** How to learn the cube and create a learning database:\n"
95 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn"
97 std::cout <<
"\n** How to track the cube with initialization from learning database:\n"
98 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init"
105 if (model_depth.empty()) {
106 model_depth = model_color;
109 if (config_color.empty()) {
110 config_color = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".xml";
112 if (config_depth.empty()) {
113 config_depth = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
"_depth.xml";
115 if (init_file.empty()) {
116 init_file = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".init";
118 std::cout <<
"Tracked features: " << std::endl;
119 std::cout <<
" Use edges : " << use_edges << std::endl;
120 std::cout <<
" Use klt : " << use_klt << std::endl;
121 std::cout <<
" Use depth : " << use_depth << std::endl;
122 std::cout <<
"Tracker options: " << std::endl;
123 std::cout <<
" Use ogre : " << use_ogre << std::endl;
124 std::cout <<
" Use scanline: " << use_scanline << std::endl;
125 std::cout <<
" Proj. error : " << proj_error_threshold << std::endl;
126 std::cout <<
" Display proj. error: " << display_projection_error << std::endl;
127 std::cout <<
"Config files: " << std::endl;
128 std::cout <<
" Config color: "
129 <<
"\"" << config_color <<
"\"" << std::endl;
130 std::cout <<
" Config depth: "
131 <<
"\"" << config_depth <<
"\"" << std::endl;
132 std::cout <<
" Model color : "
133 <<
"\"" << model_color <<
"\"" << std::endl;
134 std::cout <<
" Model depth : "
135 <<
"\"" << model_depth <<
"\"" << std::endl;
136 std::cout <<
" Init file : "
137 <<
"\"" << init_file <<
"\"" << std::endl;
138 std::cout <<
"Learning options : " << std::endl;
139 std::cout <<
" Learn : " << learn << std::endl;
140 std::cout <<
" Auto init : " << auto_init << std::endl;
141 std::cout <<
" Learning data: " << learning_data << std::endl;
143 if (!use_edges && !use_klt && !use_depth) {
144 std::cout <<
"You must choose at least one visual features between edge, KLT and depth." << std::endl;
148 if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) {
149 std::cout <<
"config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || "
156 int width = 640, height = 480;
159 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
160 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
163 realsense.
open(config);
166 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
167 std::cout <<
"Check if the Realsense camera is connected..." << std::endl;
176 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
177 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
184 unsigned int _posx = 100, _posy = 50;
188 #elif defined(VISP_HAVE_GDI)
190 #elif defined(HAVE_OPENCV_HIGHGUI)
193 if (use_edges || use_klt)
194 d1.
init(I_gray, _posx, _posy,
"Color stream");
196 d2.
init(I_depth, _posx + I_gray.getWidth() + 10, _posy,
"Depth stream");
199 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap,
nullptr,
nullptr);
201 if (use_edges || use_klt) {
224 std::vector<int> trackerTypes;
225 if (use_edges && use_klt)
236 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
237 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
238 std::map<std::string, std::string> mapOfInitFiles;
239 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
240 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
241 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
243 std::vector<vpColVector> pointcloud;
247 if ((use_edges || use_klt) && use_depth) {
248 tracker.loadConfigFile(config_color, config_depth);
249 tracker.loadModel(model_color, model_depth);
250 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
251 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
252 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
253 mapOfImages[
"Camera1"] = &I_gray;
254 mapOfImages[
"Camera2"] = &I_depth;
255 mapOfInitFiles[
"Camera1"] = init_file;
256 tracker.setCameraParameters(cam_color, cam_depth);
258 else if (use_edges || use_klt) {
259 tracker.loadConfigFile(config_color);
260 tracker.loadModel(model_color);
261 tracker.setCameraParameters(cam_color);
263 else if (use_depth) {
264 tracker.loadConfigFile(config_depth);
265 tracker.loadModel(model_depth);
266 tracker.setCameraParameters(cam_depth);
269 tracker.setDisplayFeatures(
true);
270 tracker.setOgreVisibilityTest(use_ogre);
271 tracker.setScanLineVisibilityTest(use_scanline);
272 tracker.setProjectionErrorComputation(
true);
273 tracker.setProjectionErrorDisplay(display_projection_error);
275 #if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) || \
276 (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
277 std::string detectorName =
"SIFT";
278 std::string extractorName =
"SIFT";
279 std::string matcherName =
"BruteForce";
281 std::string detectorName =
"FAST";
282 std::string extractorName =
"ORB";
283 std::string matcherName =
"BruteForce-Hamming";
286 if (learn || auto_init) {
290 #if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
291 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
292 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
294 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
296 orb_detector->setNLevels(1);
304 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
310 if ((use_edges || use_klt) && use_depth)
311 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
312 else if (use_edges || use_klt)
313 tracker.initClick(I_gray, init_file,
true);
315 tracker.initClick(I_depth, init_file,
true);
321 bool run_auto_init =
false;
323 run_auto_init =
true;
325 std::vector<double> times_vec;
331 bool learn_position =
false;
337 bool tracking_failed =
false;
340 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointcloud,
nullptr,
nullptr);
342 if (use_edges || use_klt || run_auto_init) {
351 if ((use_edges || use_klt) && use_depth) {
352 mapOfImages[
"Camera1"] = &I_gray;
353 mapOfPointclouds[
"Camera2"] = &pointcloud;
354 mapOfWidths[
"Camera2"] = width;
355 mapOfHeights[
"Camera2"] = height;
357 else if (use_edges || use_klt) {
358 mapOfImages[
"Camera"] = &I_gray;
360 else if (use_depth) {
361 mapOfPointclouds[
"Camera"] = &pointcloud;
362 mapOfWidths[
"Camera"] = width;
363 mapOfHeights[
"Camera"] = height;
368 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
369 std::cout <<
"Auto init succeed" << std::endl;
370 if ((use_edges || use_klt) && use_depth) {
371 mapOfCameraPoses[
"Camera1"] = cMo;
372 mapOfCameraPoses[
"Camera2"] = depth_M_color * cMo;
373 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
375 else if (use_edges || use_klt) {
376 tracker.initFromPose(I_gray, cMo);
378 else if (use_depth) {
379 tracker.initFromPose(I_depth, depth_M_color * cMo);
383 if (use_edges || use_klt) {
397 tracker.setDisplayFeatures(
false);
399 run_auto_init =
false;
401 if ((use_edges || use_klt) && use_depth) {
402 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
404 else if (use_edges || use_klt) {
405 tracker.track(I_gray);
407 else if (use_depth) {
408 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
413 tracking_failed =
true;
415 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
416 run_auto_init =
true;
421 cMo = tracker.getPose();
424 double proj_error = 0;
427 proj_error = tracker.getProjectionError();
430 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
433 if (auto_init && proj_error > proj_error_threshold) {
434 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
435 run_auto_init =
true;
436 tracking_failed =
true;
440 if (!tracking_failed) {
442 tracker.setDisplayFeatures(
true);
444 if ((use_edges || use_klt) && use_depth) {
445 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
449 else if (use_edges || use_klt) {
450 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
453 else if (use_depth) {
454 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
459 std::stringstream ss;
460 ss <<
"Nb features: " << tracker.getError().size();
464 std::stringstream ss;
465 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
466 <<
", depth " << tracker.getNbFeaturesDepthDense();
471 std::stringstream ss;
472 ss <<
"Loop time: " << loop_t <<
" ms";
475 if (use_edges || use_klt) {
491 learn_position =
true;
494 run_auto_init =
true;
508 if (learn_position) {
510 std::vector<cv::KeyPoint> trainKeyPoints;
511 keypoint.
detect(I_gray, trainKeyPoints);
514 std::vector<vpPolygon> polygons;
515 std::vector<std::vector<vpPoint> > roisPt;
516 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
517 polygons = pair.first;
518 roisPt = pair.second;
521 std::vector<cv::Point3f> points3f;
525 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
528 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
531 learn_position =
false;
532 std::cout <<
"Data learned" << std::endl;
535 times_vec.push_back(loop_t);
538 std::cout <<
"Save learning file: " << learning_data << std::endl;
543 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
546 if (!times_vec.empty()) {
554 #elif defined(VISP_HAVE_REALSENSE2)
557 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
563 std::cout <<
"Install librealsense2 3rd party, configure and build ViSP again to use this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor none
static const vpColor yellow
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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 std::string & getStringMessage() const
const char * what() 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)
Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV libr...
unsigned int matchPoint(const vpImage< unsigned char > &I)
void setExtractor(const vpFeatureDescriptorType &extractorType)
void loadLearningData(const std::string &filename, bool binaryMode=false, bool append=false)
void detect(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, const vpRect &rectangle=vpRect())
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector< cv::KeyPoint > &candidates, const std::vector< vpPolygon > &polygons, const std::vector< std::vector< vpPoint > > &roisPt, std::vector< cv::Point3f > &points, cv::Mat *descriptors=nullptr)
void setMatcher(const std::string &matcherName)
void saveLearningData(const std::string &filename, bool binaryMode=false, bool saveTrainingImages=true)
void setDetector(const vpFeatureDetectorType &detectorType)
unsigned int buildReference(const vpImage< unsigned char > &I)
cv::Ptr< cv::FeatureDetector > getDetector(const vpFeatureDetectorType &type) const
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)
Real-time 6D object pose tracking using its CAD model.
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
VISP_EXPORT double measureTimeMs()