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/vpDisplayGDI.h>
11#include <visp3/gui/vpDisplayOpenCV.h>
12#include <visp3/gui/vpDisplayX.h>
13#include <visp3/mbt/vpMbGenericTracker.h>
14#include <visp3/sensor/vpRealSense2.h>
15#include <visp3/vision/vpKeyPoint.h>
17int 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"
67 <<
" [--model_color <object.cao>] [--model_depth <object.cao>]"
68 " [--config_color <object.xml>] [--config_depth <object.xml>]"
69 " [--init_file <object.init>] [--use_ogre] [--use_scanline]"
70 " [--proj_error_threshold <threshold between 0 and 90> (default: "
71 << proj_error_threshold
73 " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]"
74 " [--learn] [--auto_init] [--learning_data <path to .bin> (default: learning/data-learned.bin)]"
75 " [--display_proj_error]"
78 std::cout <<
"\n** How to track a 4.2 cm width cube with manual initialization:\n"
79 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl;
80 std::cout <<
"\n** How to learn the cube and create a learning database:\n"
81 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn"
83 std::cout <<
"\n** How to track the cube with initialization from learning database:\n"
84 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init"
91 if (model_depth.empty()) {
92 model_depth = model_color;
95 if (config_color.empty()) {
96 config_color = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".xml";
98 if (config_depth.empty()) {
99 config_depth = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
"_depth.xml";
101 if (init_file.empty()) {
102 init_file = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".init";
104 std::cout <<
"Tracked features: " << std::endl;
105 std::cout <<
" Use edges : " << use_edges << std::endl;
106 std::cout <<
" Use klt : " << use_klt << std::endl;
107 std::cout <<
" Use depth : " << use_depth << std::endl;
108 std::cout <<
"Tracker options: " << std::endl;
109 std::cout <<
" Use ogre : " << use_ogre << std::endl;
110 std::cout <<
" Use scanline: " << use_scanline << std::endl;
111 std::cout <<
" Proj. error : " << proj_error_threshold << std::endl;
112 std::cout <<
" Display proj. error: " << display_projection_error << std::endl;
113 std::cout <<
"Config files: " << std::endl;
114 std::cout <<
" Config color: "
115 <<
"\"" << config_color <<
"\"" << std::endl;
116 std::cout <<
" Config depth: "
117 <<
"\"" << config_depth <<
"\"" << std::endl;
118 std::cout <<
" Model color : "
119 <<
"\"" << model_color <<
"\"" << std::endl;
120 std::cout <<
" Model depth : "
121 <<
"\"" << model_depth <<
"\"" << std::endl;
122 std::cout <<
" Init file : "
123 <<
"\"" << init_file <<
"\"" << std::endl;
124 std::cout <<
"Learning options : " << std::endl;
125 std::cout <<
" Learn : " << learn << std::endl;
126 std::cout <<
" Auto init : " << auto_init << std::endl;
127 std::cout <<
" Learning data: " << learning_data << std::endl;
129 if (!use_edges && !use_klt && !use_depth) {
130 std::cout <<
"You must choose at least one visual features between edge, KLT and depth." << std::endl;
134 if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) {
135 std::cout <<
"config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || "
142 int width = 640, height = 480;
145 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
146 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
149 realsense.
open(config);
151 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
152 std::cout <<
"Check if the Realsense camera is connected..." << std::endl;
161 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
162 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
169 unsigned int _posx = 100, _posy = 50;
173#elif defined(VISP_HAVE_GDI)
175#elif defined(HAVE_OPENCV_HIGHGUI)
178 if (use_edges || use_klt)
179 d1.
init(I_gray, _posx, _posy,
"Color stream");
181 d2.
init(I_depth, _posx + I_gray.getWidth() + 10, _posy,
"Depth stream");
184 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, NULL);
186 if (use_edges || use_klt) {
209 std::vector<int> trackerTypes;
210 if (use_edges && use_klt)
221 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
222 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
223 std::map<std::string, std::string> mapOfInitFiles;
224 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
225 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
226 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
228 std::vector<vpColVector> pointcloud;
232 if ((use_edges || use_klt) && use_depth) {
233 tracker.loadConfigFile(config_color, config_depth);
234 tracker.loadModel(model_color, model_depth);
235 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
236 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
237 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
238 mapOfImages[
"Camera1"] = &I_gray;
239 mapOfImages[
"Camera2"] = &I_depth;
240 mapOfInitFiles[
"Camera1"] = init_file;
241 tracker.setCameraParameters(cam_color, cam_depth);
242 }
else if (use_edges || use_klt) {
243 tracker.loadConfigFile(config_color);
244 tracker.loadModel(model_color);
245 tracker.setCameraParameters(cam_color);
246 }
else if (use_depth) {
247 tracker.loadConfigFile(config_depth);
248 tracker.loadModel(model_depth);
249 tracker.setCameraParameters(cam_depth);
252 tracker.setDisplayFeatures(
true);
253 tracker.setOgreVisibilityTest(use_ogre);
254 tracker.setScanLineVisibilityTest(use_scanline);
255 tracker.setProjectionErrorComputation(
true);
256 tracker.setProjectionErrorDisplay(display_projection_error);
258#if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) || \
259 (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
260 std::string detectorName =
"SIFT";
261 std::string extractorName =
"SIFT";
262 std::string matcherName =
"BruteForce";
264 std::string detectorName =
"FAST";
265 std::string extractorName =
"ORB";
266 std::string matcherName =
"BruteForce-Hamming";
269 if (learn || auto_init) {
273#if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
274#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
275 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
277 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
279 orb_detector->setNLevels(1);
287 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
292 if ((use_edges || use_klt) && use_depth)
293 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
294 else if (use_edges || use_klt)
295 tracker.initClick(I_gray, init_file,
true);
297 tracker.initClick(I_depth, init_file,
true);
303 bool run_auto_init =
false;
305 run_auto_init =
true;
307 std::vector<double> times_vec;
313 bool learn_position =
false;
319 bool tracking_failed =
false;
322 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointcloud, NULL, NULL);
324 if (use_edges || use_klt || run_auto_init) {
333 if ((use_edges || use_klt) && use_depth) {
334 mapOfImages[
"Camera1"] = &I_gray;
335 mapOfPointclouds[
"Camera2"] = &pointcloud;
336 mapOfWidths[
"Camera2"] = width;
337 mapOfHeights[
"Camera2"] = height;
338 }
else if (use_edges || use_klt) {
339 mapOfImages[
"Camera"] = &I_gray;
340 }
else if (use_depth) {
341 mapOfPointclouds[
"Camera"] = &pointcloud;
342 mapOfWidths[
"Camera"] = width;
343 mapOfHeights[
"Camera"] = height;
348 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
349 std::cout <<
"Auto init succeed" << std::endl;
350 if ((use_edges || use_klt) && use_depth) {
351 mapOfCameraPoses[
"Camera1"] = cMo;
352 mapOfCameraPoses[
"Camera2"] = depth_M_color * cMo;
353 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
354 }
else if (use_edges || use_klt) {
355 tracker.initFromPose(I_gray, cMo);
356 }
else if (use_depth) {
357 tracker.initFromPose(I_depth, depth_M_color * cMo);
360 if (use_edges || use_klt) {
374 tracker.setDisplayFeatures(
false);
376 run_auto_init =
false;
378 if ((use_edges || use_klt) && use_depth) {
379 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
380 }
else if (use_edges || use_klt) {
381 tracker.track(I_gray);
382 }
else if (use_depth) {
383 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
387 tracking_failed =
true;
389 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
390 run_auto_init =
true;
395 cMo = tracker.getPose();
398 double proj_error = 0;
401 proj_error = tracker.getProjectionError();
403 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
406 if (auto_init && proj_error > proj_error_threshold) {
407 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
408 run_auto_init =
true;
409 tracking_failed =
true;
413 if (!tracking_failed) {
415 tracker.setDisplayFeatures(
true);
417 if ((use_edges || use_klt) && use_depth) {
418 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
421 }
else if (use_edges || use_klt) {
422 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
424 }
else if (use_depth) {
425 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
430 std::stringstream ss;
431 ss <<
"Nb features: " << tracker.getError().size();
435 std::stringstream ss;
436 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
437 <<
", depth " << tracker.getNbFeaturesDepthDense();
442 std::stringstream ss;
443 ss <<
"Loop time: " << loop_t <<
" ms";
446 if (use_edges || use_klt) {
461 learn_position =
true;
463 run_auto_init =
true;
477 if (learn_position) {
479 std::vector<cv::KeyPoint> trainKeyPoints;
480 keypoint.
detect(I_gray, trainKeyPoints);
483 std::vector<vpPolygon> polygons;
484 std::vector<std::vector<vpPoint> > roisPt;
485 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
486 polygons = pair.first;
487 roisPt = pair.second;
490 std::vector<cv::Point3f> points3f;
494 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
497 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
500 learn_position =
false;
501 std::cout <<
"Data learned" << std::endl;
504 times_vec.push_back(loop_t);
507 std::cout <<
"Save learning file: " << learning_data << std::endl;
511 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
514 if (!times_vec.empty()) {
522#elif defined(VISP_HAVE_REALSENSE2)
525 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
531 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="")
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)
Definition of the vpImage class member functions.
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())
void setMatcher(const std::string &matcherName)
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=NULL)
void saveLearningData(const std::string &filename, bool binaryMode=false, bool saveTrainingImages=true)
void setDetector(const vpFeatureDetectorType &detectorType)
cv::Ptr< cv::FeatureDetector > getDetector(const vpFeatureDetectorType &type) const
unsigned int buildReference(const vpImage< unsigned char > &I)
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.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
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()