Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
57void usage(const char *name, const char *badparam);
58bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
59
68void usage(const char *name, const char *badparam)
69{
70 fprintf(stdout, "\n\
71Test keypoints matching.\n\
72\n\
73SYNOPSIS\n\
74 %s [-c] [-d] [-h]\n",
75 name);
76
77 fprintf(stdout, "\n\
78OPTIONS: \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
105bool 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], NULL);
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], NULL);
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
141template <typename Type>
142void 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
179 vpMbEdgeTracker tracker;
180 // Load config for tracker
181 std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
182
183 tracker.loadConfigFile(tracker_config_file);
184 tracker.getCameraParameters(cam);
185#if 0
186 // Corresponding parameters manually set to have an example code
187 vpMe me;
188 me.setMaskSize(5);
189 me.setMaskNumber(180);
190 me.setRange(8);
192 me.setThreshold(20);
193 me.setMu1(0.5);
194 me.setMu2(0.5);
195 me.setSampleStep(4);
196 me.setNbTotalSample(250);
197 tracker.setMovingEdge(me);
198 cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
199 tracker.setCameraParameters(cam);
200 tracker.setNearClippingDistance(0.01);
201 tracker.setFarClippingDistance(100.0);
203#endif
204
205 tracker.setAngleAppear(vpMath::rad(89));
206 tracker.setAngleDisappear(vpMath::rad(89));
207
208 // Load CAO model
209 std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.cao");
210 tracker.loadModel(cao_model_file);
211
212 // Initialize the pose
213 std::string init_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.init");
214 if (opt_display && opt_click_allowed) {
215 tracker.initClick(I, init_file);
216 }
217 else {
218 vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
219 tracker.initFromPose(I, cMoi);
220 }
221
222 // Get the init pose
224 tracker.getPose(cMo);
225
226 // Init keypoints
227 cv::Ptr<cv::FeatureDetector> detector;
228 cv::Ptr<cv::DescriptorExtractor> extractor;
229 cv::Ptr<cv::DescriptorMatcher> matcher;
230
231#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
232 detector = cv::ORB::create(500, 1.2f, 1);
233 extractor = cv::ORB::create(500, 1.2f, 1);
234#elif (VISP_HAVE_OPENCV_VERSION >= 0x020301)
235 detector = cv::FeatureDetector::create("ORB");
236 extractor = cv::DescriptorExtractor::create("ORB");
237#endif
238 matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
239
240#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
241 detector->set("nLevels", 1);
242#endif
243
244 // Detect keypoints on the current image
245 std::vector<cv::KeyPoint> trainKeyPoints;
246 cv::Mat matImg;
247 vpImageConvert::convert(I, matImg);
248 detector->detect(matImg, trainKeyPoints);
249
250 // Keep only keypoints on the cube
251 std::vector<vpPolygon> polygons;
252 std::vector<std::vector<vpPoint> > roisPt;
253 std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(false);
254 polygons = pair.first;
255 roisPt = pair.second;
256
257 // Compute the 3D coordinates
258 std::vector<cv::Point3f> points3f;
259 vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
260
261 // Extract descriptors
262 cv::Mat trainDescriptors;
263 extractor->compute(matImg, trainKeyPoints, trainDescriptors);
264
265 if (trainKeyPoints.size() != (size_t)trainDescriptors.rows || trainKeyPoints.size() != points3f.size()) {
266 throw(vpException(vpException::fatalError, "Problem with training data size !"));
267 }
268
269 // Init reader for getting the input image sequence
271 g.setFileName(filenameCur);
272 g.open(I);
273 g.acquire(I);
274
275 bool opt_click = false;
277 while ((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) {
278 g.acquire(I);
279
280 vpImageConvert::convert(I, matImg);
281 std::vector<cv::KeyPoint> queryKeyPoints;
282 detector->detect(matImg, queryKeyPoints);
283
284 cv::Mat queryDescriptors;
285 extractor->compute(matImg, queryKeyPoints, queryDescriptors);
286
287 std::vector<std::vector<cv::DMatch> > knn_matches;
288 std::vector<cv::DMatch> matches;
289 matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
290 for (std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end();
291 ++it) {
292 if (it->size() > 1) {
293 double ratio = (*it)[0].distance / (*it)[1].distance;
294 if (ratio < 0.85) {
295 matches.push_back((*it)[0]);
296 }
297 }
298 }
299
300 vpPose estimated_pose;
301 for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
302 vpPoint pt(points3f[(size_t)(it->trainIdx)].x, points3f[(size_t)(it->trainIdx)].y,
303 points3f[(size_t)(it->trainIdx)].z);
304
305 double x = 0.0, y = 0.0;
306 vpPixelMeterConversion::convertPoint(cam, queryKeyPoints[(size_t)(it->queryIdx)].pt.x,
307 queryKeyPoints[(size_t)(it->queryIdx)].pt.y, x, y);
308 pt.set_x(x);
309 pt.set_y(y);
310
311 estimated_pose.addPoint(pt);
312 }
313
314 bool is_pose_estimated = false;
315 if (estimated_pose.npt >= 4) {
316 try {
317 unsigned int nb_inliers = (unsigned int)(0.6 * estimated_pose.npt);
318 estimated_pose.setRansacNbInliersToReachConsensus(nb_inliers);
319 estimated_pose.setRansacThreshold(0.01);
320 estimated_pose.setRansacMaxTrials(500);
321 estimated_pose.computePose(vpPose::RANSAC, cMo);
322 is_pose_estimated = true;
323 }
324 catch (...) {
325 is_pose_estimated = false;
326 }
327 }
328
329 if (opt_display) {
331
332 Imatch.insert(I, vpImagePoint(0, Iref.getWidth()));
333 vpDisplay::display(Imatch);
334 for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
335 vpImagePoint leftPt(trainKeyPoints[(size_t)it->trainIdx].pt.y, trainKeyPoints[(size_t)it->trainIdx].pt.x);
336 vpImagePoint rightPt(queryKeyPoints[(size_t)it->queryIdx].pt.y,
337 queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth());
338 vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
339 }
340
341 if (is_pose_estimated) {
342 tracker.setPose(I, cMo);
343 tracker.display(I, cMo, cam, vpColor::red);
344 vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
345 }
346
347 vpDisplay::flush(Imatch);
349 }
350
351 // Click requested to process next image
352 if (opt_click_allowed && opt_display) {
353 if (opt_click) {
354 vpDisplay::getClick(I, button, true);
355 if (button == vpMouseButton::button3) {
356 opt_click = false;
357 }
358 }
359 else {
360 // Use right click to enable/disable step by step tracking
361 if (vpDisplay::getClick(I, button, false)) {
362 if (button == vpMouseButton::button3) {
363 opt_click = true;
364 }
365 else if (button == vpMouseButton::button1) {
366 break;
367 }
368 }
369 }
370 }
371 }
372}
373
380int main(int argc, const char **argv)
381{
382 try {
383 std::string env_ipath;
384 bool opt_click_allowed = true;
385 bool opt_display = true;
386
387 // Read the command line options
388 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
389 return EXIT_FAILURE;
390 }
391
392 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
393 // environment variable value
395
396 if (env_ipath.empty()) {
397 std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
398 "variable value."
399 << std::endl;
400 return EXIT_FAILURE;
401 }
402
403 {
404 vpImage<unsigned char> I, Imatch, Iref;
405
406 std::cout << "-- Test on gray level images" << std::endl;
407 run_test(env_ipath, opt_click_allowed, opt_display, I, Imatch, Iref);
408 }
409
410 {
411 vpImage<vpRGBa> I, Imatch, Iref;
412
413 std::cout << "-- Test on color images" << std::endl;
414 run_test(env_ipath, opt_click_allowed, opt_display, I, Imatch, Iref);
415 }
416
417 }
418 catch (const vpException &e) {
419 std::cerr << e.what() << std::endl;
420 return EXIT_FAILURE;
421 }
422
423 std::cout << "testKeyPoint-4 is ok !" << std::endl;
424 return EXIT_SUCCESS;
425}
426
427#else
428int main()
429{
430 std::cerr << "You need OpenCV library." << std::endl;
431
432 return EXIT_SUCCESS;
433}
434
435#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).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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:132
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)
virtual void setDownScalingFactor(unsigned int scale)
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)
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
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition vpImage.h:1361
unsigned int getHeight() const
Definition vpImage.h:184
static std::string getViSPImagesDataPath()
static std::string createFilePath(const std::string &parent, const std::string &child)
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)
static double rad(double deg)
Definition vpMath.h:116
Make the complete tracking of an object by using its CAD model.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void setClipping(const unsigned int &flags)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void setMovingEdge(const vpMe &me)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void getCameraParameters(vpCameraParameters &cam) const
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void setAngleDisappear(const double &a)
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)
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 vpMe.h:122
void setMu1(const double &mu_1)
Definition vpMe.h:353
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition vpMe.h:445
void setMaskSize(const unsigned int &a)
Definition vpMe.cpp:452
void setNbTotalSample(const int &nb)
Definition vpMe.h:367
void setMu2(const double &mu_2)
Definition vpMe.h:360
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
void setMaskNumber(const unsigned int &a)
Definition vpMe.cpp:445
void setThreshold(const double &t)
Definition vpMe.h:435
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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:81
void setRansacMaxTrials(const int &rM)
Definition vpPose.h:256
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition vpPose.h:245
@ RANSAC
Definition vpPose.h:91
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
void setRansacThreshold(const double &t)
Definition vpPose.h:246
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