Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tutorial-mb-tracker-stereo.cpp
1 #include <visp3/gui/vpDisplayGDI.h>
3 #include <visp3/gui/vpDisplayOpenCV.h>
4 #include <visp3/gui/vpDisplayX.h>
5 #include <visp3/io/vpImageIo.h>
6 #include <visp3/core/vpIoTools.h>
8 #include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
10 #include <visp3/io/vpVideoReader.h>
11 
12 
13 int main(int argc, char** argv)
14 {
15 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
16  try {
17  std::string opt_videoname_left = "teabox_left.mpg";
18  std::string opt_videoname_right = "teabox_right.mpg";
19  int opt_tracker = 0;
20 
21  for (int i=0; i<argc; i++) {
22  if (std::string(argv[i]) == "--name" && i+2 < argc) {
23  opt_videoname_left = std::string(argv[i+1]);
24  opt_videoname_right = std::string(argv[i+2]);
25  } else if (std::string(argv[i]) == "--tracker" && i+1 < argc) {
26  opt_tracker = atoi(argv[i+1]);
27  } else if (std::string(argv[i]) == "--help") {
28  std::cout << "\nUsage: " << argv[0] << " [--name <video name left> <video name right>] "
29  "[--tracker <0=egde|1=klt|2=hybrid>] [--help]\n" << std::endl;
30  return 0;
31  }
32  }
33  std::string parentname = vpIoTools::getParent(opt_videoname_left);
34  std::string objectname_left = vpIoTools::getNameWE(opt_videoname_left);
35  std::string objectname_right = vpIoTools::getNameWE(opt_videoname_right);
36 
37  if(! parentname.empty()) {
38  objectname_left = parentname + "/" + objectname_left;
39  }
40 
41  std::cout << "Video name: " << opt_videoname_left << " ; " << opt_videoname_right << std::endl;
42  std::cout << "Tracker requested config files: " << objectname_left
43  << ".[init, cao]" << " and " << objectname_right
44  << ".[init, cao]" << std::endl;
45  std::cout << "Tracker optional config files: " << opt_videoname_left << ".ppm"
46  << " and " << opt_videoname_right << ".ppm" << std::endl;
47 
49  vpImage<unsigned char> I_left, I_right;
51 
52  vpVideoReader g_left, g_right;
53  g_left.setFileName(opt_videoname_left);
54  g_left.open(I_left);
55  g_right.setFileName(opt_videoname_right);
56  g_right.open(I_right);
57 
58  vpDisplay *display_left = NULL, *display_right = NULL;
59 #if defined(VISP_HAVE_X11)
60  display_left = new vpDisplayX;
61  display_right = new vpDisplayX;
62 #elif defined(VISP_HAVE_GDI)
63  display_left = new vpDisplayGDI;
64  display_right = new vpDisplayGDI;
65 #else
66  display_left = new vpDisplayOpenCV;
67  display_right = new vpDisplayOpenCV;
68 #endif
70  display_right->setDownScalingFactor(vpDisplay::SCALE_AUTO);
71  display_left->init(I_left, 100, 100, "Model-based tracker (Left)");
72  display_right->init(I_right, 110 + (int) I_left.getWidth(), 100, "Model-based tracker (Right)");
73 
75  vpMbTracker *tracker;
76  if (opt_tracker == 0)
77  tracker = new vpMbEdgeMultiTracker(2);
78 #ifdef VISP_HAVE_MODULE_KLT
79  else if (opt_tracker == 1)
80  tracker = new vpMbKltMultiTracker(2);
81  else
82  tracker = new vpMbEdgeKltMultiTracker(2);
83 #else
84  else {
85  std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
86  return 0;
87  }
88 #endif
89 
92  if(opt_tracker == 0)
93  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
94  else if(opt_tracker == 1)
95  dynamic_cast<vpMbKltMultiTracker*> (tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
96  else
97  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
99 
101  vpCameraParameters cam_left, cam_right;
102  if(opt_tracker == 0)
103  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->getCameraParameters(cam_left, cam_right);
104  else if(opt_tracker == 1)
105  dynamic_cast<vpMbKltMultiTracker*> (tracker)->getCameraParameters(cam_left, cam_right);
106  else
107  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->getCameraParameters(cam_left, cam_right);
109 
111  tracker->loadModel("teabox.cao");
114  tracker->setDisplayFeatures(true);
116 
117  vpHomogeneousMatrix cRightMcLeft;
118  std::ifstream file_cRightMcLeft("cRightMcLeft.txt");
119  cRightMcLeft.load(file_cRightMcLeft);
120 
122  std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrix;
123  mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
124  mapOfCameraTransformationMatrix["Camera2"] = cRightMcLeft;
125 
126  if(opt_tracker == 0)
127  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
128  else if(opt_tracker == 1)
129  dynamic_cast<vpMbKltMultiTracker*> (tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
130  else
131  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
133 
134 #ifndef VISP_HAVE_XML2
135  std::cout << "\n**********************************************************\n"
136  << "Warning: we are not able to load the tracker settings from\n"
137  << "the xml config files since ViSP is not build with libxml2\n"
138  << "3rd party. As a consequence, the tracking may fail !"
139  << "\n**********************************************************\n"
140  << std::endl;
141 #endif
142 
144  if(opt_tracker == 0)
145  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
146  else if(opt_tracker == 1)
147  dynamic_cast<vpMbKltMultiTracker*> (tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
148  else
149  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
151 
153  vpHomogeneousMatrix cLeftMo, cRightMo;
155  while(!g_left.end() && !g_right.end()) {
156  g_left.acquire(I_left);
157  g_right.acquire(I_right);
158 
159  vpDisplay::display(I_left);
160  vpDisplay::display(I_right);
161 
163  if(opt_tracker == 0)
164  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->track(I_left, I_right);
165  else if(opt_tracker == 1)
166  dynamic_cast<vpMbKltMultiTracker*> (tracker)->track(I_left, I_right);
167  else
168  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->track(I_left, I_right);
170 
172  if(opt_tracker == 0)
173  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->getPose(cLeftMo, cRightMo);
174  else if(opt_tracker == 1)
175  dynamic_cast<vpMbKltMultiTracker*> (tracker)->getPose(cLeftMo, cRightMo);
176  else
177  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->getPose(cLeftMo, cRightMo);
179 
181  if(opt_tracker == 0)
182  dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
183  else if(opt_tracker == 1)
184  dynamic_cast<vpMbKltMultiTracker*> (tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
185  else
186  dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
188 
189  vpDisplay::displayFrame(I_left, cLeftMo, cam_left, 0.025, vpColor::none, 3);
190  vpDisplay::displayFrame(I_right, cRightMo, cam_right, 0.025, vpColor::none, 3);
191  vpDisplay::displayText(I_left, 10, 10, "A click to exit...", vpColor::red);
192 
193  vpDisplay::flush(I_left);
194  vpDisplay::flush(I_right);
195 
196  if (vpDisplay::getClick(I_left, false)) {
197  break;
198  }
199  }
200  vpDisplay::getClick(I_left);
201 
203  delete display_left;
204  delete display_right;
205  delete tracker;
207  }
208  catch(vpException &e) {
209  std::cerr << "Catch a ViSP exception: " << e.getMessage() << std::endl;
210  }
211 #else
212  (void)argc;
213  (void)argv;
214  std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
215 #endif
216 }
virtual void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:411
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:169
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
unsigned int getWidth() const
Definition: vpImage.h:226
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void setDownScalingFactor(unsigned int scale)
Definition: vpDisplay.cpp:248
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
Hybrid stereo (or more) tracker based on moving-edges and keypoints tracked using KLT tracker...
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1306
static void flush(const vpImage< unsigned char > &I)
void load(std::ifstream &f)
static const vpColor red
Definition: vpColor.h:163
void open(vpImage< vpRGBa > &I)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:103
void acquire(vpImage< vpRGBa > &I)
const char * getMessage(void) const
Definition: vpException.cpp:97
void setFileName(const char *filename)
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, vpImagePoint offset=vpImagePoint(0, 0))
virtual void loadModel(const char *modelFile, const bool verbose=false)
static std::string getNameWE(const std::string &pathname)
Definition: vpIoTools.cpp:1293
Make the complete stereo (or more) tracking of an object by using its CAD model.
Model based stereo (or more) tracker using only KLT.