Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
testKeyPoint-4.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 with mostly OpenCV functions
32  * calls to detect potential memory leaks in testKeyPoint-2.cpp.
33  */
34 
35 #include <iostream>
36 
37 #include <visp3/core/vpConfig.h>
38 
39 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_FEATURES2D)
40 
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/core/vpImage.h>
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/gui/vpDisplayGDI.h>
45 #include <visp3/gui/vpDisplayGTK.h>
46 #include <visp3/gui/vpDisplayOpenCV.h>
47 #include <visp3/gui/vpDisplayX.h>
48 #include <visp3/io/vpImageIo.h>
49 #include <visp3/io/vpParseArgv.h>
50 #include <visp3/io/vpVideoReader.h>
51 #include <visp3/mbt/vpMbEdgeTracker.h>
52 #include <visp3/vision/vpKeyPoint.h>
53 
54 // List of allowed command line options
55 #define GETOPTARGS "cdh"
56 
57 void usage(const char *name, const char *badparam);
58 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
59 
68 void usage(const char *name, const char *badparam)
69 {
70  fprintf(stdout, "\n\
71 Test keypoints matching.\n\
72 \n\
73 SYNOPSIS\n\
74  %s [-c] [-d] [-h]\n",
75  name);
76 
77  fprintf(stdout, "\n\
78 OPTIONS: \n\
79 \n\
80  -c\n\
81  Disable the mouse click. Useful to automate the \n\
82  execution of this program without human intervention.\n\
83 \n\
84  -d \n\
85  Turn off the display.\n\
86 \n\
87  -h\n\
88  Print the help.\n");
89 
90  if (badparam)
91  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
92 }
93 
105 bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
106 {
107  const char *optarg_;
108  int c;
109  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
110 
111  switch (c) {
112  case 'c':
113  click_allowed = false;
114  break;
115  case 'd':
116  display = false;
117  break;
118  case 'h':
119  usage(argv[0], nullptr);
120  return false;
121  break;
122 
123  default:
124  usage(argv[0], optarg_);
125  return false;
126  break;
127  }
128  }
129 
130  if ((c == 1) || (c == -1)) {
131  // standalone param or error
132  usage(argv[0], nullptr);
133  std::cerr << "ERROR: " << std::endl;
134  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
135  return false;
136  }
137 
138  return true;
139 }
140 
141 template <typename Type>
142 void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_display, vpImage<Type> &I,
143  vpImage<Type> &Imatch, vpImage<Type> &Iref)
144 {
145 #if VISP_HAVE_DATASET_VERSION >= 0x030600
146  std::string ext("png");
147 #else
148  std::string ext("pgm");
149 #endif
150  // Set the path location of the image sequence
151  std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube");
152 
153  // Build the name of the image files
154  std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000." + ext);
155  vpImageIo::read(I, filenameRef);
156  Iref = I;
157  std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d." + ext);
158 
159 #if defined(VISP_HAVE_X11)
160  vpDisplayX display, display2;
161 #elif defined(VISP_HAVE_GTK)
162  vpDisplayGTK display, display2;
163 #elif defined(VISP_HAVE_GDI)
164  vpDisplayGDI display, display2;
165 #elif defined(HAVE_OPENCV_HIGHGUI)
166  vpDisplayOpenCV display, display2;
167 #endif
168 
169  if (opt_display) {
170  display.setDownScalingFactor(vpDisplay::SCALE_AUTO);
171  display.init(I, 0, 0, "ORB keypoints matching");
172  Imatch.resize(I.getHeight(), 2 * I.getWidth());
173  Imatch.insert(I, vpImagePoint(0, 0));
175  display2.init(Imatch, 0, (int)I.getHeight() / vpDisplay::getDownScalingFactor(I) + 70, "ORB keypoints matching");
176  }
177 
178  vpCameraParameters cam;
179  vpMbEdgeTracker tracker;
180  // Load config for tracker
181  std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
182 
183 #if defined(VISP_HAVE_PUGYXML)
184  tracker.loadConfigFile(tracker_config_file);
185  tracker.getCameraParameters(cam);
186 #else
187  // Corresponding parameters manually set to have an example code
188  vpMe me;
189  me.setMaskSize(5);
190  me.setMaskNumber(180);
191  me.setRange(8);
193  me.setThreshold(20);
194  me.setMu1(0.5);
195  me.setMu2(0.5);
196  me.setSampleStep(4);
197  me.setNbTotalSample(250);
198  tracker.setMovingEdge(me);
199  cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
200  tracker.setCameraParameters(cam);
201  tracker.setNearClippingDistance(0.01);
202  tracker.setFarClippingDistance(100.0);
204 #endif
205 
206  tracker.setAngleAppear(vpMath::rad(89));
207  tracker.setAngleDisappear(vpMath::rad(89));
208 
209  // Load CAO model
210  std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.cao");
211  tracker.loadModel(cao_model_file);
212 
213  // Initialize the pose
214  std::string init_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.init");
215  if (opt_display && opt_click_allowed) {
216  tracker.initClick(I, init_file);
217  }
218  else {
219  vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
220  tracker.initFromPose(I, cMoi);
221  }
222 
223  // Get the init pose
225  tracker.getPose(cMo);
226 
227  // Init keypoints
228  cv::Ptr<cv::FeatureDetector> detector;
229  cv::Ptr<cv::DescriptorExtractor> extractor;
230  cv::Ptr<cv::DescriptorMatcher> matcher;
231 
232 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
233  detector = cv::ORB::create(500, 1.2f, 1);
234  extractor = cv::ORB::create(500, 1.2f, 1);
235 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020301)
236  detector = cv::FeatureDetector::create("ORB");
237  extractor = cv::DescriptorExtractor::create("ORB");
238 #endif
239  matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
240 
241 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
242  detector->set("nLevels", 1);
243 #endif
244 
245  // Detect keypoints on the current image
246  std::vector<cv::KeyPoint> trainKeyPoints;
247  cv::Mat matImg;
248  vpImageConvert::convert(I, matImg);
249  detector->detect(matImg, trainKeyPoints);
250 
251  // Keep only keypoints on the cube
252  std::vector<vpPolygon> polygons;
253  std::vector<std::vector<vpPoint> > roisPt;
254  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(false);
255  polygons = pair.first;
256  roisPt = pair.second;
257 
258  // Compute the 3D coordinates
259  std::vector<cv::Point3f> points3f;
260  vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
261 
262  // Extract descriptors
263  cv::Mat trainDescriptors;
264  extractor->compute(matImg, trainKeyPoints, trainDescriptors);
265 
266  if (trainKeyPoints.size() != (size_t)trainDescriptors.rows || trainKeyPoints.size() != points3f.size()) {
267  throw(vpException(vpException::fatalError, "Problem with training data size !"));
268  }
269 
270  // Init reader for getting the input image sequence
271  vpVideoReader g;
272  g.setFileName(filenameCur);
273  g.open(I);
274  g.acquire(I);
275 
276  bool opt_click = false;
278  while ((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) {
279  g.acquire(I);
280 
281  vpImageConvert::convert(I, matImg);
282  std::vector<cv::KeyPoint> queryKeyPoints;
283  detector->detect(matImg, queryKeyPoints);
284 
285  cv::Mat queryDescriptors;
286  extractor->compute(matImg, queryKeyPoints, queryDescriptors);
287 
288  std::vector<std::vector<cv::DMatch> > knn_matches;
289  std::vector<cv::DMatch> matches;
290  matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
291  for (std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end();
292  ++it) {
293  if (it->size() > 1) {
294  double ratio = (*it)[0].distance / (*it)[1].distance;
295  if (ratio < 0.85) {
296  matches.push_back((*it)[0]);
297  }
298  }
299  }
300 
301  vpPose estimated_pose;
302  for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
303  vpPoint pt(points3f[(size_t)(it->trainIdx)].x, points3f[(size_t)(it->trainIdx)].y,
304  points3f[(size_t)(it->trainIdx)].z);
305 
306  double x = 0.0, y = 0.0;
307  vpPixelMeterConversion::convertPoint(cam, queryKeyPoints[(size_t)(it->queryIdx)].pt.x,
308  queryKeyPoints[(size_t)(it->queryIdx)].pt.y, x, y);
309  pt.set_x(x);
310  pt.set_y(y);
311 
312  estimated_pose.addPoint(pt);
313  }
314 
315  bool is_pose_estimated = false;
316  if (estimated_pose.npt >= 4) {
317  try {
318  unsigned int nb_inliers = (unsigned int)(0.6 * estimated_pose.npt);
319  estimated_pose.setRansacNbInliersToReachConsensus(nb_inliers);
320  estimated_pose.setRansacThreshold(0.01);
321  estimated_pose.setRansacMaxTrials(500);
322  estimated_pose.computePose(vpPose::RANSAC, cMo);
323  is_pose_estimated = true;
324  }
325  catch (...) {
326  is_pose_estimated = false;
327  }
328  }
329 
330  if (opt_display) {
332 
333  Imatch.insert(I, vpImagePoint(0, Iref.getWidth()));
334  vpDisplay::display(Imatch);
335  for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
336  vpImagePoint leftPt(trainKeyPoints[(size_t)it->trainIdx].pt.y, trainKeyPoints[(size_t)it->trainIdx].pt.x);
337  vpImagePoint rightPt(queryKeyPoints[(size_t)it->queryIdx].pt.y,
338  queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth());
339  vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
340  }
341 
342  if (is_pose_estimated) {
343  tracker.setPose(I, cMo);
344  tracker.display(I, cMo, cam, vpColor::red);
345  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
346  }
347 
348  vpDisplay::flush(Imatch);
349  vpDisplay::flush(I);
350  }
351 
352  // Click requested to process next image
353  if (opt_click_allowed && opt_display) {
354  if (opt_click) {
355  vpDisplay::getClick(I, button, true);
356  if (button == vpMouseButton::button3) {
357  opt_click = false;
358  }
359  }
360  else {
361  // Use right click to enable/disable step by step tracking
362  if (vpDisplay::getClick(I, button, false)) {
363  if (button == vpMouseButton::button3) {
364  opt_click = true;
365  }
366  else if (button == vpMouseButton::button1) {
367  break;
368  }
369  }
370  }
371  }
372  }
373 }
374 
381 int main(int argc, const char **argv)
382 {
383  try {
384  std::string env_ipath;
385  bool opt_click_allowed = true;
386  bool opt_display = true;
387 
388  // Read the command line options
389  if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
390  return EXIT_FAILURE;
391  }
392 
393  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
394  // environment variable value
395  env_ipath = vpIoTools::getViSPImagesDataPath();
396 
397  if (env_ipath.empty()) {
398  std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
399  "variable value."
400  << std::endl;
401  return EXIT_FAILURE;
402  }
403 
404  {
405  vpImage<unsigned char> I, Imatch, Iref;
406 
407  std::cout << "-- Test on gray level images" << std::endl;
408  run_test(env_ipath, opt_click_allowed, opt_display, I, Imatch, Iref);
409  }
410 
411  {
412  vpImage<vpRGBa> I, Imatch, Iref;
413 
414  std::cout << "-- Test on color images" << std::endl;
415  run_test(env_ipath, opt_click_allowed, opt_display, I, Imatch, Iref);
416  }
417 
418  }
419  catch (const vpException &e) {
420  std::cerr << e.what() << std::endl;
421  return EXIT_FAILURE;
422  }
423 
424  std::cout << "testKeyPoint-4 is ok !" << std::endl;
425  return EXIT_SUCCESS;
426 }
427 
428 #else
429 int main()
430 {
431  std::cerr << "You need OpenCV library." << std::endl;
432 
433  return EXIT_SUCCESS;
434 }
435 
436 #endif
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
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...
Definition: vpDisplayX.h:128
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)
virtual void setDownScalingFactor(unsigned int scale)
Definition: vpDisplay.cpp:227
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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 flush(const vpImage< unsigned char > &I)
@ SCALE_AUTO
Definition: vpDisplay.h:179
unsigned int getDownScalingFactor()
Definition: vpDisplay.h:231
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
const char * what() const
Definition: vpException.cpp:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:143
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
Definition of the vpImage class member functions.
Definition: vpImage.h:69
unsigned int getWidth() const
Definition: vpImage.h:245
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:783
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1533
unsigned int getHeight() const
Definition: vpImage.h:184
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1781
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:2142
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)
Definition: vpKeyPoint.cpp:460
static double rad(double deg)
Definition: vpMath.h:127
Make the complete tracking of an object by using its CAD model.
virtual void setNearClippingDistance(const double &dist) vp_override
virtual void setFarClippingDistance(const double &dist) vp_override
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual void setClipping(const unsigned int &flags) vp_override
virtual void setCameraParameters(const vpCameraParameters &cam) vp_override
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
void setMovingEdge(const vpMe &me)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) vp_override
virtual void getCameraParameters(vpCameraParameters &cam) const
Definition: vpMbTracker.h:248
virtual void getPose(vpHomogeneousMatrix &cMo) const
Definition: vpMbTracker.h:414
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:481
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:470
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
Definition: vpMe.h:124
void setMu1(const double &mu_1)
Definition: vpMe.h:399
void setRange(const unsigned int &range)
Definition: vpMe.h:429
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition: vpMe.h:519
void setNbTotalSample(const int &ntotal_sample)
Definition: vpMe.h:413
void setMaskNumber(const unsigned int &mask_number)
Definition: vpMe.cpp:488
void setThreshold(const double &threshold)
Definition: vpMe.h:480
void setSampleStep(const double &sample_step)
Definition: vpMe.h:436
void setMaskSize(const unsigned int &mask_size)
Definition: vpMe.cpp:496
void setMu2(const double &mu_2)
Definition: vpMe.h:406
@ NORMALIZED_THRESHOLD
Definition: vpMe.h:135
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:404
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:93
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:385
@ RANSAC
Definition: vpPose.h:88
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:114
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:333
void setRansacThreshold(const double &t)
Definition: vpPose.h:390
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
long getFrameIndex() const
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.