MRPT  1.9.9
tracking_KL.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/memory.h>
14 #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 int window_width = extra_params.getWithDefaultVal("window_width", 15);
44  const int window_height =
45  extra_params.getWithDefaultVal("window_height", 15);
46 
47  const int LK_levels = extra_params.getWithDefaultVal("LK_levels", 3);
48  const int LK_max_iters = extra_params.getWithDefaultVal("LK_max_iters", 10);
49  const int LK_epsilon = extra_params.getWithDefaultVal("LK_epsilon", 0.1);
50  const float LK_max_tracking_error =
51  extra_params.getWithDefaultVal("LK_max_tracking_error", 150.0f);
52 
53  // Both images must be of the same size
54  ASSERT_(
55  old_img.getWidth() == new_img.getWidth() &&
56  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  std::vector<cv::Point2f> points_prev(nFeatures), points_cur;
71  std::vector<uchar> status(nFeatures);
72  std::vector<float> track_error(nFeatures);
73 
74  for (size_t i = 0; i < nFeatures; ++i)
75  {
76  points_prev[i].x = featureList.getFeatureX(i);
77  points_prev[i].y = featureList.getFeatureY(i);
78  }
79 
80  const cv::Mat& prev = prev_gray.asCvMatRef();
81  const cv::Mat& cur = cur_gray.asCvMatRef();
82 
83  cv::calcOpticalFlowPyrLK(
84  prev, cur, points_prev, points_cur, status, track_error,
85  cv::Size(window_width, window_height), LK_levels,
86  cv::TermCriteria(
87  cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,
88  LK_max_iters, LK_epsilon));
89 
90  for (size_t i = 0; i < nFeatures; ++i)
91  {
92  const bool trck_err_too_large =
93  track_error[i] > LK_max_tracking_error;
94 
95  if (status[i] == 1 && !trck_err_too_large && points_cur[i].x > 0 &&
96  points_cur[i].y > 0 && points_cur[i].x < img_width &&
97  points_cur[i].y < img_height)
98  {
99  // Feature could be tracked
100  featureList.setFeatureXf(i, points_cur[i].x);
101  featureList.setFeatureYf(i, points_cur[i].y);
102  featureList.setTrackStatus(i, status_TRACKED);
103  }
104  else // Feature could not be tracked
105  {
106  featureList.setFeatureX(i, -1);
107  featureList.setFeatureY(i, -1);
108  featureList.setTrackStatus(
109  i, trck_err_too_large ? status_LOST : status_OOB);
110  }
111  }
112 
113  // In case it needs to rebuild a kd-tree or whatever
114  featureList.mark_as_outdated();
115  }
116 
117 #else
118  THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
119 #endif
120 
121  MRPT_END
122 } // end trackFeatures
123 
125  const CImage& old_img, const CImage& new_img, TKeyPointList& featureList)
126 {
127  trackFeatures_impl_templ<TKeyPointList>(old_img, new_img, featureList);
128 }
129 
131  const CImage& old_img, const CImage& new_img, TKeyPointfList& featureList)
132 {
133  trackFeatures_impl_templ<TKeyPointfList>(old_img, new_img, featureList);
134 }
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Unable to track this feature (mismatch is too high for the given tracking window: lack of texture...
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
Definition: CImage.cpp:233
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:878
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:847
Feature fell Out Of Bounds (out of the image limits, too close to image borders)
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MRPT_END
Definition: exceptions.h:245
void trackFeatures_impl(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, TKeyPointList &inout_featureList) override
The tracking method implementation, to be implemented in children classes.
_u8 status
Definition: rplidar_cmd.h:19
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019