MRPT  1.9.9
CFeatureExtraction_harris_KLT.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
13 
14 // Universal include for all versions of OpenCV
15 #include <mrpt/otherlibs/do_opencv_includes.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::img;
19 using namespace mrpt::vision;
20 using namespace mrpt::img;
21 using namespace mrpt::system;
22 using namespace std;
23 
24 /************************************************************************************************
25 * extractFeaturesKLT
26 ************************************************************************************************/
27 void CFeatureExtraction::extractFeaturesKLT(
28  const mrpt::img::CImage& inImg, CFeatureList& feats, unsigned int init_ID,
29  unsigned int nDesiredFeatures, const TImageROI& ROI) const
30 {
31 //#define VERBOSE_TIMING
32 
33 #ifdef VERBOSE_TIMING
34  CTicTac tictac;
35 #endif
37 
38 #if MRPT_HAS_OPENCV
39  const unsigned int MAX_COUNT = 300;
40 
41  // -----------------------------------------------------------------
42  // Create OpenCV Local Variables
43  // -----------------------------------------------------------------
44  int count = 0;
45  int nPts;
46 
47 #ifdef VERBOSE_TIMING
48  tictac.Tic();
49 #endif
50  const cv::Mat img(cv::cvarrToMat(inImg.getAs<IplImage>()));
51 
52 #ifdef VERBOSE_TIMING
53  cout << "[KLT] Attach: " << tictac.Tac() * 1000.0f << endl;
54 #endif
55  const CImage inImg_gray(inImg, FAST_REF_OR_CONVERT_TO_GRAY);
56  const cv::Mat cGrey(cv::cvarrToMat(inImg_gray.getAs<IplImage>()));
57 
58  nDesiredFeatures <= 0 ? nPts = MAX_COUNT : nPts = nDesiredFeatures;
59 
60 #ifdef VERBOSE_TIMING
61  tictac.Tic();
62 #endif
63 
64 #ifdef VERBOSE_TIMING
65  cout << "[KLT] Create: " << tictac.Tac() * 1000.0f << endl;
66 #endif
67  count = nPts; // Number of points to find
68 
69  // -----------------------------------------------------------------
70  // Select good features with subpixel accuracy (USING HARRIS OR KLT)
71  // -----------------------------------------------------------------
72  const bool use_harris = (options.featsType == featHarris);
73 
74 #ifdef VERBOSE_TIMING
75  tictac.Tic();
76 #endif
77  std::vector<cv::Point2f> points;
78  cv::goodFeaturesToTrack(
79  cGrey, points, nPts,
80  (double)options.harrisOptions.threshold, // for rejecting weak local
81  // maxima ( with min_eig <
82  // threshold*max(eig_image) )
83  (double)options.harrisOptions
84  .min_distance, // minimum distance between features
85  cv::noArray(), // mask
86  3, // blocksize
87  use_harris, /* harris */
88  options.harrisOptions.k);
89 #ifdef VERBOSE_TIMING
90  cout << "[KLT] Find feats: " << tictac.Tac() * 1000.0f << endl;
91 #endif
92 
93  if (nDesiredFeatures > 0 && count < nPts)
94  cout << "\n[WARNING][selectGoodFeaturesKLT]: Only " << count << " of "
95  << nDesiredFeatures << " points could be extracted in the image."
96  << endl;
97 
98  if (options.FIND_SUBPIXEL)
99  {
100 #ifdef VERBOSE_TIMING
101  tictac.Tic();
102 #endif
103  // Subpixel interpolation
104  cv::cornerSubPix(
105  cGrey, points, cv::Size(3, 3), cv::Size(-1, -1),
106  cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.05));
107 
108 #ifdef VERBOSE_TIMING
109  cout << "[KLT] subpixel: " << tictac.Tac() * 1000.0f << endl;
110 #endif
111  }
112 
113 // -----------------------------------------------------------------
114 // Fill output structure
115 // -----------------------------------------------------------------
116 #ifdef VERBOSE_TIMING
117  tictac.Tic();
118 #endif
119 
120  feats.clear();
121  unsigned int borderFeats = 0;
122  unsigned int nCFeats = init_ID;
123  int i = 0;
124  const int limit = min(nPts, count);
125  int offset = (int)this->options.patchSize / 2 + 1;
126  unsigned int imgH = inImg.getHeight();
127  unsigned int imgW = inImg.getWidth();
128 
129  while (i < limit)
130  {
131  const int xBorderInf = (int)floor(points[i].x - options.patchSize / 2);
132  const int xBorderSup = (int)floor(points[i].x + options.patchSize / 2);
133  const int yBorderInf = (int)floor(points[i].y - options.patchSize / 2);
134  const int yBorderSup = (int)floor(points[i].y + options.patchSize / 2);
135 
136  if (options.patchSize == 0 ||
137  ((xBorderSup < (int)imgW) && (xBorderInf > 0) &&
138  (yBorderSup < (int)imgH) && (yBorderInf > 0)))
139  {
140  CFeature::Ptr ft = mrpt::make_aligned_shared<CFeature>();
141 
142  ft->type = featKLT;
143  ft->x = points[i].x; // X position
144  ft->y = points[i].y; // Y position
145  ft->track_status = status_TRACKED; // Feature Status
146  ft->response = 0.0; // A value proportional to the quality of the
147  // feature (unused yet)
148  ft->ID = nCFeats++; // Feature ID into extraction
149  ft->patchSize = options.patchSize; // The size of the feature patch
150 
151  if (options.patchSize > 0)
152  {
153  inImg.extract_patch(
154  ft->patch, round(ft->x) - offset, round(ft->y) - offset,
155  options.patchSize,
156  options.patchSize); // Image patch surronding the feature
157  }
158 
159  feats.push_back(ft);
160 
161  } // end if
162  else
163  borderFeats++;
164 
165  i++;
166  } // end while
167 
168 #ifdef VERBOSE_TIMING
169  cout << "[KLT] Create output: " << tictac.Tac() * 1000.0f << endl;
170 #endif
171 
172 #else
173  THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
174 #endif
175 
176  MRPT_END
177 
178 } // end of function
179 
180 MRPT_TODO("Delete? Refactor / join to mrpt::vision::CGenericFeatureTracker?")
181 #if 0
182 /*------------------------------------------------------------
183  findMoreFeatures
184 -------------------------------------------------------------*/
185 void CFeatureExtraction::findMoreFeatures( const mrpt::img::CImage &img,
186  const CFeatureList &inList,
187  CFeatureList &outList,
188  unsigned int nDesiredFeats) const
189 {
190 #if MRPT_HAS_OPENCV
191  MRPT_START
192 
193  if( options.featsType == featHarris || options.featsType == featKLT )
194  {
195  // Mask the points already stored in the list
196  cv::Mat mask = cv::Mat::ones(img.getHeight(), img.getWidth(), CV_8UC1 ); // Set the whole mask to 1 initially
197 
199 
200  TFeatureID mxID = 0;
201 
202  for( itKLT = inList.begin(); itKLT != inList.end(); itKLT++ )
203  {
204  int cx = (int)(*itKLT)->x;
205  int cy = (int)(*itKLT)->y;
206 
207  // Mask surronding pixels
208  size_t xxI = max( 0, cx - 15 );
209  size_t xxE = min( cx + 15, mask.cols()-1 );
210  size_t yyI = max( 0, cy - 15 );
211  size_t yyE = min( cy + 15, (int)mask.rows()-1 );
212 
213  for(size_t yy = yyI; yy < yyE; yy++)
214  for(size_t xx = xxI; xx < xxE; xx++)
215  cvSet2D( mask, yy, xx, zero );
216 
217  if( (*itKLT)->ID > mxID )
218  mxID = (*itKLT)->ID;
219 
220  }
221  selectGoodFeaturesKLT( img, outList, mxID + 1, nDesiredFeats, &mask );
222  }
223  else if( options.featsType == featFAST )
224  {
225  TFeatureID mxID = inList.getMaxID();
226  extractFeaturesFAST( img, outList, mxID+1 ); // Find all the possible FAST features
227 
228  // Delete all that are too close to the current ones
229  CFeatureList::iterator itOutList;
231  for( itInList = inList.begin(); itInList != inList.end(); ++itInList )
232  {
233  for( itOutList = outList.begin(); itOutList != outList.end(); )
234  {
235  if( fabs((*itOutList)->x-(*itInList)->x) < 15 || fabs((*itOutList)->y-(*itInList)->y) < 15 )
236  itOutList = outList.erase( itOutList );
237  else
238  ++itOutList;
239  } // end-for
240  } // end-for
241 
242  if( nDesiredFeats != 0 && outList.size() > nDesiredFeats )
243  outList.resize( nDesiredFeats );
244  }
245  MRPT_END
246 #else
247  THROW_EXCEPTION("MRPT was compiled without OpenCV");
248 #endif
249 } // end findMoreFeatures
250 #endif
251 
252 MRPT_TODO("Delete? Is not this a duplicate of extractFeaturesKLT ()???")
253 #if 0
254 /************************************************************************************************
255 * selectGoodFeaturesKLT *
256 ************************************************************************************************/
257 void CFeatureExtraction::selectGoodFeaturesKLT(
258  const mrpt::img::CImage &img,
259  CFeatureList &feats,
260  unsigned int init_ID,
261  unsigned int nDesiredFeatures) const
262 {
263 #if MRPT_HAS_OPENCV
264  // Get image size
265  size_t imgW = img.getWidth();
266  size_t imgH = img.getHeight();
267 
268  // Check image size and ROI limits
269  ASSERT_( imgH > 0 && imgW > 0 ); // Proper Image Size
270  ASSERT_( ROI.xMin >= 0 && ROI.yMin >= 0 && ROI.xMax < imgW && ROI.yMax < imgH ); // Proper TImageROI Limits
271 
272  // Mask
273  CvMatrix mask( imgH, imgW, CV_8UC1 );
274  CvScalar zero = cvRealScalar( 0.0 );
275  CvScalar one = cvRealScalar( 1.0 );
276 
277  if( ROI.xMin == 0 && ROI.xMax == 0 && ROI.yMin == 0 && ROI.yMax == 0 ) // Use the whole image (except the borders)
278  {
279  if( options.patchSize == 0 )
280  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, nullptr );
281  else
282  {
283  // We mask the borders of the image in order to get an appropiate patch (according to the patch size)
284  // ---------------------------------------------------------------------
285  // 00000000000000000000000000000000000
286  // 00000000000000000000000000000000000
287  // 00111111111111111111111111111111100
288  // 00111111111111111111111111111111100
289  // 00111111111111111111111111111111100
290  // 00111111111111111111111111111111100
291  // 00111111111111111111111111111111100
292  // 00111111111111111111111111111111100
293  // 00111111111111111111111111111111100
294  // 00000000000000000000000000000000000
295  // 00000000000000000000000000000000000
296 
297  cvSet( mask, one ); // Set the whole mask to 1 initially
298 
299  size_t border = (options.patchSize-1)/2 + 1;
300  size_t xx,yy; // Put outside of the "for"s to avoid errors in VC6
301  for( xx = 0; xx < imgW; xx++ )
302  for( yy = 0; yy < border; yy++ )
303  cvSet2D( mask, yy, xx, zero ); // Set to 0 the pixels not to be processed
304 
305  for( xx = 0; xx < imgW; xx++ )
306  for( yy = imgH - border; yy < imgH; yy++ )
307  cvSet2D( mask, yy, xx, zero ); // Set to 0 the pixels not to be processed
308 
309  for( xx = 0; xx < border; xx++ )
310  for( yy = border; yy < imgH - border; yy++ )
311  cvSet2D( mask, yy, xx, zero ) ; // Set to 0 the pixels not to be processed
312 
313  for( xx = imgW - border; xx < imgW; xx++ )
314  for( yy = border; yy < imgH - border; yy++ )
315  cvSet2D( mask, yy, xx, zero ) ; // Set to 0 the pixels not to be processed
316 
317  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
318  } // end if-else patchSize == 0
319  } // end if ROI does not exist
320  else
321  {
322  cvSet( mask, zero ); // Set the whole mask to 0 initially
323  for( unsigned int xx = ROI.xMin; xx < ROI.xMax; xx++ )
324  for( unsigned int yy = ROI.yMin; yy < ROI.yMax; yy++ )
325  cvSet2D( mask, yy, xx, one) ; // Set to 1 the pixels to be processed
326 
327  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
328  } // end if-else there exists a ROI
329 #else
330  THROW_EXCEPTION("MRPT was compiled without OpenCV");
331 #endif
332 }
333 
334 #endif
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
GLuint GLuint GLsizei count
Definition: glext.h:3528
GLenum GLint GLuint mask
Definition: glext.h:4050
const T * getAs() const
Returns a pointer to a const T* containing the image - the idea is to call like "img.getAs<IplImage>()" so we can avoid here including OpenCV&#39;s headers.
Definition: img/CImage.h:599
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
#define min(a, b)
TInternalFeatList::iterator iterator
Definition: CFeature.h:365
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
size_t size() const
Definition: CFeature.h:386
GLintptr offset
Definition: glext.h:3925
A high-performance stopwatch, with typical resolution of nanoseconds.
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
STL namespace.
GLint limit
Definition: glext.h:8188
GLsizei const GLfloat * points
Definition: glext.h:5339
FAST feature detector, OpenCV&#39;s implementation ("Faster and better: A machine learning approach to...
A structure for defining a ROI within an image.
GLint GLint GLsizei GLsizei GLsizei GLint border
Definition: glext.h:3600
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
void resize(size_t N)
Definition: CFeature.h:392
GLint GLvoid * img
Definition: glext.h:3763
Harris border and corner detector [HARRIS].
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:18
iterator erase(const iterator &it)
Definition: CFeature.h:379
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:304
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:366
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MRPT_TODO(x)
Definition: common.h:129
#define MRPT_END
Definition: exceptions.h:266
TFeatureID getMaxID() const
Get the maximum ID into the list.
Definition: CFeature.cpp:1328
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
Definition: CImage.cpp:1359
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
EIGEN_STRONG_INLINE void ones(const size_t row, const size_t col)
Resize matrix and set all elements to one.
Definition: eigen_plugins.h:82
Kanade-Lucas-Tomasi feature [SHI&#39;94].
void push_back(const CFeature::Ptr &f)
Definition: CFeature.h:398
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020