Visual Servoing Platform  version 3.0.1
testKeyPoint-2.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test keypoint matching and pose estimation.
32  *
33  * Authors:
34  * Souriya Trinh
35  *
36  *****************************************************************************/
37 
38 #include <iostream>
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
43 
44 #include <visp3/vision/vpKeyPoint.h>
45 #include <visp3/core/vpImage.h>
46 #include <visp3/io/vpImageIo.h>
47 #include <visp3/gui/vpDisplayX.h>
48 #include <visp3/gui/vpDisplayGTK.h>
49 #include <visp3/gui/vpDisplayGDI.h>
50 #include <visp3/gui/vpDisplayOpenCV.h>
51 #include <visp3/io/vpVideoReader.h>
52 #include <visp3/core/vpIoTools.h>
53 #include <visp3/mbt/vpMbEdgeTracker.h>
54 #include <visp3/io/vpParseArgv.h>
55 
56 // List of allowed command line options
57 #define GETOPTARGS "cdh"
58 
59 void usage(const char *name, const char *badparam);
60 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
61 
70 void usage(const char *name, const char *badparam)
71 {
72  fprintf(stdout, "\n\
73 Test keypoints matching.\n\
74 \n\
75 SYNOPSIS\n\
76  %s [-c] [-d] [-h]\n", name);
77 
78  fprintf(stdout, "\n\
79 OPTIONS: \n\
80 \n\
81  -c\n\
82  Disable the mouse click. Useful to automaze the \n\
83  execution of this program without humain intervention.\n\
84 \n\
85  -d \n\
86  Turn off the display.\n\
87 \n\
88  -h\n\
89  Print the help.\n");
90 
91  if (badparam)
92  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
93 }
94 
106 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
107 {
108  const char *optarg_;
109  int c;
110  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
111 
112  switch (c) {
113  case 'c': click_allowed = false; break;
114  case 'd': display = false; break;
115  case 'h': usage(argv[0], NULL); return false; break;
116 
117  default:
118  usage(argv[0], optarg_);
119  return false; break;
120  }
121  }
122 
123  if ((c == 1) || (c == -1)) {
124  // standalone param or error
125  usage(argv[0], NULL);
126  std::cerr << "ERROR: " << std::endl;
127  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
128  return false;
129  }
130 
131  return true;
132 }
133 
139 int main(int argc, const char ** argv) {
140  try {
141  std::string env_ipath;
142  bool opt_click_allowed = true;
143  bool opt_display = true;
144 
145  // Read the command line options
146  if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
147  exit (-1);
148  }
149 
150  //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
151  env_ipath = vpIoTools::getViSPImagesDataPath();
152 
153  if(env_ipath.empty()) {
154  std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
155  return -1;
156  }
157 
159 
160  //Set the path location of the image sequence
161  std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
162 
163  //Build the name of the image files
164  std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
165  vpImageIo::read(I, filenameRef);
166  std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
167 
168 #if defined VISP_HAVE_X11
169  vpDisplayX display;
170 #elif defined VISP_HAVE_GTK
171  vpDisplayGTK display;
172 #elif defined VISP_HAVE_GDI
173  vpDisplayGDI display;
174 #else
175  vpDisplayOpenCV display;
176 #endif
177 
178  if (opt_display) {
180  display.init(I, 0, 0, "ORB keypoints matching and pose estimation");
181  }
182 
183  vpCameraParameters cam;
184  vpMbEdgeTracker tracker;
185  //Load config for tracker
186  std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
187 
188  bool usexml = false;
189 #ifdef VISP_HAVE_XML2
190  tracker.loadConfigFile(tracker_config_file);
191  tracker.getCameraParameters(cam);
192 
193  usexml = true;
194 #endif
195  if (! usexml) {
196  vpMe me;
197  me.setMaskSize(5);
198  me.setMaskNumber(180);
199  me.setRange(8);
200  me.setThreshold(10000);
201  me.setMu1(0.5);
202  me.setMu2(0.5);
203  me.setSampleStep(4);
204  me.setNbTotalSample(250);
205  tracker.setMovingEdge(me);
206  cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
207  tracker.setCameraParameters(cam);
208  tracker.setNearClippingDistance(0.01);
209  tracker.setFarClippingDistance(100.0);
211  }
212 
213  tracker.setAngleAppear(vpMath::rad(89));
214  tracker.setAngleDisappear(vpMath::rad(89));
215 
216  //Load CAO model
217  std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.cao");
218  tracker.loadModel(cao_model_file);
219 
220  //Initialize the pose
221  std::string init_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.init");
222  if (opt_display && opt_click_allowed) {
223  tracker.initClick(I, init_file);
224  }
225  else
226  {
227  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
228  tracker.initFromPose(I, cMoi);
229  }
230 
231  //Get the init pose
233  tracker.getPose(cMo);
234 
235  //Init keypoints
236  vpKeyPoint keypoints("ORB", "ORB", "BruteForce-Hamming");
237 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
238  //Bug when using LSH index with FLANN and OpenCV 2.3.1.
239  //see http://code.opencv.org/issues/1741 (Bug #1741)
240  keypoints.setMatcher("FlannBased");
241 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
242  keypoints.setDetectorParameter("ORB", "nLevels", 1);
243 #else
244  cv::Ptr<cv::ORB> orb_detector = keypoints.getDetector("ORB").dynamicCast<cv::ORB>();
245  if(orb_detector != NULL) {
246  orb_detector->setNLevels(1);
247  }
248 #endif
249 #endif
250 
251  //Detect keypoints on the current image
252  std::vector<cv::KeyPoint> trainKeyPoints;
253  double elapsedTime;
254  keypoints.detect(I, trainKeyPoints, elapsedTime);
255 
256  //Keep only keypoints on the cube
257  std::vector<vpPolygon> polygons;
258  std::vector<std::vector<vpPoint> > roisPt;
259  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(true); //To detect an issue with CI
260  polygons = pair.first;
261  roisPt = pair.second;
262 
263  //Compute the 3D coordinates
264  std::vector<cv::Point3f> points3f;
265  vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
266 
267  //Build the reference keypoints
268  keypoints.buildReference(I, trainKeyPoints, points3f, false, 1);
269 
270 
271  //Read image 150
272  filenameRef = vpIoTools::createFilePath(dirname, "image0150.pgm");
273  vpImageIo::read(I, filenameRef);
274 
275  //Init pose at image 150
276  cMo.buildFrom(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828);
277  tracker.initFromPose(I, cMo);
278 
279  //Detect keypoints on the image 150
280  keypoints.detect(I, trainKeyPoints, elapsedTime);
281 
282  //Keep only keypoints on the cube
283  pair = tracker.getPolygonFaces(true, true, true); //To detect an issue with CI
284  polygons = pair.first;
285  roisPt = pair.second;
286 
287  //Compute the 3D coordinates
288  vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
289 
290  //Build the reference keypoints
291  keypoints.buildReference(I, trainKeyPoints, points3f, true, 2);
292 
293 
294  //Read image 200
295  filenameRef = vpIoTools::createFilePath(dirname, "image0200.pgm");
296  vpImageIo::read(I, filenameRef);
297 
298  //Init pose at image 200
299  cMo.buildFrom(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025);
300  tracker.initFromPose(I, cMo);
301 
302  //Detect keypoints on the image 200
303  keypoints.detect(I, trainKeyPoints, elapsedTime);
304 
305  //Keep only keypoints on the cube
306  pair = tracker.getPolygonFaces(false); //To detect an issue with CI
307  polygons = pair.first;
308  roisPt = pair.second;
309 
310  //Compute the 3D coordinates
311  vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
312 
313  //Build the reference keypoints
314  keypoints.buildReference(I, trainKeyPoints, points3f, true, 3);
315 
316 
317  //Init reader for getting the input image sequence
318  vpVideoReader g;
319  g.setFileName(filenameCur);
320  g.open(I);
321  g.acquire(I);
322 
323 #if defined VISP_HAVE_X11
324  vpDisplayX display2;
325 #elif defined VISP_HAVE_GTK
326  vpDisplayGTK display2;
327 #elif defined VISP_HAVE_GDI
328  vpDisplayGDI display2;
329 #else
330  vpDisplayOpenCV display2;
331 #endif
332 
333  vpImage<unsigned char> IMatching;
334 
335  keypoints.createImageMatching(I, IMatching);
336 
337  if (opt_display) {
339  display2.init(IMatching, 0, (int)I.getHeight()/vpDisplay::getDownScalingFactor(I) + 80, "IMatching");
340  }
341 
342  bool opt_click = false;
343  double error;
345  while((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) {
346  g.acquire(I);
347 
348  if(opt_display) {
350 
351  //Display image matching
352  keypoints.insertImageMatching(I, IMatching);
353 
354  vpDisplay::display(IMatching);
355  }
356 
357  //Match keypoints and estimate the pose
358  if(keypoints.matchPoint(I, cam, cMo, error, elapsedTime)) {
359  tracker.setCameraParameters(cam);
360  tracker.setPose(I, cMo);
361 
362  if(opt_display) {
363  tracker.display(I, cMo, cam, vpColor::red, 2);
364  vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
365 
366  std::vector<vpImagePoint> ransacInliers = keypoints.getRansacInliers();
367  std::vector<vpImagePoint> ransacOutliers = keypoints.getRansacOutliers();
368 
369  for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
371  vpImagePoint imPt(*it);
372  imPt.set_u(imPt.get_u() + I.getWidth());
373  imPt.set_v(imPt.get_v() + I.getHeight());
374  vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::green);
375  }
376 
377  for(std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
379  vpImagePoint imPt(*it);
380  imPt.set_u(imPt.get_u() + I.getWidth());
381  imPt.set_v(imPt.get_v() + I.getHeight());
382  vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::red);
383  }
384 
385  keypoints.displayMatching(I, IMatching);
386 
387  //Display model in the correct sub-image in IMatching
388  vpCameraParameters cam2;
389  cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(),
390  cam.get_u0() + I.getWidth(), cam.get_v0() + I.getHeight());
391  tracker.setCameraParameters(cam2);
392  tracker.setPose(IMatching, cMo);
393  tracker.display(IMatching, cMo, cam2, vpColor::red, 2);
394  vpDisplay::displayFrame(IMatching, cMo, cam2, 0.025, vpColor::none, 3);
395  }
396  }
397 
398  if(opt_display) {
399  vpDisplay::flush(I);
400  vpDisplay::flush(IMatching);
401  }
402 
403  if (opt_click_allowed && opt_display) {
404  //Click requested to process next image
405  if(opt_click) {
406  vpDisplay::getClick(I, button, true);
407  if(button == vpMouseButton::button3) {
408  opt_click = false;
409  }
410  } else {
411  //Use right click to enable/disable step by step tracking
412  if(vpDisplay::getClick(I, button, false)) {
413  if (button == vpMouseButton::button3) {
414  opt_click = true;
415  }
416  else if(button == vpMouseButton::button1) {
417  break;
418  }
419  }
420  }
421  }
422  }
423 
424  } catch(vpException &e) {
425  std::cerr << e.what() << std::endl;
426  return -1;
427  }
428 
429  std::cout << "testKeyPoint-2 is ok !" << std::endl;
430  return 0;
431 }
432 #else
433 int main() {
434  std::cerr << "You need OpenCV library." << std::endl;
435 
436  return 0;
437 }
438 
439 #endif
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:208
void setMovingEdge(const vpMe &me)
virtual void getPose(vpHomogeneousMatrix &cMo_) const
Definition: vpMbTracker.h:333
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1157
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:382
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:488
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
void setSampleStep(const double &s)
Definition: vpMe.h:263
void setNbTotalSample(const int &nb)
Definition: vpMe.h:240
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
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Definition: vpMe.h:59
Make the complete tracking of an object by using its CAD model.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void loadConfigFile(const std::string &configFile)
static const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
void setMu1(const double &mu_1)
Definition: vpMe.h:226
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
static const vpColor red
Definition: vpColor.h:163
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
virtual void setNearClippingDistance(const double &dist)
void open(vpImage< vpRGBa > &I)
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)
Definition: vpKeyPoint.cpp:765
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:496
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1366
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.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void acquire(vpImage< vpRGBa > &I)
virtual void setFarClippingDistance(const double &dist)
void setFileName(const char *filename)
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:371
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
const char * what() const
static double rad(double deg)
Definition: vpMath.h:104
static void displayCircle(const vpImage< unsigned char > &I, const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)
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))
void setMu2(const double &mu_2)
Definition: vpMe.h:233
long getFrameIndex() const
virtual void loadModel(const char *modelFile, const bool verbose=false)
unsigned int getHeight() const
Definition: vpImage.h:175
virtual void getCameraParameters(vpCameraParameters &camera) const
Definition: vpMbTracker.h:201
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:205
Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV libr...
Definition: vpKeyPoint.h:217
unsigned int getDownScalingFactor()
Definition: vpDisplay.h:214
void setThreshold(const double &t)
Definition: vpMe.h:284
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void setRange(const unsigned int &r)
Definition: vpMe.h:256
virtual void setClipping(const unsigned int &flags)
unsigned int getWidth() const
Definition: vpImage.h:226
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(const bool orderPolygons=true, const bool useVisibility=true, const bool clipPolygon=false)