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