Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
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  } catch (vpException &) {
145  return false;
146  }
147 }
148 
149 int vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
150 {
151  return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
152 }
153 
154 int vpMeTracker::outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
155 {
156  int i = vpMath::round(iP.get_i());
157  int j = vpMath::round(iP.get_j());
158  return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
159 }
160 
169 {
170  if (!me) {
171  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
172  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
173  }
174 
175  // Must set range to 0
176  unsigned int range_tmp = me->getRange();
178 
179  nGoodElement = 0;
180 
181  int d = 0;
182 
183  // Loop through list of sites to track
184  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
185  vpMeSite refp = *it; // current reference pixel
186 
187  d++;
188  // If element hasn't been suppressed
189  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
190  try {
191  refp.track(I, me, false);
192  } catch (...) {
193  // EM verifier quel signal est de sortie !!!
194  vpERROR_TRACE("Error caught");
195  throw;
196  }
197  if (refp.getState() == vpMeSite::NO_SUPPRESSION)
198  nGoodElement++;
199  }
200 
201 #if (DEBUG_LEVEL2)
202  {
203  vpImagePoint ip1, ip2;
204  double a, b;
205  a = refp.i_1 - refp.i;
206  b = refp.j_1 - refp.j;
207  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
208  ip1.set_i(refp.i);
209  ip1.set_j(refp.j);
210  ip2.set_i(refp.i + a);
211  ip2.set_j(refp.j + b);
213  }
214  }
215 #endif
216  *it = refp;
217  }
218 
219  /*
220  if (res != OK)
221  {
222  std::cout<< "In vpMeTracker::initTracking(): " ;
223  switch (res)
224  {
225  case ERR_TRACKING:
226  std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " <<
227  std::endl ; break ; case fatalError: std::cout <<
228  "vpMeTracker::initTracking:Track return fatalError" << std::endl ; break ;
229  default:
230  std::cout << "vpMeTracker::initTracking:Track return error " << res <<
231  std::endl ;
232  }
233  return res ;
234  }
235  */
236 
237  me->setRange(range_tmp);
238 }
239 
250 {
251  if (!me) {
252  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
253  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
254  }
255 
256  if (list.empty()) {
257  vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
258  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Too few pixel to track"));
259  }
260 
261  nGoodElement = 0;
262 
263  // Loop through list of sites to track
264  std::list<vpMeSite>::iterator it = list.begin();
265  while (it != list.end()) {
266  vpMeSite s = *it; // current reference pixel
267 
268  // If element hasn't been suppressed
269  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
270 
271  try {
272  s.track(I, me, true);
273  } catch (...) {
275  }
276 
277  if (vpMeTracker::inMask(m_mask, s.i, s.j)) {
278  if (s.getState() != vpMeSite::THRESHOLD) {
279  nGoodElement++;
280 
281 #if (DEBUG_LEVEL2)
282  {
283  double a, b;
284  a = s.i_1 - s.i;
285  b = s.j_1 - s.j;
286  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
287  ip1.set_i(s.i);
288  ip1.set_j(s.j);
289  ip2.set_i(s.i + a * 5);
290  ip2.set_j(s.j + b * 5);
292  }
293  }
294 #endif
295  }
296  *it = s;
297  ++it;
298  } else {
299  // Site outside mask: it is no more tracked.
300  it = list.erase(it);
301  }
302  } else {
303  ++it;
304  }
305  }
306 }
307 
324 {
325 #if (DEBUG_LEVEL1)
326  {
327  std::cout << "begin vpMeTracker::displayList() " << std::endl;
328  std::cout << " There are " << list.size() << " sites in the list " << std::endl;
329  }
330 #endif
331  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
332  vpMeSite p_me = *it;
333  p_me.display(I);
334  }
335 }
336 
338 {
339  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
340  vpMeSite p_me = *it;
341  p_me.display(I);
342  }
343 }
344 
351 void vpMeTracker::display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w)
352 {
353  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
354  vpMeSite P = *it;
355 
356  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
357  P.weight = w[index_w];
358  index_w++;
359  }
360 
361  *it = P;
362  }
363  display(I);
364 }
365 
366 #undef DEBUG_LEVEL1
367 #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:314
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:89
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:132
void set_i(double ii)
Definition: vpImagePoint.h:298
double get_i() const
Definition: vpImagePoint.h:121
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1343
static int round(double x)
Definition: vpMath.h:318
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