4 #include <visp3/core/vpConfig.h>
6 #if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && 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/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]);
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 ST::CaptureSessionSettings settings;
134 settings.source = ST::CaptureSessionSourceId::StructureCore;
135 settings.structureCore.visibleEnabled =
true;
136 settings.applyExpensiveCorrection =
true;
142 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
143 std::cout <<
"Check if the Structure Core 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;
158 unsigned int _posx = 100, _posy = 50;
162 #elif defined(VISP_HAVE_GDI)
164 #elif defined(VISP_HAVE_OPENCV)
167 if (use_edges || use_klt)
168 d1.
init(I_gray, _posx, _posy,
"Color stream");
170 d2.
init(I_depth, _posx + I_gray.getWidth()+10, _posy,
"Depth stream");
173 sc.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, NULL, NULL);
175 if (use_edges || use_klt) {
197 std::vector<int> trackerTypes;
198 if (use_edges && use_klt)
210 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
211 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
212 std::map<std::string, std::string> mapOfInitFiles;
213 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
214 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
215 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
217 std::vector<vpColVector> pointcloud;
221 if ((use_edges || use_klt) && use_depth) {
222 tracker.loadConfigFile(config_color, config_depth);
223 tracker.loadModel(model_color, model_depth);
224 std::cout <<
"Sensor internal depth_M_color: \n" << depth_M_color << std::endl;
225 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
226 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
227 mapOfImages[
"Camera1"] = &I_gray;
228 mapOfImages[
"Camera2"] = &I_depth;
229 mapOfInitFiles[
"Camera1"] = init_file;
230 tracker.setCameraParameters(cam_color, cam_depth);
232 else if (use_edges || use_klt) {
233 tracker.loadConfigFile(config_color);
234 tracker.loadModel(model_color);
235 tracker.setCameraParameters(cam_color);
237 else if (use_depth) {
238 tracker.loadConfigFile(config_depth);
239 tracker.loadModel(model_depth);
240 tracker.setCameraParameters(cam_depth);
243 tracker.setDisplayFeatures(
true);
244 tracker.setOgreVisibilityTest(use_ogre);
245 tracker.setScanLineVisibilityTest(use_scanline);
246 tracker.setProjectionErrorComputation(
true);
247 tracker.setProjectionErrorDisplay(display_projection_error);
249 #if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) || \
250 (VISP_HAVE_OPENCV_VERSION >= 0x030411 && CV_MAJOR_VERSION < 4) || (VISP_HAVE_OPENCV_VERSION >= 0x040400)
251 std::string detectorName =
"SIFT";
252 std::string extractorName =
"SIFT";
253 std::string matcherName =
"BruteForce";
255 std::string detectorName =
"FAST";
256 std::string extractorName =
"ORB";
257 std::string matcherName =
"BruteForce-Hamming";
260 if (learn || auto_init) {
264 #if !(defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
265 # if (VISP_HAVE_OPENCV_VERSION < 0x030000)
266 keypoint.setDetectorParameter(
"ORB",
"nLevels", 1);
268 cv::Ptr<cv::ORB> orb_detector = keypoint.
getDetector(
"ORB").dynamicCast<cv::ORB>();
270 orb_detector->setNLevels(1);
278 std::cout <<
"Cannot enable auto detection. Learning file \"" << learning_data <<
"\" doesn't exist" << std::endl;
283 if ((use_edges || use_klt) && use_depth)
284 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
285 else if (use_edges || use_klt)
286 tracker.initClick(I_gray, init_file,
true);
288 tracker.initClick(I_depth, init_file,
true);
295 bool run_auto_init =
false;
297 run_auto_init =
true;
299 std::vector<double> times_vec;
305 bool learn_position =
false;
311 bool tracking_failed =
false;
314 sc.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointcloud);
316 if (use_edges || use_klt || run_auto_init) {
325 if ((use_edges || use_klt) && use_depth) {
326 mapOfImages[
"Camera1"] = &I_gray;
327 mapOfPointclouds[
"Camera2"] = &pointcloud;
328 mapOfWidths[
"Camera2"] = width;
329 mapOfHeights[
"Camera2"] = height;
330 }
else if (use_edges || use_klt) {
331 mapOfImages[
"Camera"] = &I_gray;
332 }
else if (use_depth) {
333 mapOfPointclouds[
"Camera"] = &pointcloud;
334 mapOfWidths[
"Camera"] = width;
335 mapOfHeights[
"Camera"] = height;
340 if (keypoint.
matchPoint(I_gray, cam_color, cMo)) {
341 std::cout <<
"Auto init succeed" << std::endl;
342 if ((use_edges || use_klt) && use_depth) {
343 mapOfCameraPoses[
"Camera1"] = cMo;
344 mapOfCameraPoses[
"Camera2"] = depth_M_color *cMo;
345 tracker.initFromPose(mapOfImages, mapOfCameraPoses);
346 }
else if (use_edges || use_klt) {
347 tracker.initFromPose(I_gray, cMo);
348 }
else if (use_depth) {
349 tracker.initFromPose(I_depth, depth_M_color*cMo);
352 if (use_edges || use_klt) {
366 tracker.setDisplayFeatures(
true);
368 run_auto_init =
false;
370 if ((use_edges || use_klt) && use_depth) {
371 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
372 }
else if (use_edges || use_klt) {
373 tracker.track(I_gray);
374 }
else if (use_depth) {
375 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
379 tracking_failed =
true;
381 std::cout <<
"Tracker needs to restart (tracking exception)" << std::endl;
382 run_auto_init =
true;
387 cMo = tracker.getPose();
390 double proj_error = 0;
393 proj_error = tracker.getProjectionError();
396 proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color);
399 if (auto_init && proj_error > proj_error_threshold) {
400 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
401 run_auto_init =
true;
402 tracking_failed =
true;
406 if (!tracking_failed) {
408 tracker.setDisplayFeatures(
true);
410 if ((use_edges || use_klt) && use_depth) {
411 tracker.display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
414 }
else if (use_edges || use_klt) {
415 tracker.display(I_gray, cMo, cam_color,
vpColor::red, 3);
417 }
else if (use_depth) {
418 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
423 std::stringstream ss;
424 ss <<
"Nb features: " << tracker.getError().size();
428 std::stringstream ss;
429 ss <<
"Features: edges " << tracker.getNbFeaturesEdge()
430 <<
", klt " << tracker.getNbFeaturesKlt()
431 <<
", depth " << tracker.getNbFeaturesDepthDense();
436 std::stringstream ss;
437 ss <<
"Loop time: " << loop_t <<
" ms";
440 if (use_edges || use_klt) {
455 learn_position =
true;
457 run_auto_init =
true;
471 if (learn_position) {
473 std::vector<cv::KeyPoint> trainKeyPoints;
474 keypoint.
detect(I_gray, trainKeyPoints);
477 std::vector<vpPolygon> polygons;
478 std::vector<std::vector<vpPoint> > roisPt;
479 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
480 polygons = pair.first;
481 roisPt = pair.second;
484 std::vector<cv::Point3f> points3f;
488 keypoint.
buildReference(I_gray, trainKeyPoints, points3f,
true, learn_id++);
491 for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
494 learn_position =
false;
495 std::cout <<
"Data learned" << std::endl;
498 times_vec.push_back(loop_t);
501 std::cout <<
"Save learning file: " << learning_data << std::endl;
505 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
508 if (!times_vec.empty()) {
515 #elif defined(VISP_HAVE_OCCIPITAL_STRUCTURE)
517 std::cout <<
"Install OpenCV 3rd party, configure and build ViSP again to use this example" << std::endl;
522 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="")
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.
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())
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.
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=NULL)
unsigned int getWidth(vpOccipitalStructureStream stream_type)
vpHomogeneousMatrix getTransform(const vpOccipitalStructureStream from, const vpOccipitalStructureStream to)
bool open(const ST::CaptureSessionSettings &settings)
VISP_EXPORT double measureTimeMs()