Visual Servoing Platform  version 3.5.1 under development (2022-09-25)
vpMbtMeEllipse.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  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #ifndef DOXYGEN_SHOULD_SKIP_THIS
40 
41 #include <visp3/mbt/vpMbtMeEllipse.h>
42 
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpRobust.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/me/vpMe.h>
48 
49 #include <algorithm> // (std::min)
50 #include <cmath> // std::fabs
51 #include <limits> // numeric_limits
52 
56 vpMbtMeEllipse::vpMbtMeEllipse() : vpMeEllipse() {}
57 
61 vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &me_ellipse) : vpMeEllipse(me_ellipse) {}
65 vpMbtMeEllipse::~vpMbtMeEllipse() {}
66 
82 void vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char> &I, double &sumErrorRad,
83  unsigned int &nbFeatures, const vpMatrix &SobelX, const vpMatrix &SobelY,
84  bool display, unsigned int length, unsigned int thickness)
85 {
86  sumErrorRad = 0;
87  nbFeatures = 0;
88 
89  double offset = static_cast<double>(std::floor(SobelX.getRows() / 2.0f));
90  int height = static_cast<int>(I.getHeight());
91  int width = static_cast<int>(I.getWidth());
92 
93  double max_iImg = height - 1.;
94  double max_jImg = width - 1.;
95 
96  vpColVector vecSite(2);
97  vpColVector vecGrad(2);
98 
99  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
100  double iSite = it->ifloat;
101  double jSite = it->jfloat;
102 
103  if (!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height, width)) { // Check if necessary
104  // The tangent angle to the ellipse at a site
105  double theta = computeTheta(vpImagePoint(iSite, jSite));
106 
107  vecSite[0] = cos(theta);
108  vecSite[1] = sin(theta);
109  vecSite.normalize();
110 
111  double gradientX = 0;
112  double gradientY = 0;
113 
114  for (unsigned int i = 0; i < SobelX.getRows(); i++) {
115  double iImg = iSite + (i - offset);
116  for (unsigned int j = 0; j < SobelX.getCols(); j++) {
117  double jImg = jSite + (j - offset);
118 
119  if (iImg < 0)
120  iImg = 0.0;
121  if (jImg < 0)
122  jImg = 0.0;
123 
124  if (iImg > max_iImg)
125  iImg = max_iImg;
126  if (jImg > max_jImg)
127  jImg = max_jImg;
128 
129  gradientX += SobelX[i][j] * I((unsigned int)iImg, (unsigned int)jImg);
130  }
131  }
132 
133  for (unsigned int i = 0; i < SobelY.getRows(); i++) {
134  double iImg = iSite + (i - offset);
135  for (unsigned int j = 0; j < SobelY.getCols(); j++) {
136  double jImg = jSite + (j - offset);
137 
138  if (iImg < 0)
139  iImg = 0.0;
140  if (jImg < 0)
141  jImg = 0.0;
142 
143  if (iImg > max_iImg)
144  iImg = max_iImg;
145  if (jImg > max_jImg)
146  jImg = max_jImg;
147 
148  gradientY += SobelY[i][j] * I((unsigned int)iImg, (unsigned int)jImg);
149  }
150  }
151 
152  double angle = atan2(gradientY, gradientX);
153  while (angle < 0)
154  angle += M_PI;
155  while (angle > M_PI)
156  angle -= M_PI;
157 
158  vecGrad[0] = cos(angle);
159  vecGrad[1] = sin(angle);
160  vecGrad.normalize();
161 
162  double angle1 = acos(vecSite * vecGrad);
163  double angle2 = acos(vecSite * (-vecGrad));
164 
165  if (display) {
166  vpDisplay::displayArrow(I, it->get_i(), it->get_j(), static_cast<int>(it->get_i() + length * cos(theta)),
167  static_cast<int>(it->get_j() + length * sin(theta)), vpColor::blue,
168  length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
169  if (angle1 < angle2) {
170  vpDisplay::displayArrow(I, it->get_i(), it->get_j(), static_cast<int>(it->get_i() + length * cos(angle)),
171  static_cast<int>(it->get_j() + length * sin(angle)), vpColor::red,
172  length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
173  } else {
174  vpDisplay::displayArrow(I, it->get_i(), it->get_j(),
175  static_cast<int>(it->get_i() + length * cos(angle + M_PI)),
176  static_cast<int>(it->get_j() + length * sin(angle + M_PI)), vpColor::red,
177  length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
178  }
179  }
180 
181  sumErrorRad += (std::min)(angle1, angle2);
182 
183  nbFeatures++;
184  }
185  }
186 }
187 
188 void vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &center_p, double n20_p,
189  double n11_p, double n02_p, bool doNotTrack, vpImagePoint *pt1,
190  const vpImagePoint *pt2)
191 {
192  if (pt1 != NULL && pt2 != NULL) {
193  m_trackArc = true;
194  }
195 
196  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
197  m_uc = center_p.get_u();
198  m_vc = center_p.get_v();
199  m_n20 = n20_p;
200  m_n11 = n11_p;
201  m_n02 = n02_p;
202 
203  computeAbeFromNij();
204  computeKiFromNij();
205 
206  if (m_trackArc) {
207  alpha1 = computeAngleOnEllipse(*pt1);
208  alpha2 = computeAngleOnEllipse(*pt2);
209  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
210  alpha2 += 2.0 * M_PI;
211  }
212  // useful for track(I)
213  iP1 = *pt1;
214  iP2 = *pt2;
215  } else {
216  alpha1 = 0.0;
217  alpha2 = 2.0 * M_PI;
218  // useful for track(I)
219  vpImagePoint ip;
220  computePointOnEllipse(alpha1, ip);
221  iP1 = iP2 = ip;
222  }
223  // useful for display(I) so useless if no display before track(I)
224  iPc.set_uv(m_uc, m_vc);
225 
226  sample(I, doNotTrack);
227 
228  try {
229  if (!doNotTrack)
230  track(I);
231  } catch (const vpException &exception) {
232  throw(exception);
233  }
234 }
235 
241 void vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
242 {
243  try {
245  if (m_mask != NULL) {
246  // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask.
247  m_expectedDensity = static_cast<unsigned int>(list.size());
248  }
249  } catch (const vpException &exception) {
250  throw(exception);
251  }
252 }
253 
257 void vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &center_p, double n20_p,
258  double n11_p, double n02_p)
259 {
260  m_uc = center_p.get_u();
261  m_vc = center_p.get_v();
262  m_n20 = n20_p;
263  m_n11 = n11_p;
264  m_n02 = n02_p;
265 
266  computeAbeFromNij();
267  computeKiFromNij();
268 
269  suppressPoints();
270  reSample(I);
271 
272  // remet a jour l'angle delta pour chaque point de la liste
273  updateTheta();
274 }
275 
289 void vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
290 {
291  if (!me) {
292  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
293  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
294  }
295 
296  unsigned int n = numberOfSignal();
297  if ((double)n < 0.9 * m_expectedDensity) {
298  sample(I);
300  }
301 }
302 
315 void vpMbtMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
316 {
317  // Warning: similar code in vpMeEllipse::sample() except for display that is removed here
318  if (!me) {
319  throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
320  }
321  // Delete old lists
322  list.clear();
323  angle.clear();
324 
325  int nbrows = static_cast<int>(I.getHeight());
326  int nbcols = static_cast<int>(I.getWidth());
327 
328  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
329  std::cout << "In vpMeEllipse::sample: ";
330  std::cout << "function called with sample step = 0, set to 10 dg";
331  me->setSampleStep(10.0);
332  }
333  double incr = vpMath::rad(me->getSampleStep()); // angle increment
334  // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
335  m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
336 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
337  expecteddensity = static_cast<double>(m_expectedDensity);
338 #endif
339 
340  // starting angle for sampling
341  double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr) / 2.0;
342  // sample positions
343  for (unsigned int i = 0; i < m_expectedDensity; i++) {
344  vpImagePoint iP;
345  computePointOnEllipse(ang, iP);
346  // If point is in the image, add to the sample list
347  // Check done in (i,j) frame)
348  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
349  double theta = computeTheta(iP);
350  vpMeSite pix;
351  // (i,j) frame used for vpMeSite
352  pix.init(iP.get_i(), iP.get_j(), theta);
353  pix.setDisplay(selectDisplay);
355  list.push_back(pix);
356  angle.push_back(ang);
357  }
358  ang += incr;
359  }
360  if (!doNotTrack) {
362  }
363 }
364 
369 void vpMbtMeEllipse::suppressPoints()
370 {
371  // Loop through list of sites to track
372  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
373  vpMeSite s = *it; // current reference pixel
375  it = list.erase(it);
376  else
377  ++it;
378  }
379 }
380 
381 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
unsigned int getCols() const
Definition: vpArray2D.h:281
unsigned int getRows() const
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
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
@ fatalError
Fatal error.
Definition: vpException.h:96
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
double get_j() const
Definition: vpImagePoint.h:132
double get_u() const
Definition: vpImagePoint.h:143
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
double get_i() const
Definition: vpImagePoint.h:121
double get_v() const
Definition: vpImagePoint.h:154
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:117
static int round(double x)
Definition: vpMath.h:318
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:98
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void init()
Definition: vpMeSite.cpp:66
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void initTracking(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Error that can be emited by the vpTracker class and its derivates.
#define vpDERROR_TRACE
Definition: vpDebug.h:464