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 <<
" [--model_color <object.cao>] [--model_depth <object.cao>]"
67 " [--config_color <object.xml>] [--config_depth <object.xml>]"
68 " [--init_file <object.init>] [--use_ogre] [--use_scanline]"
69 " [--proj_error_threshold <threshold between 0 and 90> (default: "<< proj_error_threshold <<
")]"
70 " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]"
71 " [--learn] [--auto_init] [--learning_data <path to .bin> (default: learning/data-learned.bin)]"
72 " [--display_proj_error]" << std::endl;
74 std::cout <<
"\n** How to track a 4.2 cm width cube with manual initialization:\n"
76 <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1"
78 std::cout <<
"\n** How to learn the cube and create a learning database:\n" << argv[0]
79 <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn"
81 std::cout <<
"\n** How to track the cube with initialization from learning database:\n" << argv[0]
82 <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init"
89 if (model_depth.empty()) {
90 model_depth = model_color;
93 if (config_color.empty()) {
94 config_color = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".xml";
96 if (config_depth.empty()) {
97 config_depth = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
"_depth.xml";
99 if (init_file.empty()) {
100 init_file = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".init";
102 std::cout <<
"Tracked features: " << std::endl;
103 std::cout <<
" Use edges : " << use_edges << std::endl;
104 std::cout <<
" Use klt : " << use_klt << std::endl;
105 std::cout <<
" Use depth : " << use_depth << std::endl;
106 std::cout <<
"Tracker options: " << std::endl;
107 std::cout <<
" Use ogre : " << use_ogre << std::endl;
108 std::cout <<
" Use scanline: " << use_scanline << std::endl;
109 std::cout <<
" Proj. error : " << proj_error_threshold << std::endl;
110 std::cout <<
" Display proj. error: " << display_projection_error << std::endl;
111 std::cout <<
"Config files: " << std::endl;
112 std::cout <<
" Config color: " <<
"\"" << config_color <<
"\"" << std::endl;
113 std::cout <<
" Config depth: " <<
"\"" << config_depth <<
"\"" << std::endl;
114 std::cout <<
" Model color : " <<
"\"" << model_color <<
"\"" << std::endl;
115 std::cout <<
" Model depth : " <<
"\"" << model_depth <<
"\"" << std::endl;
116 std::cout <<
" Init file : " <<
"\"" << init_file <<
"\"" << std::endl;
117 std::cout <<
"Learning options : " << std::endl;
118 std::cout <<
" Learn : " << learn << std::endl;
119 std::cout <<
" Auto init : " << auto_init << std::endl;
120 std::cout <<
" Learning data: " << learning_data << std::endl;
122 if (!use_edges && !use_klt && !use_depth) {
123 std::cout <<
"You must choose at least one visual features between edge, KLT and depth." << std::endl;
127 if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) {
128 std::cout <<
"config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()" << std::endl;
133 int width = 640, height = 480;
136 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
137 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
140 realsense.
open(config);
143 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
144 std::cout <<
"Check if the Realsense camera is connected..." << std::endl;
151 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
152 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
159 unsigned int _posx = 100, _posy = 50;
163 #elif defined(VISP_HAVE_GDI)
165 #elif defined(VISP_HAVE_OPENCV)
168 if (use_edges || use_klt)
169 d1.
init(I_gray, _posx, _posy,
"Color stream");
171 d2.
init(I_depth, _posx + I_gray.getWidth()+10, _posy,
"Depth stream");
174 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, NULL, NULL);
176 if (use_edges || use_klt) {
199 std::vector<int> trackerTypes;
200 if (use_edges && use_klt)
211 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
212 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
213 std::map<std::string, std::string> mapOfInitFiles;
214 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
215 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
216 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
218 std::vector<vpColVector> pointcloud;
222 if ((use_edges || use_klt) && use_depth) {
223 tracker.loadConfigFile(config_color, config_depth);
224 tracker.loadModel(model_color, model_depth);
225 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
226 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
227 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
228 mapOfImages[
"Camera1"] = &I_gray;
229 mapOfImages[
"Camera2"] = &I_depth;
230 mapOfInitFiles[
"Camera1"] = init_file;
231 tracker.setCameraParameters(cam_color, cam_depth);
233 else if (use_edges || use_klt) {
234 tracker.loadConfigFile(config_color);
235 tracker.loadModel(model_color);
236 tracker.setCameraParameters(cam_color);
238 else if (use_depth) {
239 tracker.loadConfigFile(config_depth);
240 tracker.loadModel(model_depth);
241 tracker.setCameraParameters(cam_depth);
244 tracker.setDisplayFeatures(
true);
245 tracker.setOgreVisibilityTest(use_ogre);
246 tracker.setScanLineVisibilityTest(use_scanline);
247 tracker.setProjectionErrorComputation(
true);
248 tracker.setProjectionErrorDisplay(display_projection_error);
250 #if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) || \
251 (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
252 std::string detectorName =
"SIFT";
253 std::string extractorName =
"SIFT";
254 std::string matcherName =
"BruteForce";
256 std::string detectorName =
"FAST";
257 std::string extractorName =
"ORB";
258 std::string matcherName =
"BruteForce-Hamming";
261 if (learn || auto_init) {
265 #if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
266 # if (VISP_HAVE_OPENCV_VERSION < 0x030000)
267 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
269 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
271 orb_detector->setNLevels(1);
279 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
284 if ((use_edges || use_klt) && use_depth)
285 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
286 else if (use_edges || use_klt)
287 tracker.initClick(I_gray, init_file,
true);
289 tracker.initClick(I_depth, init_file,
true);
296 bool run_auto_init =
false;
298 run_auto_init =
true;
300 std::vector<double> times_vec;
306 bool learn_position =
false;
312 bool tracking_failed =
false;
315 realsense.
acquire((
unsigned char *) I_color.bitmap, (
unsigned char *) I_depth_raw.bitmap, &pointcloud, NULL, NULL);
317 if (use_edges || use_klt || run_auto_init) {
326 if ((use_edges || use_klt) && use_depth) {
327 mapOfImages[
"Camera1"] = &I_gray;
328 mapOfPointclouds[
"Camera2"] = &pointcloud;
329 mapOfWidths[
"Camera2"] = width;
330 mapOfHeights[
"Camera2"] = height;
331 }
else if (use_edges || use_klt) {
332 mapOfImages[
"Camera"] = &I_gray;
333 }
else if (use_depth) {
334 mapOfPointclouds[
"Camera"] = &pointcloud;
335 mapOfWidths[
"Camera"] = width;
336 mapOfHeights[
"Camera"] = height;
341 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
342 std::cout <<
"Auto init succeed" << std::endl;
343 if ((use_edges || use_klt) && use_depth) {
344 mapOfCameraPoses[
"Camera1"] = cMo;
345 mapOfCameraPoses[
"Camera2"] = depth_M_color *cMo;
346 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
347 }
else if (use_edges || use_klt) {
348 tracker.initFromPose(I_gray, cMo);
349 }
else if (use_depth) {
350 tracker.initFromPose(I_depth, depth_M_color*cMo);
353 if (use_edges || use_klt) {
367 tracker.setDisplayFeatures(
false);
369 run_auto_init =
false;
371 if ((use_edges || use_klt) && use_depth) {
372 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
373 }
else if (use_edges || use_klt) {
374 tracker.track(I_gray);
375 }
else if (use_depth) {
376 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
380 tracking_failed =
true;
382 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
383 run_auto_init =
true;
388 cMo = tracker.getPose();
391 double proj_error = 0;
394 proj_error = tracker.getProjectionError();
397 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
400 if (auto_init && proj_error > proj_error_threshold) {
401 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
402 run_auto_init =
true;
403 tracking_failed =
true;
407 if (!tracking_failed) {
409 tracker.setDisplayFeatures(
true);
411 if ((use_edges || use_klt) && use_depth) {
412 tracker.display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
415 }
else if (use_edges || use_klt) {
416 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
418 }
else if (use_depth) {
419 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
424 std::stringstream ss;
425 ss <<
"Nb features: " << tracker.getError().size();
429 std::stringstream ss;
430 ss <<
"Features: edges " << tracker.getNbFeaturesEdge()
431 <<
", klt " << tracker.getNbFeaturesKlt()
432 <<
", depth " << tracker.getNbFeaturesDepthDense();
437 std::stringstream ss;
438 ss <<
"Loop time: " << loop_t <<
" ms";
441 if (use_edges || use_klt) {
456 learn_position =
true;
458 run_auto_init =
true;
472 if (learn_position) {
474 std::vector<cv::KeyPoint> trainKeyPoints;
475 keypoint.
detect(I_gray, trainKeyPoints);
478 std::vector<vpPolygon> polygons;
479 std::vector<std::vector<vpPoint> > roisPt;
480 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
481 polygons = pair.first;
482 roisPt = pair.second;
485 std::vector<cv::Point3f> points3f;
489 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
492 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
495 learn_position =
false;
496 std::cout <<
"Data learned" << std::endl;
499 times_vec.push_back(loop_t);
502 std::cout <<
"Save learning file: " << learning_data << std::endl;
506 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
509 if (!times_vec.empty()) {
516 #elif defined(VISP_HAVE_REALSENSE2)
518 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
523 std::cout <<
"Install librealsense2 3rd party, configure and build ViSP again to use this example" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
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 winx=-1, int winy=-1, const std::string &title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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 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))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
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())
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)
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.
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()