Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
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  *****************************************************************************/
35 
41 #include <visp3/core/vpColor.h>
42 #include <visp3/core/vpDisplay.h>
43 #include <visp3/me/vpMeTracker.h>
44 
45 #include <algorithm>
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpTrackingException.h>
48 
49 #define DEBUG_LEVEL1 0
50 #define DEBUG_LEVEL2 0
51 
53 {
55  p.resize(2);
57 }
58 
60  : list(), me(NULL), init_range(1), nGoodElement(0), m_mask(NULL), selectDisplay(vpMeSite::NONE)
61 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
62  ,
63  query_range(0), display_point(false)
64 #endif
65 {
66  init();
67 }
68 
70  : vpTracker(meTracker), list(), me(NULL), init_range(1), nGoodElement(0), m_mask(NULL), selectDisplay(vpMeSite::NONE)
71 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
72  ,
73  query_range(0), display_point(false)
74 #endif
75 {
76  init();
77 
78  me = meTracker.me;
79  list = meTracker.list;
80  nGoodElement = meTracker.nGoodElement;
81  init_range = meTracker.init_range;
82  selectDisplay = meTracker.selectDisplay;
83 
84 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
85  display_point = meTracker.display_point;
86  query_range = meTracker.query_range;
87 #endif
88 }
89 
94 {
95  nGoodElement = 0;
96  list.clear();
97 }
98 
100 
102 {
103  list = p_me.list;
104  me = p_me.me;
106  init_range = p_me.init_range;
107  nGoodElement = p_me.nGoodElement;
108 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
109  query_range = p_me.query_range;
111 #endif
112  return *this;
113 }
114 
115 static bool isSuppressZero(const vpMeSite &P) { return (P.getState() == vpMeSite::NO_SUPPRESSION); }
116 
118 {
119  unsigned int number_signal = 0;
120 
121  // Loop through all the points tracked from the contour
122  number_signal = static_cast<unsigned int>(std::count_if(list.begin(), list.end(), isSuppressZero));
123  return number_signal;
124 }
125 
126 unsigned int vpMeTracker::totalNumberOfSignal() { return (unsigned int)list.size(); }
127 
137 bool vpMeTracker::inMask(const vpImage<bool> *mask, unsigned int i, unsigned int j)
138 {
139  try {
140  return (mask == NULL || mask->getValue(i, j));
141  } catch (vpException &) {
142  return false;
143  }
144 }
145 
146 int vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
147 {
148  return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
149 }
150 
151 int vpMeTracker::outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
152 {
153  int i = vpMath::round(iP.get_i());
154  int j = vpMath::round(iP.get_j());
155  return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2))));
156 }
157 
166 {
167  if (!me) {
168  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
169  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
170  }
171 
172  // Must set range to 0
173  unsigned int range_tmp = me->getRange();
175 
176  nGoodElement = 0;
177 
178  // Loop through list of sites to track
179  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
180  vpMeSite refp = *it; // current reference pixel
181 
182  // If element hasn't been suppressed
183  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
184  try {
185  refp.track(I, me, false);
186  } catch (...) {
187  // EM verifier quel signal est de sortie !!!
188  vpERROR_TRACE("Error caught");
189  throw;
190  }
191  if (refp.getState() == vpMeSite::NO_SUPPRESSION)
192  nGoodElement++;
193  }
194 
195 #if (DEBUG_LEVEL2)
196  {
197  vpImagePoint ip1, ip2;
198  double a, b;
199  a = refp.i_1 - refp.i;
200  b = refp.j_1 - refp.j;
201  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
202  ip1.set_i(refp.i);
203  ip1.set_j(refp.j);
204  ip2.set_i(refp.i + a);
205  ip2.set_j(refp.j + b);
207  }
208  }
209 #endif
210  *it = refp;
211  }
212 
213  me->setRange(range_tmp);
214 }
215 
225 {
226  if (!me) {
227  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
228  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
229  }
230 
231  if (list.empty()) {
232  vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
233  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Too few pixel to track"));
234  }
235 
236  nGoodElement = 0;
237 
238  // Loop through list of sites to track
239  std::list<vpMeSite>::iterator it = list.begin();
240  while (it != list.end()) {
241  vpMeSite s = *it; // current reference pixel
242 
243  // If element hasn't been suppressed
244  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
245 
246  try {
247  s.track(I, me, true);
248  } catch (...) {
250  }
251 
252  if (vpMeTracker::inMask(m_mask, s.i, s.j)) {
253  if (s.getState() != vpMeSite::THRESHOLD) {
254  nGoodElement++;
255 
256 #if (DEBUG_LEVEL2)
257  {
258  double a, b;
259  a = s.i_1 - s.i;
260  b = s.j_1 - s.j;
261  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
262  ip1.set_i(s.i);
263  ip1.set_j(s.j);
264  ip2.set_i(s.i + a * 5);
265  ip2.set_j(s.j + b * 5);
267  }
268  }
269 #endif
270  }
271  *it = s;
272  ++it;
273  } else {
274  // Site outside mask: it is no more tracked.
275  it = list.erase(it);
276  }
277  } else {
278  ++it;
279  }
280  }
281 }
282 
299 {
300 #if (DEBUG_LEVEL1)
301  {
302  std::cout << "begin vpMeTracker::displayList() " << std::endl;
303  std::cout << " There are " << list.size() << " sites in the list " << std::endl;
304  }
305 #endif
306  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
307  vpMeSite p_me = *it;
308  p_me.display(I);
309  }
310 }
311 
313 {
314  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
315  vpMeSite p_me = *it;
316  p_me.display(I);
317  }
318 }
319 
326 void vpMeTracker::display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w)
327 {
328  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
329  vpMeSite P = *it;
330 
331  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
332  P.weight = w[index_w];
333  index_w++;
334  }
335 
336  *it = P;
337  }
338  display(I);
339 }
340 
341 #undef DEBUG_LEVEL1
342 #undef DEBUG_LEVEL2
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
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:1590
static int round(double x)
Definition: vpMath.h:321
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:67
int j
Definition: vpMeSite.h:86
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:79
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:74
double weight
Definition: vpMeSite.h:98
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:597
@ NONE
Definition: vpMeSite.h:69
int i
Definition: vpMeSite.h:86
int j_1
Definition: vpMeSite.h:87
vpMeSiteState getState() const
Definition: vpMeSite.h:189
int i_1
Definition: vpMeSite.h:87
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contrast=true)
Definition: vpMeSite.cpp:350
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:62
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:80
unsigned int numberOfSignal()
virtual ~vpMeTracker()
Definition: vpMeTracker.cpp:99
int nGoodElement
Definition: vpMeTracker.h:78
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:86
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:74
unsigned int init_range
Definition: vpMeTracker.h:77
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
bool display_point
Definition: vpMeTracker.h:189
vpMeTracker & operator=(vpMeTracker &f)
virtual void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness)=0
void reset()
Definition: vpMeTracker.cpp:93
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:76
void setRange(const unsigned int &r)
Definition: vpMe.h:330
unsigned int getRange() const
Definition: vpMe.h:237
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