Visual Servoing Platform version 3.5.0
vpMeTracker.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Moving edges.
33 *
34 * Authors:
35 * Andrew Comport
36 *
37 *****************************************************************************/
38
44#include <visp3/core/vpColor.h>
45#include <visp3/core/vpDisplay.h>
46#include <visp3/me/vpMeTracker.h>
47
48#include <algorithm>
49#include <visp3/core/vpDebug.h>
50#include <visp3/core/vpTrackingException.h>
51
52#define DEBUG_LEVEL1 0
53#define DEBUG_LEVEL2 0
54
56{
58 p.resize(2);
60}
61
63 : list(), me(NULL), init_range(1), nGoodElement(0), m_mask(NULL), selectDisplay(vpMeSite::NONE)
64#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
65 ,
66 query_range(0), display_point(false)
67#endif
68{
69 init();
70}
71
73 : vpTracker(meTracker), list(), me(NULL), init_range(1), nGoodElement(0), m_mask(NULL), selectDisplay(vpMeSite::NONE)
74#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
75 ,
76 query_range(0), display_point(false)
77#endif
78{
79 init();
80
81 me = meTracker.me;
82 list = meTracker.list;
83 nGoodElement = meTracker.nGoodElement;
84 init_range = meTracker.init_range;
85 selectDisplay = meTracker.selectDisplay;
86
87#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
88 display_point = meTracker.display_point;
89 query_range = meTracker.query_range;
90#endif
91}
92
97{
98 nGoodElement = 0;
99 list.clear();
100}
101
103
105{
106 list = p_me.list;
107 me = p_me.me;
109 init_range = p_me.init_range;
111#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
114#endif
115 return *this;
116}
117
118static bool isSuppressZero(const vpMeSite &P) { return (P.getState() == vpMeSite::NO_SUPPRESSION); }
119
121{
122 unsigned int number_signal = 0;
123
124 // Loop through all the points tracked from the contour
125 number_signal = static_cast<unsigned int>(std::count_if(list.begin(), list.end(), isSuppressZero));
126 return number_signal;
127}
128
129unsigned int vpMeTracker::totalNumberOfSignal() { return (unsigned int)list.size(); }
130
140bool vpMeTracker::inMask(const vpImage<bool> *mask, unsigned int i, unsigned int j)
141{
142 try {
143 return (mask == NULL || mask->getValue(i, j));
144 }
145 catch (vpException &) {
146 return false;
147 }
148}
149
150int vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
151{
152 return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
153}
154
155int vpMeTracker::outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
156{
157 int i = vpMath::round(iP.get_i());
158 int j = vpMath::round(iP.get_j());
159 return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
160}
161
170{
171 if (!me) {
172 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
173 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
174 }
175
176 // Must set range to 0
177 unsigned int range_tmp = me->getRange();
179
180 nGoodElement = 0;
181
182 int d = 0;
183
184 // Loop through list of sites to track
185 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
186 vpMeSite refp = *it; // current reference pixel
187
188 d++;
189 // If element hasn't been suppressed
190 if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
191 try {
192 refp.track(I, me, false);
193 } catch (...) {
194 // EM verifier quel signal est de sortie !!!
195 vpERROR_TRACE("Error caught");
196 throw;
197 }
199 nGoodElement++;
200 }
201
202#if (DEBUG_LEVEL2)
203 {
204 vpImagePoint ip1, ip2;
205 double a, b;
206 a = refp.i_1 - refp.i;
207 b = refp.j_1 - refp.j;
208 if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
209 ip1.set_i(refp.i);
210 ip1.set_j(refp.j);
211 ip2.set_i(refp.i + a);
212 ip2.set_j(refp.j + b);
214 }
215 }
216#endif
217 *it = refp;
218 }
219
220 /*
221 if (res != OK)
222 {
223 std::cout<< "In vpMeTracker::initTracking(): " ;
224 switch (res)
225 {
226 case ERR_TRACKING:
227 std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " <<
228 std::endl ; break ; case fatalError: std::cout <<
229 "vpMeTracker::initTracking:Track return fatalError" << std::endl ; break ;
230 default:
231 std::cout << "vpMeTracker::initTracking:Track return error " << res <<
232 std::endl ;
233 }
234 return res ;
235 }
236 */
237
238 me->setRange(range_tmp);
239}
240
251{
252 if (!me) {
253 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
254 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
255 }
256
257 if (list.empty()) {
258 vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
259 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Too few pixel to track"));
260 }
261
262 nGoodElement = 0;
263
264 // Loop through list of sites to track
265 std::list<vpMeSite>::iterator it = list.begin();
266 while (it != list.end()) {
267 vpMeSite s = *it; // current reference pixel
268
269 // If element hasn't been suppressed
271
272 try {
273 s.track(I, me, true);
274 } catch (...) {
276 }
277
278 if (vpMeTracker::inMask(m_mask, s.i, s.j)) {
279 if (s.getState() != vpMeSite::THRESHOLD) {
280 nGoodElement++;
281
282#if (DEBUG_LEVEL2)
283 {
284 double a, b;
285 a = s.i_1 - s.i;
286 b = s.j_1 - s.j;
288 ip1.set_i(s.i);
289 ip1.set_j(s.j);
290 ip2.set_i(s.i + a * 5);
291 ip2.set_j(s.j + b * 5);
293 }
294 }
295#endif
296 }
297 *it = s;
298 ++it;
299 }
300 else {
301 // Site outside mask: it is no more tracked.
302 it = list.erase(it);
303 }
304 }
305 else {
306 ++it;
307 }
308 }
309}
310
327{
328#if (DEBUG_LEVEL1)
329 {
330 std::cout << "begin vpMeTracker::displayList() " << std::endl;
331 std::cout << " There are " << list.size() << " sites in the list " << std::endl;
332 }
333#endif
334 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
335 vpMeSite p_me = *it;
336 p_me.display(I);
337 }
338}
339
341{
342 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
343 vpMeSite p_me = *it;
344 p_me.display(I);
345 }
346}
347
354void vpMeTracker::display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w)
355{
356 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
357 vpMeSite P = *it;
358
360 P.weight = w[index_w];
361 index_w++;
362 }
363
364 *it = P;
365 }
366 display(I);
367}
368
369#undef DEBUG_LEVEL1
370#undef DEBUG_LEVEL2
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static const vpColor black
Definition: vpColor.h:211
static const vpColor green
Definition: vpColor.h:220
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_i() const
Definition: vpImagePoint.h:203
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
static int round(double x)
Definition: vpMath.h:247
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
double weight
Definition: vpMeSite.h:99
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:601
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
@ NONE
Definition: vpMeSite.h:74
int i
Definition: vpMeSite.h:87
int j_1
Definition: vpMeSite.h:88
vpMeSiteState getState() const
Definition: vpMeSite.h:190
int i_1
Definition: vpMeSite.h:88
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:66
void initTracking(const vpImage< unsigned char > &I)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMeTracker.h:84
unsigned int numberOfSignal()
virtual ~vpMeTracker()
int nGoodElement
Definition: vpMeTracker.h:82
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
unsigned int totalNumberOfSignal()
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
unsigned int init_range
Definition: vpMeTracker.h:81
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
bool display_point
Definition: vpMeTracker.h:193
vpMeTracker & operator=(vpMeTracker &f)
void reset()
Definition: vpMeTracker.cpp:96
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void setRange(const unsigned int &r)
Definition: vpMe.h:271
unsigned int getRange() const
Definition: vpMe.h:179
Class that defines what is a feature generic tracker.
Definition: vpTracker.h:65
void init()
Default initialization.
Definition: vpTracker.cpp:47
vpColVector p
Definition: vpTracker.h:73
Error that can be emited by the vpTracker class and its derivates.
#define vpDERROR_TRACE
Definition: vpDebug.h:464
#define vpERROR_TRACE
Definition: vpDebug.h:393