Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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()
57  : iPc(), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
58 {
59 }
60 
64 vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse)
65  : vpMeTracker(meellipse), iPc(meellipse.iPc), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.),
66  thresholdWeight(0.), expecteddensity(0.)
67 {
68  a = meellipse.a;
69  b = meellipse.b;
70  e = meellipse.e;
71 
72  ce = meellipse.ce;
73  se = meellipse.se;
74 
75  mu11 = meellipse.mu11;
76  mu20 = meellipse.mu20;
77  mu02 = meellipse.mu02;
78 
79  expecteddensity = meellipse.expecteddensity;
80 }
81 
85 vpMbtMeEllipse::~vpMbtMeEllipse() { list.clear(); }
86 
102 void vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad,
103  unsigned int &_nbFeatures,
104  const vpMatrix &SobelX, const vpMatrix &SobelY,
105  bool display, unsigned int length,
106  unsigned int thickness)
107 {
108  _sumErrorRad = 0;
109  _nbFeatures = 0;
110 
111  double offset = std::floor(SobelX.getRows() / 2.0f);
112  // std::cout << "offset=" << offset << std::endl;
113  int height = (int)_I.getHeight();
114  int width = (int)_I.getWidth();
115 
116  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
117  double iSite = it->ifloat;
118  double jSite = it->jfloat;
119 
120  if (!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height,
121  width)) { // Check if necessary
122  // The tangent angle to the ellipse at a site
123  double theta = atan((-mu02 * jSite + mu02 * iPc.get_j() + mu11 * iSite - mu11 * iPc.get_i()) /
124  (mu20 * iSite - mu11 * jSite + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
125  M_PI / 2;
126 
127  double deltaNormalized = theta;
128  while (deltaNormalized < 0)
129  deltaNormalized += M_PI;
130  while (deltaNormalized > M_PI)
131  deltaNormalized -= M_PI;
132 
133  vpColVector vecSite(2);
134  vecSite[0] = cos(deltaNormalized);
135  vecSite[1] = sin(deltaNormalized);
136  vecSite.normalize();
137 
138  double gradientX = 0;
139  double gradientY = 0;
140 
141  for (unsigned int i = 0; i < SobelX.getRows(); i++) {
142  double iImg = iSite + (i - offset);
143  for (unsigned int j = 0; j < SobelX.getCols(); j++) {
144  double jImg = jSite + (j - offset);
145 
146  if (iImg < 0)
147  iImg = 0.0;
148  if (jImg < 0)
149  jImg = 0.0;
150 
151  if (iImg > _I.getHeight() - 1)
152  iImg = _I.getHeight() - 1;
153  if (jImg > _I.getWidth() - 1)
154  jImg = _I.getWidth() - 1;
155 
156  gradientX += SobelX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
157  }
158  }
159 
160  for (unsigned int i = 0; i < SobelY.getRows(); i++) {
161  double iImg = iSite + (i - offset);
162  for (unsigned int j = 0; j < SobelY.getCols(); j++) {
163  double jImg = jSite + (j - offset);
164 
165  if (iImg < 0)
166  iImg = 0.0;
167  if (jImg < 0)
168  jImg = 0.0;
169 
170  if (iImg > _I.getHeight() - 1)
171  iImg = _I.getHeight() - 1;
172  if (jImg > _I.getWidth() - 1)
173  jImg = _I.getWidth() - 1;
174 
175  gradientY += SobelY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
176  }
177  }
178 
179  double angle = atan2(gradientY, gradientX);
180  while (angle < 0)
181  angle += M_PI;
182  while (angle > M_PI)
183  angle -= M_PI;
184 
185  // if(std::fabs(deltaNormalized-angle) > M_PI / 2)
186  // {
187  // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI
188  // / 2;
189  // } else {
190  // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
191  // }
192 
193  // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
194  // double angle2 = sqrt(vpMath::sqr(deltaNormalized-
195  // (angle-M_PI)));
196 
197  vpColVector vecGrad(2);
198  vecGrad[0] = cos(angle);
199  vecGrad[1] = sin(angle);
200  vecGrad.normalize();
201 
202  double angle1 = acos(vecSite * vecGrad);
203  double angle2 = acos(vecSite * (-vecGrad));
204 
205  if (display) {
206  vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(deltaNormalized)),
207  (int)(it->get_j() + length*sin(deltaNormalized)), vpColor::blue,
208  length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
209  if (angle1 < angle2) {
210  vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(angle)),
211  (int)(it->get_j() + length*sin(angle)), vpColor::red,
212  length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
213  } else {
214  vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(angle+M_PI)),
215  (int)(it->get_j() + length*sin(angle+M_PI)), vpColor::red,
216  length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
217  }
218  }
219 
220  _sumErrorRad += (std::min)(angle1, angle2);
221 
222  _nbFeatures++;
223  }
224  }
225 }
226 
238 void vpMbtMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
239 {
240  if (!me) {
241  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
242  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
243  }
244 
245  int height = (int)I.getHeight();
246  int width = (int)I.getWidth();
247 
248  // if (me->getSampleStep()==0)
249  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
250  std::cout << "In vpMbtMeEllipse::sample: ";
251  std::cout << "function called with sample step = 0";
252  // return fatalError;
253  }
254 
255  // Approximation of the circumference of an ellipse:
256  // [Ramanujan, S., "Modular Equations and Approximations to ,"
257  // Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
258  double t = (a - b) / (a + b);
259  double circumference = M_PI * (a + b) * (1 + 3 * vpMath::sqr(t) / (10 + sqrt(4 - 3 * vpMath::sqr(t))));
260  int nb_points_to_track = (int)(circumference / me->getSampleStep());
261  double incr = 2 * M_PI / nb_points_to_track;
262 
263  expecteddensity = 0; // nb_points_to_track;
264 
265  // Delete old list
266  list.clear();
267 
268  // sample positions
269  double k = 0;
270  for (int pt = 0; pt < nb_points_to_track; pt++) {
271  double j = a * cos(k); // equation of an ellipse
272  double i = b * sin(k); // equation of an ellipse
273 
274  double iP_j = iPc.get_j() + ce * j - se * i;
275  double iP_i = iPc.get_i() + se * j + ce * i;
276 
277  // vpColor col = vpColor::red;
278  // vpDisplay::displayCross(I, vpImagePoint(iP_i, iP_j), 5, col) ; //debug
279  // only
280 
281  // If point is in the image, add to the sample list
282  if (!outOfImage(vpMath::round(iP_i), vpMath::round(iP_j), 0, height, width)) {
283  // The tangent angle to the ellipse at a site
284  double theta = atan((-mu02 * iP_j + mu02 * iPc.get_j() + mu11 * iP_i - mu11 * iPc.get_i()) /
285  (mu20 * iP_i - mu11 * iP_j + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
286  M_PI / 2;
287 
288  vpMeSite pix;
289  pix.init((int)iP_i, (int)iP_j, theta);
290  pix.setDisplay(selectDisplay);
292 
293  list.push_back(pix);
294  expecteddensity++;
295  }
296  k += incr;
297  }
298 
299  if (!doNotTrack)
301 }
302 
316 void vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
317 {
318  if (!me) {
319  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
320  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
321  }
322 
323  unsigned int n = numberOfSignal();
324  if ((double)n < 0.9 * expecteddensity) {
325  sample(I);
327  }
328 }
329 
335 void vpMbtMeEllipse::updateTheta()
336 {
337  vpMeSite p_me;
338  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
339  p_me = *it;
340  vpImagePoint iP;
341  iP.set_i(p_me.ifloat);
342  iP.set_j(p_me.jfloat);
343 
344  // The tangent angle to the ellipse at a site
345  double theta = atan((-mu02 * p_me.jfloat + mu02 * iPc.get_j() + mu11 * p_me.ifloat - mu11 * iPc.get_i()) /
346  (mu20 * p_me.ifloat - mu11 * p_me.jfloat + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
347  M_PI / 2;
348 
349  p_me.alpha = theta;
350  *it = p_me;
351  }
352 }
353 
358 void vpMbtMeEllipse::suppressPoints()
359 {
360  // Loop through list of sites to track
361  for (std::list<vpMeSite>::iterator itList = list.begin(); itList != list.end();) {
362  vpMeSite s = *itList; // current reference pixel
364  itList = list.erase(itList);
365  else
366  ++itList;
367  }
368 }
369 
379 void vpMbtMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
380 {
381  vpDisplay::displayEllipse(I, iPc, mu20, mu11, mu02, true, col);
382 }
383 
384 void vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p,
385  double mu02_p,
386  bool doNotTrack)
387 {
388  iPc = ic;
389  mu20 = mu20_p;
390  mu11 = mu11_p;
391  mu02 = mu02_p;
392 
393  if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
394 
395  double val_p = sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p));
396  a = sqrt((mu20_p + mu02_p + val_p) / 2);
397  b = sqrt((mu20_p + mu02_p - val_p) / 2);
398 
399  e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
400  } else {
401  a = sqrt(mu20_p);
402  b = sqrt(mu02_p);
403  e = 0.;
404  }
405 
406  e = atan(e);
407 
408  ce = cos(e);
409  se = sin(e);
410 
411  sample(I, doNotTrack);
412 
413  if (!doNotTrack)
415 
416  try {
417  if (!doNotTrack)
418  track(I);
419  } catch (const vpException &exception) {
420  throw(exception);
421  }
422 }
423 
429 void vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
430 {
431  try {
433  if (m_mask != NULL) {
434  // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask.
435  expecteddensity = (double)list.size();
436  }
437  } catch (const vpException &exception) {
438  throw(exception);
439  }
440 }
441 
442 void vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p,
443  double mu11_p, double mu02_p)
444 {
445  iPc = ic;
446  mu20 = mu20_p;
447  mu11 = mu11_p;
448  mu02 = mu02_p;
449 
450  if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
451 
452  double val_p = sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p));
453  a = sqrt((mu20_p + mu02_p + val_p) / 2);
454  b = sqrt((mu20_p + mu02_p - val_p) / 2);
455 
456  e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
457  } else {
458  a = sqrt(mu20_p);
459  b = sqrt(mu02_p);
460  e = 0.;
461  }
462 
463  e = atan(e);
464 
465  ce = cos(e);
466  se = sin(e);
467 
468  suppressPoints();
469  reSample(I);
470 
471  // remet a jour l'angle delta pour chaque point de la liste
472  updateTheta();
473 }
474 
475 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
double alpha
Definition: vpMeSite.h:93
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
static const vpColor red
Definition: vpColor.h:179
unsigned int getCols() const
Definition: vpArray2D.h:279
double ifloat
Definition: vpMeSite.h:89
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:167
double getSampleStep() const
Definition: vpMe.h:285
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)
static double sqr(double x)
Definition: vpMath.h:114
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:65
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
static int round(double x)
Definition: vpMath.h:241
void set_j(double jj)
Definition: vpImagePoint.h:178
#define vpDERROR_TRACE
Definition: vpDebug.h:464
unsigned int getHeight() const
Definition: vpImage.h:186
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void initTracking(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:244
static const vpColor blue
Definition: vpColor.h:185