Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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;
110  nGoodElement = p_me.nGoodElement;
111 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
112  query_range = p_me.query_range;
114 #endif
115  return *this;
116 }
117 
118 static 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 
129 unsigned int vpMeTracker::totalNumberOfSignal() { return (unsigned int)list.size(); }
130 
140 bool 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 
150 int 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 
155 int 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  }
198  if (refp.getState() == vpMeSite::NO_SUPPRESSION)
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
270  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
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;
287  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
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 
354 void 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 
359  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
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
double get_i() const
Definition: vpImagePoint.h:203
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:601
unsigned int numberOfSignal()
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
unsigned int totalNumberOfSignal()
static const vpColor black
Definition: vpColor.h:211
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
#define vpERROR_TRACE
Definition: vpDebug.h:393
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
int i
Definition: vpMeSite.h:87
error that can be emited by ViSP classes.
Definition: vpException.h:71
void reset()
Definition: vpMeTracker.cpp:96
static const vpColor green
Definition: vpColor.h:220
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMeTracker.h:84
Class that defines what is a feature generic tracker.
Definition: vpTracker.h:64
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
Error that can be emited by the vpTracker class and its derivates.
Point used by the tracker.
Definition: vpMeSite.h:78
void set_i(double ii)
Definition: vpImagePoint.h:166
int outOfImage(int i, int j, int half, int row, int cols)
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)
double get_j() const
Definition: vpImagePoint.h:214
vpMeTracker & operator=(vpMeTracker &f)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
bool display_point
Definition: vpMeTracker.h:193
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:65
int nGoodElement
Definition: vpMeTracker.h:82
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
int j_1
Definition: vpMeSite.h:88
static int round(double x)
Definition: vpMath.h:247
void set_j(double jj)
Definition: vpImagePoint.h:177
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
#define vpDERROR_TRACE
Definition: vpDebug.h:464
int j
Definition: vpMeSite.h:87
double weight
Definition: vpMeSite.h:99
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
virtual ~vpMeTracker()
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
void initTracking(const vpImage< unsigned char > &I)
void init()
Default initialization.
Definition: vpTracker.cpp:47
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setRange(const unsigned int &r)
Definition: vpMe.h:271
int i_1
Definition: vpMeSite.h:88
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
unsigned int getRange() const
Definition: vpMe.h:179
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
unsigned int init_range
Definition: vpMeTracker.h:81
vpColVector p
Definition: vpTracker.h:73