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



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