Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
vpMeTracker.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2019 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  * Moving edges.
32  */
33 
39 #include <visp3/core/vpColor.h>
40 #include <visp3/core/vpDisplay.h>
41 #include <visp3/me/vpMeTracker.h>
42 
43 #include <algorithm>
44 #include <visp3/core/vpDebug.h>
45 #include <visp3/core/vpTrackingException.h>
46 
47 #define DEBUG_LEVEL1 0
48 #define DEBUG_LEVEL2 0
49 
51 {
53  p.resize(2);
55 }
56 
58  : m_meList(), m_me(nullptr), m_init_range(1), m_nGoodElement(0), m_mask(nullptr), m_maskCandidates(nullptr), m_selectDisplay(vpMeSite::NONE)
59 {
60  init();
61 }
62 
64  : vpTracker(meTracker), m_meList(), m_me(nullptr), m_init_range(1), m_nGoodElement(0), m_mask(nullptr), m_maskCandidates(nullptr), m_selectDisplay(vpMeSite::NONE)
65 {
66  init();
67 
68  m_me = meTracker.m_me;
69  m_meList = meTracker.m_meList;
70  m_nGoodElement = meTracker.m_nGoodElement;
71  m_init_range = meTracker.m_init_range;
72  m_selectDisplay = meTracker.m_selectDisplay;
73 }
74 
76 {
77  m_nGoodElement = 0;
78  m_meList.clear();
79 }
80 
82 
84 {
85  m_meList = meTracker.m_meList;
86  m_me = meTracker.m_me;
87  m_selectDisplay = meTracker.m_selectDisplay;
88  m_init_range = meTracker.m_init_range;
89  m_nGoodElement = meTracker.m_nGoodElement;
90  return *this;
91 }
92 
93 static bool isSuppressZero(const vpMeSite &P) { return (P.getState() == vpMeSite::NO_SUPPRESSION); }
94 
96 {
97  unsigned int number_signal = 0;
98 
99  // Loop through all the points tracked from the contour
100  number_signal = static_cast<unsigned int>(std::count_if(m_meList.begin(), m_meList.end(), isSuppressZero));
101  return number_signal;
102 }
103 
104 unsigned int vpMeTracker::totalNumberOfSignal() { return static_cast<unsigned int>(m_meList.size()); }
105 
106 bool vpMeTracker::inRoiMask(const vpImage<bool> *mask, unsigned int i, unsigned int j)
107 {
108  try {
109  return ((mask == nullptr) || (mask->getValue(i, j)));
110  }
111  catch (vpException &) {
112  return false;
113  }
114 }
115 
116 bool vpMeTracker::inMeMaskCandidates(const vpImage<bool> *meMaskCandidates, unsigned int i, unsigned int j)
117 {
118  if (meMaskCandidates == nullptr) {
119  return true;
120  }
121  else {
122  const unsigned int kernelSize = 3;
123  const unsigned int halfKernelSize = (kernelSize - 1) / 2;
124  const unsigned int nbRows = meMaskCandidates->getRows();
125  const unsigned int nbCols = meMaskCandidates->getCols();
126 
127  if ((i >= nbRows) || (j >= nbCols)) {
128  // The asked point is outside the mask
129  return false;
130  }
131  if ((*meMaskCandidates)[i][j]) {
132  // The asked point is a candidate
133  return true;
134  }
135  unsigned int iStart = 0, jStart = 0;
136  unsigned int iStop = nbRows - 1, jStop = nbCols - 1;
137  // Ensuring we won't go outside the limits of the mask
138  if (i >= halfKernelSize) {
139  iStart = i - halfKernelSize;
140  }
141  if (j >= halfKernelSize) {
142  jStart = j - halfKernelSize;
143  }
144  if ((i + halfKernelSize) < nbRows) {
145  iStop = i + halfKernelSize;
146  }
147  if ((j + halfKernelSize) < nbCols) {
148  jStop = j + halfKernelSize;
149  }
150  // Looking in its neighborhood
151  bool isACandidate = false;
152  unsigned int iter_i = iStart, iter_j = jStart;
153 
154  while ((!isACandidate) && (iter_i <= iStop)) {
155  iter_j = jStart;
156  while ((!isACandidate) && (iter_j <= jStop)) {
157  isACandidate = (*meMaskCandidates)[iter_i][iter_j];
158  ++iter_j;
159  }
160  ++iter_i;
161  }
162 
163  return isACandidate;
164  }
165 }
166 
167 bool vpMeTracker::outOfImage(int i, int j, int border, int nrows, int ncols)
168 {
169  int borderWith2SparedPixels = border + 2;
170  return (!((i > borderWith2SparedPixels) && (i < (nrows - borderWith2SparedPixels))
171  && (j > borderWith2SparedPixels) && (j < (ncols - borderWith2SparedPixels))
172  ));
173 }
174 
175 bool vpMeTracker::outOfImage(const vpImagePoint &iP, int border, int nrows, int ncols)
176 {
177  const int borderPlus2 = border + 2;
178  int i = vpMath::round(iP.get_i());
179  int j = vpMath::round(iP.get_j());
180  return (!((i > borderPlus2) && (i < (nrows - borderPlus2)) && (j > borderPlus2) && (j < (ncols - borderPlus2))));
181 }
182 
184 {
185  if (!m_me) {
186  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
187  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
188  }
189 
190  // Must set range to 0
191  unsigned int range_tmp = m_me->getRange();
193 
194  m_nGoodElement = 0;
195 
196  // Loop through list of sites to track
197  std::list<vpMeSite>::iterator end = m_meList.end();
198  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != end; ++it) {
199  vpMeSite refp = *it; // current reference pixel
200 
201  // If element hasn't been suppressed
202  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
203 
204  refp.track(I, m_me, false);
205 
206  if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
207  ++m_nGoodElement;
208  }
209  }
210 
211  *it = refp;
212  }
213 
214  m_me->setRange(range_tmp);
215 }
216 
218 {
219  if (!m_me) {
220  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
221  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
222  }
223 
224  if (m_meList.empty()) {
225  vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
226  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Too few pixel to track"));
227  }
228 
229  m_nGoodElement = 0;
230 
231  // Loop through list of sites to track
232  std::list<vpMeSite>::iterator it = m_meList.begin();
233  std::list<vpMeSite>::iterator end = m_meList.end();
234  while (it != end) {
235  vpMeSite s = *it; // current reference pixel
236 
237  // If element hasn't been suppressed
238  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
239  s.track(I, m_me, true);
240 
241 
242  if (vpMeTracker::inRoiMask(m_mask, s.get_i(), s.get_j())) {
243  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
244  ++m_nGoodElement;
245  }
246  }
247  else {
248  // Site outside mask
250  }
251  }
252 
253  *it = s;
254  ++it;
255  }
256 }
257 
259 {
260  std::list<vpMeSite>::const_iterator end = m_meList.end();
261  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != end; ++it) {
262  vpMeSite p_me = *it;
263  p_me.display(I);
264  }
265 }
266 
268 {
269  std::list<vpMeSite>::const_iterator end = m_meList.end();
270  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != end; ++it) {
271  vpMeSite p_me = *it;
272  p_me.display(I);
273  }
274 }
275 
276 void vpMeTracker::display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w)
277 {
278  std::list<vpMeSite>::iterator end = m_meList.end();
279  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != end; ++it) {
280  vpMeSite P = *it;
281 
282  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
283  P.setWeight(w[index_w]);
284  ++index_w;
285  }
286 
287  *it = P;
288  }
289  display(I);
290 }
291 
292 #undef DEBUG_LEVEL1
293 #undef DEBUG_LEVEL2
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
error that can be emitted by ViSP classes.
Definition: vpException.h:59
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1863
unsigned int getCols() const
Definition: vpImage.h:175
unsigned int getRows() const
Definition: vpImage.h:215
static int round(double x)
Definition: vpMath.h:405
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
@ OUTSIDE_ROI_MASK
Point is outside the region of interest mask, but retained in the ME list.
Definition: vpMeSite.h:92
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:83
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:385
void setWeight(const double &weight)
Definition: vpMeSite.h:273
@ NONE
Not displayed.
Definition: vpMeSite.h:72
vpMeSiteState getState() const
Definition: vpMeSite.h:266
int get_j() const
Definition: vpMeSite.h:186
int get_i() const
Definition: vpMeSite.h:180
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:267
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:256
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:60
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:317
unsigned int numberOfSignal()
Definition: vpMeTracker.cpp:95
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
Definition: vpMeTracker.h:321
void track(const vpImage< unsigned char > &I)
unsigned int m_init_range
Initial range.
Definition: vpMeTracker.h:313
unsigned int totalNumberOfSignal()
virtual ~vpMeTracker() vp_override
Definition: vpMeTracker.cpp:81
vpMe * m_me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:311
vpMeTracker & operator=(vpMeTracker &meTracker)
Definition: vpMeTracker.cpp:83
static bool inRoiMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
void display(const vpImage< unsigned char > &I)
int m_nGoodElement
Number of good moving-edges that are tracked.
Definition: vpMeTracker.h:315
bool outOfImage(int i, int j, int border, int nrows, int ncols)
std::list< vpMeSite > m_meList
Definition: vpMeTracker.h:309
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void reset()
Definition: vpMeTracker.cpp:75
void setRange(const unsigned int &range)
Definition: vpMe.h:405
unsigned int getRange() const
Definition: vpMe.h:258
Class that defines what is a feature generic tracker.
Definition: vpTracker.h:59
void init()
Default initialization.
Definition: vpTracker.cpp:44
vpColVector p
Definition: vpTracker.h:67
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ initializationError
Tracker initialization error.
#define vpDERROR_TRACE
Definition: vpDebug.h:452