4 #include <visp3/core/vpConfig.h>
6 #if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_PUGIXML)
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/vpOccipitalStructure.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]);
37 else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
38 config_depth = std::string(argv[i + 1]);
40 else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
41 model_color = std::string(argv[i + 1]);
43 else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
44 model_depth = std::string(argv[i + 1]);
46 else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
47 init_file = std::string(argv[i + 1]);
49 else if (std::string(argv[i]) ==
"--proj_error_threshold" && i + 1 < argc) {
50 proj_error_threshold = std::atof(argv[i + 1]);
52 else if (std::string(argv[i]) ==
"--use_ogre") {
55 else if (std::string(argv[i]) ==
"--use_scanline") {
58 else if (std::string(argv[i]) ==
"--use_edges" && i + 1 < argc) {
59 use_edges = (std::atoi(argv[i + 1]) == 0 ? false :
true);
61 else if (std::string(argv[i]) ==
"--use_klt" && i + 1 < argc) {
62 use_klt = (std::atoi(argv[i + 1]) == 0 ? false :
true);
64 else if (std::string(argv[i]) ==
"--use_depth" && i + 1 < argc) {
65 use_depth = (std::atoi(argv[i + 1]) == 0 ? false :
true);
67 else if (std::string(argv[i]) ==
"--learn") {
70 else if (std::string(argv[i]) ==
"--learning_data" && i + 1 < argc) {
71 learning_data = argv[i + 1];
73 else if (std::string(argv[i]) ==
"--auto_init") {
76 else if (std::string(argv[i]) ==
"--display_proj_error") {
77 display_projection_error =
true;
79 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
80 std::cout <<
"Usage: \n"
82 <<
" [--model_color <object.cao>] [--model_depth <object.cao>]"
83 " [--config_color <object.xml>] [--config_depth <object.xml>]"
84 " [--init_file <object.init>] [--use_ogre] [--use_scanline]"
85 " [--proj_error_threshold <threshold between 0 and 90> (default: "
86 << proj_error_threshold
88 " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]"
89 " [--learn] [--auto_init] [--learning_data <path to .bin> (default: learning/data-learned.bin)]"
90 " [--display_proj_error]"
93 std::cout <<
"\n** How to track a 4.2 cm width cube with manual initialization:\n"
94 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl;
95 std::cout <<
"\n** How to learn the cube and create a learning database:\n"
96 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn"
98 std::cout <<
"\n** How to track the cube with initialization from learning database:\n"
99 << argv[0] <<
" --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init"
106 if (model_depth.empty()) {
107 model_depth = model_color;
110 if (config_color.empty()) {
111 config_color = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".xml";
113 if (config_depth.empty()) {
114 config_depth = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
"_depth.xml";
116 if (init_file.empty()) {
117 init_file = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model_color) +
".init";
119 std::cout <<
"Tracked features: " << std::endl;
120 std::cout <<
" Use edges : " << use_edges << std::endl;
121 std::cout <<
" Use klt : " << use_klt << std::endl;
122 std::cout <<
" Use depth : " << use_depth << std::endl;
123 std::cout <<
"Tracker options: " << std::endl;
124 std::cout <<
" Use ogre : " << use_ogre << std::endl;
125 std::cout <<
" Use scanline: " << use_scanline << std::endl;
126 std::cout <<
" Proj. error : " << proj_error_threshold << std::endl;
127 std::cout <<
" Display proj. error: " << display_projection_error << std::endl;
128 std::cout <<
"Config files: " << std::endl;
129 std::cout <<
" Config color: "
130 <<
"\"" << config_color <<
"\"" << std::endl;
131 std::cout <<
" Config depth: "
132 <<
"\"" << config_depth <<
"\"" << std::endl;
133 std::cout <<
" Model color : "
134 <<
"\"" << model_color <<
"\"" << std::endl;
135 std::cout <<
" Model depth : "
136 <<
"\"" << model_depth <<
"\"" << std::endl;
137 std::cout <<
" Init file : "
138 <<
"\"" << init_file <<
"\"" << std::endl;
139 std::cout <<
"Learning options : " << std::endl;
140 std::cout <<
" Learn : " << learn << std::endl;
141 std::cout <<
" Auto init : " << auto_init << std::endl;
142 std::cout <<
" Learning data: " << learning_data << std::endl;
144 if (!use_edges && !use_klt && !use_depth) {
145 std::cout <<
"You must choose at least one visual features between edge, KLT and depth." << std::endl;
149 if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) {
150 std::cout <<
"config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || "
157 ST::CaptureSessionSettings settings;
158 settings.source = ST::CaptureSessionSourceId::StructureCore;
159 settings.structureCore.visibleEnabled =
true;
160 settings.applyExpensiveCorrection =
true;
166 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
167 std::cout <<
"Check if the Structure Core camera is connected..." << std::endl;
175 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
176 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
182 unsigned int _posx = 100, _posy = 50;
186 #elif defined(VISP_HAVE_GDI)
188 #elif defined(HAVE_OPENCV_HIGHGUI)
191 if (use_edges || use_klt)
192 d1.
init(I_gray, _posx, _posy,
"Color stream");
194 d2.
init(I_depth, _posx + I_gray.getWidth() + 10, _posy,
"Depth stream");
197 sc.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap,
nullptr,
nullptr,
nullptr);
199 if (use_edges || use_klt) {
221 std::vector<int> trackerTypes;
222 if (use_edges && use_klt)
234 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
235 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
236 std::map<std::string, std::string> mapOfInitFiles;
237 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
238 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
239 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
241 std::vector<vpColVector> pointcloud;
245 if ((use_edges || use_klt) && use_depth) {
246 tracker.loadConfigFile(config_color, config_depth);
247 tracker.loadModel(model_color, model_depth);
248 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
249 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
250 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
251 mapOfImages[
"Camera1"] = &I_gray;
252 mapOfImages[
"Camera2"] = &I_depth;
253 mapOfInitFiles[
"Camera1"] = init_file;
254 tracker.setCameraParameters(cam_color, cam_depth);
256 else if (use_edges || use_klt) {
257 tracker.loadConfigFile(config_color);
258 tracker.loadModel(model_color);
259 tracker.setCameraParameters(cam_color);
261 else if (use_depth) {
262 tracker.loadConfigFile(config_depth);
263 tracker.loadModel(model_depth);
264 tracker.setCameraParameters(cam_depth);
267 tracker.setDisplayFeatures(
true);
268 tracker.setOgreVisibilityTest(use_ogre);
269 tracker.setScanLineVisibilityTest(use_scanline);
270 tracker.setProjectionErrorComputation(
true);
271 tracker.setProjectionErrorDisplay(display_projection_error);
273 #if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) || \
274 (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
275 std::string detectorName =
"SIFT";
276 std::string extractorName =
"SIFT";
277 std::string matcherName =
"BruteForce";
279 std::string detectorName =
"FAST";
280 std::string extractorName =
"ORB";
281 std::string matcherName =
"BruteForce-Hamming";
284 if (learn || auto_init) {
288 #if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
289 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
290 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
292 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
294 orb_detector->setNLevels(1);
302 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
308 if ((use_edges || use_klt) && use_depth)
309 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
310 else if (use_edges || use_klt)
311 tracker.initClick(I_gray, init_file,
true);
313 tracker.initClick(I_depth, init_file,
true);
319 bool run_auto_init =
false;
321 run_auto_init =
true;
323 std::vector<double> times_vec;
329 bool learn_position =
false;
335 bool tracking_failed =
false;
338 sc.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointcloud);
340 if (use_edges || use_klt || run_auto_init) {
349 if ((use_edges || use_klt) && use_depth) {
350 mapOfImages[
"Camera1"] = &I_gray;
351 mapOfPointclouds[
"Camera2"] = &pointcloud;
352 mapOfWidths[
"Camera2"] = width;
353 mapOfHeights[
"Camera2"] = height;
355 else if (use_edges || use_klt) {
356 mapOfImages[
"Camera"] = &I_gray;
358 else if (use_depth) {
359 mapOfPointclouds[
"Camera"] = &pointcloud;
360 mapOfWidths[
"Camera"] = width;
361 mapOfHeights[
"Camera"] = height;
366 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
367 std::cout <<
"Auto init succeed" << std::endl;
368 if ((use_edges || use_klt) && use_depth) {
369 mapOfCameraPoses[
"Camera1"] = cMo;
370 mapOfCameraPoses[
"Camera2"] = depth_M_color * cMo;
371 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
373 else if (use_edges || use_klt) {
374 tracker.initFromPose(I_gray, cMo);
376 else if (use_depth) {
377 tracker.initFromPose(I_depth, depth_M_color * cMo);
381 if (use_edges || use_klt) {
395 tracker.setDisplayFeatures(
true);
397 run_auto_init =
false;
399 if ((use_edges || use_klt) && use_depth) {
400 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
402 else if (use_edges || use_klt) {
403 tracker.track(I_gray);
405 else if (use_depth) {
406 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
411 tracking_failed =
true;
413 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
414 run_auto_init =
true;
419 cMo = tracker.getPose();
422 double proj_error = 0;
425 proj_error = tracker.getProjectionError();
428 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
431 if (auto_init && proj_error > proj_error_threshold) {
432 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
433 run_auto_init =
true;
434 tracking_failed =
true;
438 if (!tracking_failed) {
440 tracker.setDisplayFeatures(
true);
442 if ((use_edges || use_klt) && use_depth) {
443 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
447 else if (use_edges || use_klt) {
448 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
451 else if (use_depth) {
452 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
457 std::stringstream ss;
458 ss <<
"Nb features: " << tracker.getError().size();
462 std::stringstream ss;
463 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
464 <<
", depth " << tracker.getNbFeaturesDepthDense();
469 std::stringstream ss;
470 ss <<
"Loop time: " << loop_t <<
" ms";
473 if (use_edges || use_klt) {
489 learn_position =
true;
492 run_auto_init =
true;
506 if (learn_position) {
508 std::vector<cv::KeyPoint> trainKeyPoints;
509 keypoint.
detect(I_gray, trainKeyPoints);
512 std::vector<vpPolygon> polygons;
513 std::vector<std::vector<vpPoint> > roisPt;
514 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
515 polygons = pair.first;
516 roisPt = pair.second;
519 std::vector<cv::Point3f> points3f;
523 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
526 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
529 learn_position =
false;
530 std::cout <<
"Data learned" << std::endl;
533 times_vec.push_back(loop_t);
536 std::cout <<
"Save learning file: " << learning_data << std::endl;
541 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
544 if (!times_vec.empty()) {
552 #elif defined(VISP_HAVE_OCCIPITAL_STRUCTURE)
555 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
561 std::cout <<
"Install libStructure 3rd party, configure and build ViSP again to use this example" << std::endl;
Generic class defining intrinsic camera parameters.
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.
vpHomogeneousMatrix inverse() const
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.
unsigned int getHeight(vpOccipitalStructureStream stream_type)
vpCameraParameters getCameraParameters(const vpOccipitalStructureStream stream_type, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithoutDistortion)
void acquire(vpImage< unsigned char > &gray, bool undistorted=false, double *ts=nullptr)
unsigned int getWidth(vpOccipitalStructureStream stream_type)
vpHomogeneousMatrix getTransform(const vpOccipitalStructureStream from, const vpOccipitalStructureStream to)
bool open(const ST::CaptureSessionSettings &settings)
VISP_EXPORT double measureTimeMs()