Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
12 #include <mrpt/vision/CFeatureExtraction.h>
13 
14 // Universal include for all versions of OpenCV
15 #include <mrpt/otherlibs/do_opencv_includes.h>
16 
17 
18 using namespace mrpt;
19 using namespace mrpt::vision;
20 using namespace mrpt::system;
21 using namespace mrpt::utils;
22 using namespace std;
23 
24 
25 /************************************************************************************************
26 * extractFeaturesKLT
27 ************************************************************************************************/
28 void CFeatureExtraction::extractFeaturesKLT(
29  const mrpt::utils::CImage &inImg,
30  CFeatureList &feats,
31  unsigned int init_ID,
32  unsigned int nDesiredFeatures,
33  const TImageROI &ROI) const
34 {
35 //#define VERBOSE_TIMING
36 
37 #ifdef VERBOSE_TIMING
38  CTicTac tictac;
39 #endif
40  MRPT_START
41 
42  #if MRPT_HAS_OPENCV
43  const unsigned int MAX_COUNT = 300;
44 
45  // -----------------------------------------------------------------
46  // Create OpenCV Local Variables
47  // -----------------------------------------------------------------
48  int count = 0;
49  int nPts;
50 
51 #ifdef VERBOSE_TIMING
52  tictac.Tic();
53 #endif
54  const cv::Mat img( cv::cvarrToMat( inImg.getAs<IplImage>() ) );
55 
56 #ifdef VERBOSE_TIMING
57  cout << "[KLT] Attach: " << tictac.Tac()*1000.0f << endl;
58 #endif
59  const CImage inImg_gray( inImg, FAST_REF_OR_CONVERT_TO_GRAY );
60  const cv::Mat cGrey( cv::cvarrToMat( inImg_gray.getAs<IplImage>() ) );
61 
62  nDesiredFeatures <= 0 ? nPts = MAX_COUNT : nPts = nDesiredFeatures;
63 
64 #ifdef VERBOSE_TIMING
65  tictac.Tic();
66 #endif
67 
68 #ifdef VERBOSE_TIMING
69  cout << "[KLT] Create: " << tictac.Tac()*1000.0f << endl;
70 #endif
71  count = nPts; // Number of points to find
72 
73  // -----------------------------------------------------------------
74  // Select good features with subpixel accuracy (USING HARRIS OR KLT)
75  // -----------------------------------------------------------------
76  const bool use_harris = ( options.featsType == featHarris );
77 
78 #ifdef VERBOSE_TIMING
79  tictac.Tic();
80 #endif
81  std::vector<cv::Point2f> points;
82  cv::goodFeaturesToTrack(
83  cGrey,points, nPts,
84  (double)options.harrisOptions.threshold, // for rejecting weak local maxima ( with min_eig < threshold*max(eig_image) )
85  (double)options.harrisOptions.min_distance, // minimum distance between features
86  cv::noArray(), // mask
87  3, // blocksize
88  use_harris, /* harris */
89  options.harrisOptions.k
90  );
91 #ifdef VERBOSE_TIMING
92  cout << "[KLT] Find feats: " << tictac.Tac()*1000.0f << endl;
93 #endif
94 
95  if( nDesiredFeatures > 0 && count < nPts )
96  cout << "\n[WARNING][selectGoodFeaturesKLT]: Only " << count << " of " << nDesiredFeatures << " points could be extracted in the image." << endl;
97 
98  if( options.FIND_SUBPIXEL )
99  {
100 #ifdef VERBOSE_TIMING
101  tictac.Tic();
102 #endif
103  // Subpixel interpolation
104  cv::cornerSubPix(cGrey,points,
105  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 || ( (xBorderSup < (int)imgW) && (xBorderInf > 0) && (yBorderSup < (int)imgH) && (yBorderInf > 0) ) )
137  {
138  CFeaturePtr ft = CFeature::Create();
139 
140  ft->type = featKLT;
141  ft->x = points[i].x; // X position
142  ft->y = points[i].y; // Y position
143  ft->track_status = status_TRACKED; // Feature Status
144  ft->response = 0.0; // A value proportional to the quality of the feature (unused yet)
145  ft->ID = nCFeats++; // Feature ID into extraction
146  ft->patchSize = options.patchSize; // The size of the feature patch
147 
148  if( options.patchSize > 0 )
149  {
150  inImg.extract_patch(
151  ft->patch,
152  round( ft->x ) - offset,
153  round( ft->y ) - offset,
154  options.patchSize,
155  options.patchSize ); // Image patch surronding the feature
156  }
157 
158  feats.push_back( ft );
159 
160  } // end if
161  else
162  borderFeats++;
163 
164  i++;
165  } // end while
166 
167 #ifdef VERBOSE_TIMING
168  cout << "[KLT] Create output: " << tictac.Tac()*1000.0f << endl;
169 #endif
170 
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::utils::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 
198  CFeatureList::const_iterator itKLT;
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;
230  CFeatureList::const_iterator itInList;
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 
253 MRPT_TODO("Delete? Is not this a duplicate of extractFeaturesKLT ()???")
254 #if 0
255 /************************************************************************************************
256 * selectGoodFeaturesKLT *
257 ************************************************************************************************/
258 void CFeatureExtraction::selectGoodFeaturesKLT(
259  const mrpt::utils::CImage &img,
260  CFeatureList &feats,
261  unsigned int init_ID,
262  unsigned int nDesiredFeatures) const
263 {
264 #if MRPT_HAS_OPENCV
265  // Get image size
266  size_t imgW = img.getWidth();
267  size_t imgH = img.getHeight();
268 
269  // Check image size and ROI limits
270  ASSERT_( imgH > 0 && imgW > 0 ); // Proper Image Size
271  ASSERT_( ROI.xMin >= 0 && ROI.yMin >= 0 && ROI.xMax < imgW && ROI.yMax < imgH ); // Proper TImageROI Limits
272 
273  // Mask
274  CvMatrix mask( imgH, imgW, CV_8UC1 );
275  CvScalar zero = cvRealScalar( 0.0 );
276  CvScalar one = cvRealScalar( 1.0 );
277 
278  if( ROI.xMin == 0 && ROI.xMax == 0 && ROI.yMin == 0 && ROI.yMax == 0 ) // Use the whole image (except the borders)
279  {
280  if( options.patchSize == 0 )
281  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, NULL );
282  else
283  {
284  // We mask the borders of the image in order to get an appropiate patch (according to the patch size)
285  // ---------------------------------------------------------------------
286  // 00000000000000000000000000000000000
287  // 00000000000000000000000000000000000
288  // 00111111111111111111111111111111100
289  // 00111111111111111111111111111111100
290  // 00111111111111111111111111111111100
291  // 00111111111111111111111111111111100
292  // 00111111111111111111111111111111100
293  // 00111111111111111111111111111111100
294  // 00111111111111111111111111111111100
295  // 00000000000000000000000000000000000
296  // 00000000000000000000000000000000000
297 
298  cvSet( mask, one ); // Set the whole mask to 1 initially
299 
300  size_t border = (options.patchSize-1)/2 + 1;
301  size_t xx,yy; // Put outside of the "for"s to avoid errors in VC6
302  for( xx = 0; xx < imgW; xx++ )
303  for( yy = 0; yy < border; yy++ )
304  cvSet2D( mask, yy, xx, zero ); // Set to 0 the pixels not to be processed
305 
306  for( xx = 0; xx < imgW; xx++ )
307  for( yy = imgH - border; yy < imgH; yy++ )
308  cvSet2D( mask, yy, xx, zero ); // Set to 0 the pixels not to be processed
309 
310  for( xx = 0; xx < border; xx++ )
311  for( yy = border; yy < imgH - border; yy++ )
312  cvSet2D( mask, yy, xx, zero ) ; // Set to 0 the pixels not to be processed
313 
314  for( xx = imgW - border; xx < imgW; xx++ )
315  for( yy = border; yy < imgH - border; yy++ )
316  cvSet2D( mask, yy, xx, zero ) ; // Set to 0 the pixels not to be processed
317 
318  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
319  } // end if-else patchSize == 0
320  } // end if ROI does not exist
321  else
322  {
323  cvSet( mask, zero ); // Set the whole mask to 0 initially
324  for( unsigned int xx = ROI.xMin; xx < ROI.xMax; xx++ )
325  for( unsigned int yy = ROI.yMin; yy < ROI.yMax; yy++ )
326  cvSet2D( mask, yy, xx, one) ; // Set to 1 the pixels to be processed
327 
328  selectGoodFeaturesKLT( img, feats, init_ID, nDesiredFeatures, &mask );
329  } // end if-else there exists a ROI
330 #else
331  THROW_EXCEPTION("MRPT was compiled without OpenCV")
332 #endif
333 }
334 
335 #endif
336 
337 
count
GLuint GLuint GLsizei count
Definition: glext.h:3512
mask
GLenum GLint GLuint mask
Definition: glext.h:3888
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::vision::CFeatureList::size
size_t size() const
Definition: CFeature.h:280
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
offset
GLintptr offset
Definition: glext.h:3780
mrpt::vision::CFeatureList::const_iterator
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:262
std
STL namespace.
mrpt::vision::status_TRACKED
Feature correctly tracked.
Definition: vision/include/mrpt/vision/types.h:76
CFeatureExtraction.h
mrpt::utils::CTicTac::Tic
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
limit
GLint limit
Definition: glext.h:7011
mrpt::utils::CImage::getAs
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: CImage.h:517
MRPT_TODO
#define MRPT_TODO(x)
Definition: maps/CColouredPointsMap.h:115
points
GLsizei const GLfloat * points
Definition: glext.h:4797
mrpt::vision::featFAST
FAST feature detector, OpenCV&#39;s implementation ("Faster and better: A machine learning approach to co...
Definition: vision/include/mrpt/vision/types.h:50
mrpt::vision::TImageROI
A structure for defining a ROI within an image.
Definition: vision/include/mrpt/vision/types.h:251
border
GLint GLint GLsizei GLsizei GLsizei GLint border
Definition: glext.h:3545
mrpt::vision::CFeatureList::resize
void resize(size_t N)
Definition: CFeature.h:283
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
mrpt::vision::CFeatureExtraction::extractFeaturesKLT
void extractFeaturesKLT(const mrpt::utils::CImage &img, CFeatureList &feats, unsigned int init_ID=0, unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI()) const
Extract features from the image based on the KLT method.
Definition: CFeatureExtraction_harris_KLT.cpp:28
img
GLint GLvoid * img
Definition: glext.h:3645
mrpt::vision::featHarris
Harris border and corner detector [HARRIS].
Definition: vision/include/mrpt/vision/types.h:45
mrpt::utils::CTicTac
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::vision
Classes for computer vision, detectors, features, etc.
Definition: bundle_adjustment.h:22
mrpt::vision::CFeatureList::push_back
void push_back(const CFeaturePtr &f)
Definition: CFeature.h:285
mrpt::vision::CFeatureList::erase
iterator erase(const iterator &it)
Definition: CFeature.h:277
mrpt::vision::TFeatureID
uint64_t TFeatureID
Definition of a feature ID.
Definition: vision/include/mrpt/vision/types.h:27
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:211
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::utils::CImage::extract_patch
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:1368
mrpt::utils::CTicTac::Tac
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::vision::CFeatureList::getMaxID
TFeatureID getMaxID() const
Get the maximum ID into the list.
Definition: CFeature.cpp:1077
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:261
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::vision::CFeature::Create
static CFeaturePtr Create()
x
GLenum GLint x
Definition: glext.h:3516
ones
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:71
mrpt::vision::featKLT
Kanade-Lucas-Tomasi feature [SHI&#39;94].
Definition: vision/include/mrpt/vision/types.h:44
mrpt::utils::CImage::getWidth
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
mrpt::utils::CImage::getHeight
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019