Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
90
92
93void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
94{
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
117void 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
156void 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
167void 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
172void 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
183void 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
194void 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
210void 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
226void 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
239void 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
255void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
256{
257 m_initial_guess = false;
258 m_points[1] = pts;
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
268void 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()) {
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
291void 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
298void 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
307void 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
313void 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
326class VISP_EXPORT dummy_vpKltOpencv
327{
328public:
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
335void 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 ...
void set_u(double u)
void set_v(double v)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition vpKltOpencv.h:73
virtual ~vpKltOpencv()
std::vector< long > m_points_id
Keypoint id.
int m_useHarrisDetector
true to use Harris detector
int m_maxCount
Max number of keypoints.
int m_blockSize
Block size.
void track(const cv::Mat &I)
double m_minDistance
Mins distance between keypoints.
vpKltOpencv & operator=(const vpKltOpencv &copy)
cv::TermCriteria m_termcrit
Term criteria.
double m_minEigThreshold
Min eigen threshold.
void getFeature(const int &index, long &id, float &x, float &y) const
int m_pyrMaxLevel
Pyramid max level.
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
double m_qualityLevel
Quality level.
void suppressFeature(const int &index)
bool m_initial_guess
true when initial guess is provided
cv::Mat m_gray
Gray image.
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void addFeature(const float &x, const float &y)
double m_harris_k
Harris parameter.
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
int m_winSize
Window criteria.
long m_next_points_id
Id for the newt keypoint.
cv::Mat m_prevGray
Previous gray image.
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.