Main MRPT website > C++ reference for MRPT 1.5.7
tracking_KL.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/system/memory.h>
13 #include <mrpt/vision/tracking.h>
15 
16 // Universal include for all versions of OpenCV
17 #include <mrpt/otherlibs/do_opencv_includes.h>
18 
19 #if HAVE_ALLOCA_H
20 # include <alloca.h>
21 #endif
22 
23 
24 using namespace mrpt;
25 using namespace mrpt::vision;
26 using namespace mrpt::utils;
27 using namespace std;
28 
29 
30 /** Track a set of features from old_img -> new_img using sparse optimal flow (classic KL method)
31  * Optional parameters that can be passed in "extra_params":
32  * - "window_width" (Default=15)
33  * - "window_height" (Default=15)
34  *
35  * \sa OpenCV's method cvCalcOpticalFlowPyrLK
36  */
37 template <typename FEATLIST>
39  const CImage &old_img,
40  const CImage &new_img,
41  FEATLIST &featureList )
42 {
44 
45 #if MRPT_HAS_OPENCV
46  const unsigned int window_width = extra_params.getWithDefaultVal("window_width",15);
47  const unsigned int window_height = extra_params.getWithDefaultVal("window_height",15);
48 
49  const int LK_levels = extra_params.getWithDefaultVal("LK_levels",3);
50  const int LK_max_iters = extra_params.getWithDefaultVal("LK_max_iters",10);
51  const int LK_epsilon = extra_params.getWithDefaultVal("LK_epsilon",0.1);
52  const float LK_max_tracking_error = extra_params.getWithDefaultVal("LK_max_tracking_error",150.0f);
53 
54 
55  // Both images must be of the same size
56  ASSERT_( old_img.getWidth() == new_img.getWidth() && old_img.getHeight() == new_img.getHeight() );
57 
58  const size_t img_width = old_img.getWidth();
59  const size_t img_height = old_img.getHeight();
60 
61  const size_t nFeatures = featureList.size(); // Number of features
62 
63  // Grayscale images
64  const CImage prev_gray(old_img, FAST_REF_OR_CONVERT_TO_GRAY);
65  const CImage cur_gray(new_img, FAST_REF_OR_CONVERT_TO_GRAY);
66 
67  // Array conversion MRPT->OpenCV
68  if (nFeatures>0)
69  {
70  CvPoint2D32f *points[2];
71 
72  points[0] = reinterpret_cast<CvPoint2D32f *>( mrpt_alloca(sizeof(CvPoint2D32f)*nFeatures) );
73  points[1] = reinterpret_cast<CvPoint2D32f *>( mrpt_alloca(sizeof(CvPoint2D32f)*nFeatures) );
74 
75  std::vector<char> status(nFeatures);
76 
77  for(size_t i=0;i<nFeatures;++i)
78  {
79  points[0][i].x = featureList.getFeatureX(i);
80  points[0][i].y = featureList.getFeatureY(i);
81  } // end for
82 
83  // local scope for auxiliary variables around cvCalcOpticalFlowPyrLK()
84  const IplImage *prev_gray_ipl = prev_gray.getAs<IplImage>();
85  const IplImage *cur_gray_ipl = cur_gray.getAs<IplImage>();
86 
87  // Pyramids
88  // JL: It seems that cache'ing the pyramids of previous images doesn't really improve the efficiency (!?!?)
89  IplImage* pPyr = NULL;
90  IplImage* cPyr = NULL;
91 
92  int flags = 0;
93 
94  float* track_error = reinterpret_cast<float*>( mrpt_alloca(sizeof(float)*nFeatures) );
95 
96  cvCalcOpticalFlowPyrLK(prev_gray_ipl, cur_gray_ipl, pPyr, cPyr,
97  &points[0][0], &points[1][0], nFeatures, cvSize( window_width, window_height ), LK_levels, &status[0], track_error,
98  cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,LK_max_iters,LK_epsilon), flags );
99 
100  cvReleaseImage( &pPyr );
101  cvReleaseImage( &cPyr );
102 
103  for(size_t i=0;i<nFeatures;++i)
104  {
105  const bool trck_err_too_large = track_error[i]>LK_max_tracking_error;
106 
107  if( status[i] == 1 &&
108  !trck_err_too_large &&
109  points[1][i].x > 0 && points[1][i].y > 0 &&
110  points[1][i].x < img_width && points[1][i].y < img_height )
111  {
112  // Feature could be tracked
113  featureList.setFeatureXf(i, points[1][i].x );
114  featureList.setFeatureYf(i, points[1][i].y );
115  featureList.setTrackStatus(i, status_TRACKED );
116  } // end if
117  else // Feature could not be tracked
118  {
119  featureList.setFeatureX(i,-1);
120  featureList.setFeatureY(i,-1);
121  featureList.setTrackStatus(i, trck_err_too_large ? status_LOST : status_OOB );
122  } // end else
123  } // end for
124 
125  mrpt_alloca_free( points[0] );
126  mrpt_alloca_free( points[1] );
127  mrpt_alloca_free( track_error );
128 
129 
130  // In case it needs to rebuild a kd-tree or whatever
131  featureList.mark_as_outdated();
132  }
133 
134 #else
135  THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
136 #endif
137 
138  MRPT_END
139 } // end trackFeatures
140 
141 
143  const CImage &old_img,
144  const CImage &new_img,
145  CFeatureList &featureList )
146 {
147  trackFeatures_impl_templ<CFeatureList>(old_img,new_img,featureList);
148 }
149 
151  const CImage &old_img,
152  const CImage &new_img,
153  TSimpleFeatureList &featureList )
154 {
155  trackFeatures_impl_templ<TSimpleFeatureList>(old_img,new_img,featureList);
156 }
157 
159  const CImage &old_img,
160  const CImage &new_img,
161  TSimpleFeaturefList &featureList )
162 {
163  trackFeatures_impl_templ<TSimpleFeaturefList>(old_img,new_img,featureList);
164 }
165 
166 
167 
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
#define THROW_EXCEPTION(msg)
STL namespace.
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
GLsizei const GLfloat * points
Definition: glext.h:4797
#define MRPT_END
void trackFeatures_impl_templ(const mrpt::utils::CImage &old_img, const mrpt::utils::CImage &new_img, FEATLIST &inout_featureList)
Track a set of features from old_img -> new_img using sparse optimal flow (classic KL method) Optiona...
Definition: tracking_KL.cpp:38
Classes for computer vision, detectors, features, etc.
#define mrpt_alloca(nBytes)
In platforms and compilers with support to "alloca", allocate a memory block on the stack; if alloca ...
Definition: memory.h:38
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:211
Unable to track this feature (mismatch is too high for the given tracking window: lack of texture...
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
_u8 status
Definition: rplidar_cmd.h:21
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
#define mrpt_alloca_free(mem_block)
This method must be called to "free" each memory block allocated with "system::alloca": If the block ...
Definition: memory.h:53
GLenum GLint x
Definition: glext.h:3516
virtual void trackFeatures_impl(const mrpt::utils::CImage &old_img, const mrpt::utils::CImage &new_img, vision::CFeatureList &inout_featureList) MRPT_OVERRIDE
This version falls back to the version with TSimpleFeatureList if the derived class does not implemen...
Feature fell Out Of Bounds (out of the image limits, too close to image borders)
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
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