Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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  }
131  else {
132  std::swap(m_points[1], m_points[0]);
133  }
134 
135  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
136  I.copyTo(m_gray);
137 
138  if (m_prevGray.empty()) {
139  m_gray.copyTo(m_prevGray);
140  }
141 
142  std::vector<uchar> status;
143 
144  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
146 
147  // Remove points that are lost
148  for (int i = (int)status.size() - 1; i >= 0; i--) {
149  if (status[(size_t)i] == 0) { // point is lost
150  m_points[0].erase(m_points[0].begin() + i);
151  m_points[1].erase(m_points[1].begin() + i);
152  m_points_id.erase(m_points_id.begin() + i);
153  }
154  }
155 }
156 
157 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
158 {
159  if ((size_t)index >= m_points[1].size()) {
160  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
161  }
162 
163  x = m_points[1][(size_t)index].x;
164  y = m_points[1][(size_t)index].y;
165  id = m_points_id[(size_t)index];
166 }
167 
168 void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
169 {
170  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
171 }
172 
173 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
174  const vpColor &color, unsigned int thickness)
175 {
176  vpImagePoint ip;
177  for (size_t i = 0; i < features.size(); i++) {
178  ip.set_u(vpMath::round(features[i].x));
179  ip.set_v(vpMath::round(features[i].y));
180  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
181  }
182 }
183 
184 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
185  unsigned int thickness)
186 {
187  vpImagePoint ip;
188  for (size_t i = 0; i < features.size(); i++) {
189  ip.set_u(vpMath::round(features[i].x));
190  ip.set_v(vpMath::round(features[i].y));
191  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
192  }
193 }
194 
195 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
196  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
197 {
198  vpImagePoint ip;
199  for (size_t i = 0; i < features.size(); i++) {
200  ip.set_u(vpMath::round(features[i].x));
201  ip.set_v(vpMath::round(features[i].y));
202  vpDisplay::displayCross(I, ip, 10, color, thickness);
203 
204  std::ostringstream id;
205  id << featuresid[i];
206  ip.set_u(vpMath::round(features[i].x + 5));
207  vpDisplay::displayText(I, ip, id.str(), color);
208  }
209 }
210 
211 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
212  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
213 {
214  vpImagePoint ip;
215  for (size_t i = 0; i < features.size(); i++) {
216  ip.set_u(vpMath::round(features[i].x));
217  ip.set_v(vpMath::round(features[i].y));
218  vpDisplay::displayCross(I, ip, 10, color, thickness);
219 
220  std::ostringstream id;
221  id << featuresid[i];
222  ip.set_u(vpMath::round(features[i].x + 5));
223  vpDisplay::displayText(I, ip, id.str(), color);
224  }
225 }
226 
227 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
228 {
229  if (guess_pts.size() != m_points[1].size()) {
231  "Cannot set initial guess: size feature vector [%d] "
232  "and guess vector [%d] doesn't match",
233  m_points[1].size(), guess_pts.size()));
234  }
235 
236  m_points[1] = guess_pts;
237  m_initial_guess = true;
238 }
239 
240 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
241  const std::vector<long> &fid)
242 {
243  if (guess_pts.size() != init_pts.size()) {
245  "Cannot set initial guess: size init vector [%d] and "
246  "guess vector [%d] doesn't match",
247  init_pts.size(), guess_pts.size()));
248  }
249 
250  m_points[0] = init_pts;
251  m_points[1] = guess_pts;
252  m_points_id = fid;
253  m_initial_guess = true;
254 }
255 
256 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
257 {
258  m_initial_guess = false;
259  m_points[1] = pts;
260  m_next_points_id = 0;
261  m_points_id.clear();
262  for (size_t i = 0; i < m_points[1].size(); i++) {
263  m_points_id.push_back(m_next_points_id++);
264  }
265 
266  I.copyTo(m_gray);
267 }
268 
269 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
270 {
271  m_initial_guess = false;
272  m_points[1] = pts;
273  m_points_id.clear();
274 
275  if (ids.size() != pts.size()) {
276  m_next_points_id = 0;
277  for (size_t i = 0; i < m_points[1].size(); i++)
278  m_points_id.push_back(m_next_points_id++);
279  }
280  else {
281  long max = 0;
282  for (size_t i = 0; i < m_points[1].size(); i++) {
283  m_points_id.push_back(ids[i]);
284  if (ids[i] > max)
285  max = ids[i];
286  }
287  m_next_points_id = max + 1;
288  }
289 
290  I.copyTo(m_gray);
291 }
292 
293 void vpKltOpencv::addFeature(const float &x, const float &y)
294 {
295  cv::Point2f f(x, y);
296  m_points[1].push_back(f);
297  m_points_id.push_back(m_next_points_id++);
298 }
299 
300 void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
301 {
302  cv::Point2f f(x, y);
303  m_points[1].push_back(f);
304  m_points_id.push_back(id);
305  if (id >= m_next_points_id)
306  m_next_points_id = id + 1;
307 }
308 
309 void vpKltOpencv::addFeature(const cv::Point2f &f)
310 {
311  m_points[1].push_back(f);
312  m_points_id.push_back(m_next_points_id++);
313 }
314 
315 void vpKltOpencv::suppressFeature(const int &index)
316 {
317  if ((size_t)index >= m_points[1].size()) {
318  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
319  }
320 
321  m_points[1].erase(m_points[1].begin() + index);
322  m_points_id.erase(m_points_id.begin() + index);
323 }
324 
325 #else
326 
327 // Work around to avoid visp_klt library empty when OpenCV is not installed or used
328 class VISP_EXPORT dummy_vpKltOpencv
329 {
330 public:
331  dummy_vpKltOpencv() { };
332 };
333 
334 #if !defined(VISP_BUILD_SHARED_LIBS)
335 // Work around to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no symbols
336 void dummy_vpKltOpenCV_fct() { };
337 #endif
338 
339 #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:330
void set_v(double v)
Definition: vpImagePoint.h:341
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:403
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.