Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpFeatureEllipse.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  * 2D ellipse visual feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeatureEllipse.h>
46 
47 // Exception
48 #include <visp3/core/vpException.h>
49 #include <visp3/visual_features/vpFeatureException.h>
50 
51 // Debug trace
52 #include <visp3/core/vpDebug.h>
53 
54 // math
55 #include <visp3/core/vpMath.h>
56 
57 #include <visp3/core/vpFeatureDisplay.h>
58 
59 /*
60 
61 
62 
63 attributes and members directly related to the vpBasicFeature needs
64 other functionalities ar useful but not mandatory
65 
66 
67 
68 
69 
70 */
71 
73 {
74  // feature dimension
75  dim_s = 5;
76  nbParameters = 8;
77 
78  // memory allocation
79  s.resize(dim_s);
80  if (flags == NULL)
81  flags = new bool[nbParameters];
82  for (unsigned int i = 0; i < nbParameters; i++)
83  flags[i] = false;
84 
85  // default depth values
86  A = B = 0;
87  C = 1;
88 }
89 
90 vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
91 
93 vpMatrix vpFeatureEllipse::interaction(const unsigned int select)
94 {
95  vpMatrix L;
96 
97  L.resize(0, 6);
98 
100  for (unsigned int i = 0; i < nbParameters; i++) {
101  if (flags[i] == false) {
102  switch (i) {
103  case 0:
104  vpTRACE("Warning !!! The interaction matrix is computed but x was "
105  "not set yet");
106  break;
107  case 1:
108  vpTRACE("Warning !!! The interaction matrix is computed but y was "
109  "not set yet");
110  break;
111  case 2:
112  vpTRACE("Warning !!! The interaction matrix is computed but mu20 "
113  "was not set yet");
114  break;
115  case 3:
116  vpTRACE("Warning !!! The interaction matrix is computed but mu11 "
117  "was not set yet");
118  break;
119  case 4:
120  vpTRACE("Warning !!! The interaction matrix is computed but mu02 "
121  "was not set yet");
122  break;
123  case 5:
124  vpTRACE("Warning !!! The interaction matrix is computed but A was "
125  "not set yet");
126  break;
127  case 6:
128  vpTRACE("Warning !!! The interaction matrix is computed but B was "
129  "not set yet");
130  break;
131  case 7:
132  vpTRACE("Warning !!! The interaction matrix is computed but C was "
133  "not set yet");
134  break;
135  default:
136  vpTRACE("Problem during the reading of the variable flags");
137  }
138  }
139  }
140  resetFlags();
141  }
142 
143  double xc = s[0];
144  double yc = s[1];
145  double mu20 = s[2];
146  double mu11 = s[3];
147  double mu02 = s[4];
148 
149  // eq 39
150  double Z = 1 / (A * xc + B * yc + C);
151 
152  if (vpFeatureEllipse::selectX() & select) {
153  vpMatrix H(1, 6);
154  H = 0;
155 
156  H[0][0] = -1 / Z;
157  H[0][1] = 0;
158  H[0][2] = xc / Z + A * mu20 + B * mu11;
159  H[0][3] = xc * yc + mu11;
160  H[0][4] = -1 - vpMath::sqr(xc) - mu20;
161  H[0][5] = yc;
162 
163  L = vpMatrix::stack(L, H);
164  }
165 
166  if (vpFeatureEllipse::selectY() & select) {
167  vpMatrix H(1, 6);
168  H = 0;
169 
170  H[0][0] = 0;
171  H[0][1] = -1 / Z;
172  H[0][2] = yc / Z + A * mu11 + B * mu02;
173  H[0][3] = 1 + vpMath::sqr(yc) + mu02;
174  H[0][4] = -xc * yc - mu11;
175  H[0][5] = -xc;
176 
177  L = vpMatrix::stack(L, H);
178  }
179 
180  if (vpFeatureEllipse::selectMu20() & select) {
181  vpMatrix H(1, 6);
182  H = 0;
183 
184  H[0][0] = -2 * (A * mu20 + B * mu11);
185  H[0][1] = 0;
186  H[0][2] = 2 * ((1 / Z + A * xc) * mu20 + B * xc * mu11);
187  H[0][3] = 2 * (yc * mu20 + xc * mu11);
188  H[0][4] = -4 * mu20 * xc;
189  H[0][5] = 2 * mu11;
190 
191  L = vpMatrix::stack(L, H);
192  }
193 
194  if (vpFeatureEllipse::selectMu11() & select) {
195  vpMatrix H(1, 6);
196  H = 0;
197 
198  H[0][0] = -A * mu11 - B * mu02;
199  H[0][1] = -A * mu20 - B * mu11;
200  H[0][2] = A * yc * mu20 + (3 / Z - C) * mu11 + B * xc * mu02;
201  H[0][3] = 3 * yc * mu11 + xc * mu02;
202  H[0][4] = -yc * mu20 - 3 * xc * mu11;
203  H[0][5] = mu02 - mu20;
204 
205  L = vpMatrix::stack(L, H);
206  }
207 
208  if (vpFeatureEllipse::selectMu02() & select) {
209  vpMatrix H(1, 6);
210  H = 0;
211 
212  H[0][0] = 0;
213  H[0][1] = -2 * (A * mu11 + B * mu02);
214  H[0][2] = 2 * ((1 / Z + B * yc) * mu02 + A * yc * mu11);
215  H[0][3] = 4 * yc * mu02;
216  H[0][4] = -2 * (yc * mu11 + xc * mu02);
217  H[0][5] = -2 * mu11;
218  L = vpMatrix::stack(L, H);
219  }
220 
221  return L;
222 }
223 
226 vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, const unsigned int select)
227 {
228  vpColVector e(0);
229 
230  try {
231  if (vpFeatureEllipse::selectX() & select) {
232  vpColVector ex(1);
233  ex[0] = s[0] - s_star[0];
234 
235  e = vpColVector::stack(e, ex);
236  }
237 
238  if (vpFeatureEllipse::selectY() & select) {
239  vpColVector ey(1);
240  ey[0] = s[1] - s_star[1];
241  e = vpColVector::stack(e, ey);
242  }
243 
244  if (vpFeatureEllipse::selectMu20() & select) {
245  vpColVector ex(1);
246  ex[0] = s[2] - s_star[2];
247 
248  e = vpColVector::stack(e, ex);
249  }
250 
251  if (vpFeatureEllipse::selectMu11() & select) {
252  vpColVector ey(1);
253  ey[0] = s[3] - s_star[3];
254  e = vpColVector::stack(e, ey);
255  }
256 
257  if (vpFeatureEllipse::selectMu02() & select) {
258  vpColVector ey(1);
259  ey[0] = s[4] - s_star[4];
260  e = vpColVector::stack(e, ey);
261  }
262 
263  } catch (...) {
264  throw;
265  }
266 
267  return e;
268 }
269 
270 void vpFeatureEllipse::print(const unsigned int select) const
271 {
272 
273  std::cout << "Ellipse: " << std::endl;
274  if (vpFeatureEllipse::selectX() & select)
275  std::cout << " x=" << s[0] << std::endl;
276  ;
277  if (vpFeatureEllipse::selectY() & select)
278  std::cout << " y=" << s[1] << std::endl;
279  if (vpFeatureEllipse::selectMu20() & select)
280  std::cout << " mu20=" << s[2] << std::endl;
281  if (vpFeatureEllipse::selectMu11() & select)
282  std::cout << " mu11=" << s[3] << std::endl;
283  if (vpFeatureEllipse::selectMu02() & select)
284  std::cout << " mu02=" << s[4] << std::endl;
285  std::cout << "A = " << A << " B = " << B << " C = " << C << std::endl;
286 }
287 
288 void vpFeatureEllipse::buildFrom(const double x, const double y, const double mu20, const double mu11,
289  const double mu02)
290 {
291 
292  s[0] = x;
293  s[1] = y;
294  s[2] = mu20;
295  s[3] = mu11;
296  s[4] = mu02;
297 
298  for (int i = 0; i < 5; i++)
299  flags[i] = true;
300 }
301 
302 void vpFeatureEllipse::buildFrom(const double x, const double y, const double mu20, const double mu11,
303  const double mu02, const double a, const double b, const double c)
304 {
305 
306  s[0] = x;
307  s[1] = y;
308  s[2] = mu20;
309  s[3] = mu11;
310  s[4] = mu02;
311 
312  this->A = a;
313  this->B = b;
314  this->C = c;
315 
316  for (unsigned int i = 0; i < nbParameters; i++)
317  flags[i] = true;
318 }
319 
320 void vpFeatureEllipse::set_x(const double x)
321 {
322  s[0] = x;
323  flags[0] = true;
324 }
325 
326 void vpFeatureEllipse::set_y(const double y)
327 {
328  s[1] = y;
329  flags[1] = true;
330 }
331 
332 void vpFeatureEllipse::set_xy(const double x, const double y)
333 {
334  s[0] = x;
335  s[1] = y;
336  for (int i = 0; i < 2; i++)
337  flags[i] = true;
338 }
339 
340 void vpFeatureEllipse::setABC(const double a, const double b, const double c)
341 {
342  this->A = a;
343  this->B = b;
344  this->C = c;
345  for (unsigned int i = 5; i < nbParameters; i++)
346  flags[i] = true;
347 }
348 
349 void vpFeatureEllipse::setMu(const double mu20, const double mu11, const double mu02)
350 {
351 
352  s[2] = mu20;
353  s[3] = mu11;
354  s[4] = mu02;
355  for (int i = 2; i < 5; i++)
356  flags[i] = true;
357 }
358 
369  unsigned int thickness) const
370 {
371  try {
372  double x = s[0];
373  double y = s[1];
374 
375  double mu20 = s[2];
376  double mu11 = s[3];
377  double mu02 = s[4];
378 
379  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness);
380 
381  } catch (...) {
382  vpERROR_TRACE("Error caught");
383  throw;
384  }
385 }
386 
396 void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
397  unsigned int thickness) const
398 {
399  try {
400  double x = s[0];
401  double y = s[1];
402 
403  double mu20 = s[2];
404  double mu11 = s[3];
405  double mu02 = s[4];
406 
407  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness);
408 
409  } catch (...) {
410  vpERROR_TRACE("Error caught");
411  throw;
412  }
413 }
414 
417 {
418  vpFeatureEllipse *feature = new vpFeatureEllipse;
419  return feature;
420 }
421 
422 unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
423 unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
424 unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
425 unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
426 unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
void init()
Default initialization.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void set_x(const double x)
static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4462
void setMu(const double mu20, const double mu11, const double mu02)
unsigned int dim_s
Dimension of the visual feature.
static unsigned int selectY()
static unsigned int selectMu11()
vpFeatureEllipse()
Default constructor.
class that defines what is a visual feature
void set_xy(const double x, const double y)
vpFeatureEllipse * duplicate() const
Feature duplication.
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
void print(const unsigned int select=FEATURE_ALL) const
print the name of the feature
static unsigned int selectMu20()
Generic class defining intrinsic camera parameters.
void setABC(const double A, const double B, const double C)
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_y(const double y)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void stack(double d)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
void buildFrom(const double x, const double y, const double mu20, const double mu11, const double mu02)
static unsigned int selectX()
Class that defines 2D ellipse visual feature.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
static unsigned int selectMu02()