Visual Servoing Platform  version 3.6.0 under development (2023-09-25)
vpKltOpencv.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
32  * with opencv.
33  */
34 
42 #include <visp3/core/vpConfig.h>
43 
44 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
45 
46 #include <string>
47 
48 #include <visp3/core/vpDisplay.h>
49 #include <visp3/core/vpTrackingException.h>
50 #include <visp3/klt/vpKltOpencv.h>
51 
53  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
54  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
55  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
56 {
57  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
58 }
59 
61  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
62  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
63  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
64 {
65  *this = copy;
66 }
67 
69 {
70  m_gray = copy.m_gray;
71  m_prevGray = copy.m_prevGray;
72  m_points[0] = copy.m_points[0];
73  m_points[1] = copy.m_points[1];
74  m_points_id = copy.m_points_id;
75  m_maxCount = copy.m_maxCount;
76  m_termcrit = copy.m_termcrit;
77  m_winSize = copy.m_winSize;
81  m_harris_k = copy.m_harris_k;
82  m_blockSize = copy.m_blockSize;
87 
88  return *this;
89 }
90 
92 
93 void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
94 {
95  m_next_points_id = 0;
96 
97  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
98  I.copyTo(m_gray);
99 
100  for (size_t i = 0; i < 2; i++) {
101  m_points[i].clear();
102  }
103 
104  m_points_id.clear();
105 
106  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false,
107  m_harris_k);
108 
109  if (m_points[1].size() > 0) {
110  cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
111 
112  for (size_t i = 0; i < m_points[1].size(); i++)
113  m_points_id.push_back(m_next_points_id++);
114  }
115 }
116 
117 void vpKltOpencv::track(const cv::Mat &I)
118 {
119  if (m_points[1].size() == 0)
120  throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
121 
122  std::vector<float> err;
123  int flags = 0;
124 
125  cv::swap(m_prevGray, m_gray);
126 
127  if (m_initial_guess) {
128  flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
129  m_initial_guess = false;
130  } else {
131  std::swap(m_points[1], m_points[0]);
132  }
133 
134  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
135  I.copyTo(m_gray);
136 
137  if (m_prevGray.empty()) {
138  m_gray.copyTo(m_prevGray);
139  }
140 
141  std::vector<uchar> status;
142 
143  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
145 
146  // Remove points that are lost
147  for (int i = (int)status.size() - 1; i >= 0; i--) {
148  if (status[(size_t)i] == 0) { // point is lost
149  m_points[0].erase(m_points[0].begin() + i);
150  m_points[1].erase(m_points[1].begin() + i);
151  m_points_id.erase(m_points_id.begin() + i);
152  }
153  }
154 }
155 
156 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
157 {
158  if ((size_t)index >= m_points[1].size()) {
159  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
160  }
161 
162  x = m_points[1][(size_t)index].x;
163  y = m_points[1][(size_t)index].y;
164  id = m_points_id[(size_t)index];
165 }
166 
167 void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
168 {
169  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
170 }
171 
172 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
173  const vpColor &color, unsigned int thickness)
174 {
175  vpImagePoint ip;
176  for (size_t i = 0; i < features.size(); i++) {
177  ip.set_u(vpMath::round(features[i].x));
178  ip.set_v(vpMath::round(features[i].y));
179  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
180  }
181 }
182 
183 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
184  unsigned int thickness)
185 {
186  vpImagePoint ip;
187  for (size_t i = 0; i < features.size(); i++) {
188  ip.set_u(vpMath::round(features[i].x));
189  ip.set_v(vpMath::round(features[i].y));
190  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
191  }
192 }
193 
194 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
195  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
196 {
197  vpImagePoint ip;
198  for (size_t i = 0; i < features.size(); i++) {
199  ip.set_u(vpMath::round(features[i].x));
200  ip.set_v(vpMath::round(features[i].y));
201  vpDisplay::displayCross(I, ip, 10, color, thickness);
202 
203  std::ostringstream id;
204  id << featuresid[i];
205  ip.set_u(vpMath::round(features[i].x + 5));
206  vpDisplay::displayText(I, ip, id.str(), color);
207  }
208 }
209 
210 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
211  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
212 {
213  vpImagePoint ip;
214  for (size_t i = 0; i < features.size(); i++) {
215  ip.set_u(vpMath::round(features[i].x));
216  ip.set_v(vpMath::round(features[i].y));
217  vpDisplay::displayCross(I, ip, 10, color, thickness);
218 
219  std::ostringstream id;
220  id << featuresid[i];
221  ip.set_u(vpMath::round(features[i].x + 5));
222  vpDisplay::displayText(I, ip, id.str(), color);
223  }
224 }
225 
226 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
227 {
228  if (guess_pts.size() != m_points[1].size()) {
230  "Cannot set initial guess: size feature vector [%d] "
231  "and guess vector [%d] doesn't match",
232  m_points[1].size(), guess_pts.size()));
233  }
234 
235  m_points[1] = guess_pts;
236  m_initial_guess = true;
237 }
238 
239 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
240  const std::vector<long> &fid)
241 {
242  if (guess_pts.size() != init_pts.size()) {
244  "Cannot set initial guess: size init vector [%d] and "
245  "guess vector [%d] doesn't match",
246  init_pts.size(), guess_pts.size()));
247  }
248 
249  m_points[0] = init_pts;
250  m_points[1] = guess_pts;
251  m_points_id = fid;
252  m_initial_guess = true;
253 }
254 
255 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
256 {
257  m_initial_guess = false;
258  m_points[1] = pts;
259  m_next_points_id = 0;
260  m_points_id.clear();
261  for (size_t i = 0; i < m_points[1].size(); i++) {
262  m_points_id.push_back(m_next_points_id++);
263  }
264 
265  I.copyTo(m_gray);
266 }
267 
268 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
269 {
270  m_initial_guess = false;
271  m_points[1] = pts;
272  m_points_id.clear();
273 
274  if (ids.size() != pts.size()) {
275  m_next_points_id = 0;
276  for (size_t i = 0; i < m_points[1].size(); i++)
277  m_points_id.push_back(m_next_points_id++);
278  } else {
279  long max = 0;
280  for (size_t i = 0; i < m_points[1].size(); i++) {
281  m_points_id.push_back(ids[i]);
282  if (ids[i] > max)
283  max = ids[i];
284  }
285  m_next_points_id = max + 1;
286  }
287 
288  I.copyTo(m_gray);
289 }
290 
291 void vpKltOpencv::addFeature(const float &x, const float &y)
292 {
293  cv::Point2f f(x, y);
294  m_points[1].push_back(f);
295  m_points_id.push_back(m_next_points_id++);
296 }
297 
298 void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
299 {
300  cv::Point2f f(x, y);
301  m_points[1].push_back(f);
302  m_points_id.push_back(id);
303  if (id >= m_next_points_id)
304  m_next_points_id = id + 1;
305 }
306 
307 void vpKltOpencv::addFeature(const cv::Point2f &f)
308 {
309  m_points[1].push_back(f);
310  m_points_id.push_back(m_next_points_id++);
311 }
312 
313 void vpKltOpencv::suppressFeature(const int &index)
314 {
315  if ((size_t)index >= m_points[1].size()) {
316  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
317  }
318 
319  m_points[1].erase(m_points[1].begin() + index);
320  m_points_id.erase(m_points_id.begin() + index);
321 }
322 
323 #else
324 
325 // Work around to avoid visp_klt library empty when OpenCV is not installed or used
326 class VISP_EXPORT dummy_vpKltOpencv
327 {
328 public:
329  dummy_vpKltOpencv(){};
330 };
331 
332 #if !defined(VISP_BUILD_SHARED_LIBS)
333 // Work around to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no
334 // symbols
335 void dummy_vpKltOpenCV_fct(){};
336 #endif
337 
338 #endif
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_u(double u)
Definition: vpImagePoint.h:328
void set_v(double v)
Definition: vpImagePoint.h:339
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:73
virtual ~vpKltOpencv()
Definition: vpKltOpencv.cpp:91
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:389
int m_useHarrisDetector
true to use Harris detector
Definition: vpKltOpencv.h:398
int m_maxCount
Max number of keypoints.
Definition: vpKltOpencv.h:390
int m_blockSize
Block size.
Definition: vpKltOpencv.h:397
void track(const cv::Mat &I)
double m_minDistance
Mins distance between keypoints.
Definition: vpKltOpencv.h:394
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:68
cv::TermCriteria m_termcrit
Term criteria.
Definition: vpKltOpencv.h:391
double m_minEigThreshold
Min eigen threshold.
Definition: vpKltOpencv.h:395
void getFeature(const int &index, long &id, float &x, float &y) const
int m_pyrMaxLevel
Pyramid max level.
Definition: vpKltOpencv.h:399
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
Definition: vpKltOpencv.h:388
double m_qualityLevel
Quality level.
Definition: vpKltOpencv.h:393
void suppressFeature(const int &index)
bool m_initial_guess
true when initial guess is provided
Definition: vpKltOpencv.h:401
cv::Mat m_gray
Gray image.
Definition: vpKltOpencv.h:386
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void addFeature(const float &x, const float &y)
double m_harris_k
Harris parameter.
Definition: vpKltOpencv.h:396
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
Definition: vpKltOpencv.cpp:93
int m_winSize
Window criteria.
Definition: vpKltOpencv.h:392
long m_next_points_id
Id for the newt keypoint.
Definition: vpKltOpencv.h:400
cv::Mat m_prevGray
Previous gray image.
Definition: vpKltOpencv.h:387
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
static int round(double x)
Definition: vpMath.h:323
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.