Visual Servoing Platform  version 3.6.1 under development (2024-10-10)
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 
52 BEGIN_VISP_NAMESPACE
54  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
55  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
56  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
57 {
58  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
59 }
60 
62  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
63  m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
64  m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
65 {
66  *this = copy;
67 }
68 
70 {
71  m_gray = copy.m_gray;
72  m_prevGray = copy.m_prevGray;
73  m_points[0] = copy.m_points[0];
74  m_points[1] = copy.m_points[1];
75  m_points_id = copy.m_points_id;
76  m_maxCount = copy.m_maxCount;
77  m_termcrit = copy.m_termcrit;
78  m_winSize = copy.m_winSize;
82  m_harris_k = copy.m_harris_k;
83  m_blockSize = copy.m_blockSize;
88 
89  return *this;
90 }
91 
93 
94 void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
95 {
96  m_next_points_id = 0;
97 
98  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
99  I.copyTo(m_gray);
100 
101  for (size_t i = 0; i < 2; i++) {
102  m_points[i].clear();
103  }
104 
105  m_points_id.clear();
106 
107  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false,
108  m_harris_k);
109 
110  if (m_points[1].size() > 0) {
111  cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
112 
113  for (size_t i = 0; i < m_points[1].size(); i++)
114  m_points_id.push_back(m_next_points_id++);
115  }
116 }
117 
118 void vpKltOpencv::track(const cv::Mat &I)
119 {
120  if (m_points[1].size() == 0)
121  throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
122 
123  std::vector<float> err;
124  int flags = 0;
125 
126  cv::swap(m_prevGray, m_gray);
127 
128  if (m_initial_guess) {
129  flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
130  m_initial_guess = false;
131  }
132  else {
133  std::swap(m_points[1], m_points[0]);
134  }
135 
136  // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
137  I.copyTo(m_gray);
138 
139  if (m_prevGray.empty()) {
140  m_gray.copyTo(m_prevGray);
141  }
142 
143  std::vector<uchar> status;
144 
145  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
147 
148  // Remove points that are lost
149  for (int i = (int)status.size() - 1; i >= 0; i--) {
150  if (status[(size_t)i] == 0) { // point is lost
151  m_points[0].erase(m_points[0].begin() + i);
152  m_points[1].erase(m_points[1].begin() + i);
153  m_points_id.erase(m_points_id.begin() + i);
154  }
155  }
156 }
157 
158 void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
159 {
160  if ((size_t)index >= m_points[1].size()) {
161  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
162  }
163 
164  x = m_points[1][(size_t)index].x;
165  y = m_points[1][(size_t)index].y;
166  id = m_points_id[(size_t)index];
167 }
168 
169 void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
170 {
171  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
172 }
173 
174 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
175  const vpColor &color, unsigned int thickness)
176 {
177  vpImagePoint ip;
178  for (size_t i = 0; i < features.size(); i++) {
179  ip.set_u(vpMath::round(features[i].x));
180  ip.set_v(vpMath::round(features[i].y));
181  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
182  }
183 }
184 
185 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
186  unsigned int thickness)
187 {
188  vpImagePoint ip;
189  for (size_t i = 0; i < features.size(); i++) {
190  ip.set_u(vpMath::round(features[i].x));
191  ip.set_v(vpMath::round(features[i].y));
192  vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
193  }
194 }
195 
196 void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
197  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
198 {
199  vpImagePoint ip;
200  for (size_t i = 0; i < features.size(); i++) {
201  ip.set_u(vpMath::round(features[i].x));
202  ip.set_v(vpMath::round(features[i].y));
203  vpDisplay::displayCross(I, ip, 10, color, thickness);
204 
205  std::ostringstream id;
206  id << featuresid[i];
207  ip.set_u(vpMath::round(features[i].x + 5));
208  vpDisplay::displayText(I, ip, id.str(), color);
209  }
210 }
211 
212 void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
213  const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
214 {
215  vpImagePoint ip;
216  for (size_t i = 0; i < features.size(); i++) {
217  ip.set_u(vpMath::round(features[i].x));
218  ip.set_v(vpMath::round(features[i].y));
219  vpDisplay::displayCross(I, ip, 10, color, thickness);
220 
221  std::ostringstream id;
222  id << featuresid[i];
223  ip.set_u(vpMath::round(features[i].x + 5));
224  vpDisplay::displayText(I, ip, id.str(), color);
225  }
226 }
227 
228 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
229 {
230  if (guess_pts.size() != m_points[1].size()) {
232  "Cannot set initial guess: size feature vector [%d] "
233  "and guess vector [%d] doesn't match",
234  m_points[1].size(), guess_pts.size()));
235  }
236 
237  m_points[1] = guess_pts;
238  m_initial_guess = true;
239 }
240 
241 void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
242  const std::vector<long> &fid)
243 {
244  if (guess_pts.size() != init_pts.size()) {
246  "Cannot set initial guess: size init vector [%d] and "
247  "guess vector [%d] doesn't match",
248  init_pts.size(), guess_pts.size()));
249  }
250 
251  m_points[0] = init_pts;
252  m_points[1] = guess_pts;
253  m_points_id = fid;
254  m_initial_guess = true;
255 }
256 
257 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
258 {
259  m_initial_guess = false;
260  m_points[1] = pts;
261  m_next_points_id = 0;
262  m_points_id.clear();
263  for (size_t i = 0; i < m_points[1].size(); i++) {
264  m_points_id.push_back(m_next_points_id++);
265  }
266 
267  I.copyTo(m_gray);
268 }
269 
270 void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
271 {
272  m_initial_guess = false;
273  m_points[1] = pts;
274  m_points_id.clear();
275 
276  if (ids.size() != pts.size()) {
277  m_next_points_id = 0;
278  for (size_t i = 0; i < m_points[1].size(); i++)
279  m_points_id.push_back(m_next_points_id++);
280  }
281  else {
282  long max = 0;
283  for (size_t i = 0; i < m_points[1].size(); i++) {
284  m_points_id.push_back(ids[i]);
285  if (ids[i] > max)
286  max = ids[i];
287  }
288  m_next_points_id = max + 1;
289  }
290 
291  I.copyTo(m_gray);
292 }
293 
294 void vpKltOpencv::addFeature(const float &x, const float &y)
295 {
296  cv::Point2f f(x, y);
297  m_points[1].push_back(f);
298  m_points_id.push_back(m_next_points_id++);
299 }
300 
301 void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
302 {
303  cv::Point2f f(x, y);
304  m_points[1].push_back(f);
305  m_points_id.push_back(id);
306  if (id >= m_next_points_id)
307  m_next_points_id = id + 1;
308 }
309 
310 void vpKltOpencv::addFeature(const cv::Point2f &f)
311 {
312  m_points[1].push_back(f);
313  m_points_id.push_back(m_next_points_id++);
314 }
315 
316 void vpKltOpencv::suppressFeature(const int &index)
317 {
318  if ((size_t)index >= m_points[1].size()) {
319  throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
320  }
321 
322  m_points[1].erase(m_points[1].begin() + index);
323  m_points_id.erase(m_points_id.begin() + index);
324 }
325 END_VISP_NAMESPACE
326 #else
327 
328 // Work around to avoid visp_klt library empty when OpenCV is not installed or used
329 class VISP_EXPORT dummy_vpKltOpencv
330 {
331 public:
332  dummy_vpKltOpencv() { };
333 };
334 
335 #if !defined(VISP_BUILD_SHARED_LIBS)
336 // Work around to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no symbols
337 void dummy_vpKltOpenCV_fct() { };
338 #endif
339 
340 #endif
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
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:335
void set_v(double v)
Definition: vpImagePoint.h:346
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:74
virtual ~vpKltOpencv()
Definition: vpKltOpencv.cpp:92
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:390
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1) const
int m_useHarrisDetector
true to use Harris detector
Definition: vpKltOpencv.h:399
int m_maxCount
Max number of keypoints.
Definition: vpKltOpencv.h:391
int m_blockSize
Block size.
Definition: vpKltOpencv.h:398
void track(const cv::Mat &I)
double m_minDistance
Mins distance between keypoints.
Definition: vpKltOpencv.h:395
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:69
cv::TermCriteria m_termcrit
Term criteria.
Definition: vpKltOpencv.h:392
double m_minEigThreshold
Min eigen threshold.
Definition: vpKltOpencv.h:396
void getFeature(const int &index, long &id, float &x, float &y) const
int m_pyrMaxLevel
Pyramid max level.
Definition: vpKltOpencv.h:400
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
Definition: vpKltOpencv.h:389
double m_qualityLevel
Quality level.
Definition: vpKltOpencv.h:394
void suppressFeature(const int &index)
bool m_initial_guess
true when initial guess is provided
Definition: vpKltOpencv.h:402
cv::Mat m_gray
Gray image.
Definition: vpKltOpencv.h:387
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:397
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
Definition: vpKltOpencv.cpp:94
int m_winSize
Window criteria.
Definition: vpKltOpencv.h:393
long m_next_points_id
Id for the newt keypoint.
Definition: vpKltOpencv.h:401
cv::Mat m_prevGray
Previous gray image.
Definition: vpKltOpencv.h:388
static int round(double x)
Definition: vpMath.h:410
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.