Visual Servoing Platform  version 3.3.1 under development (2020-06-03)
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 
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, 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(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(double x, double y, double mu20, double mu11, double mu02)
289 {
290 
291  s[0] = x;
292  s[1] = y;
293  s[2] = mu20;
294  s[3] = mu11;
295  s[4] = mu02;
296 
297  for (int i = 0; i < 5; i++)
298  flags[i] = true;
299 }
300 
301 void vpFeatureEllipse::buildFrom(double x, double y, double mu20, double mu11,
302  double mu02, double a, double b, double c)
303 {
304 
305  s[0] = x;
306  s[1] = y;
307  s[2] = mu20;
308  s[3] = mu11;
309  s[4] = mu02;
310 
311  this->A = a;
312  this->B = b;
313  this->C = c;
314 
315  for (unsigned int i = 0; i < nbParameters; i++)
316  flags[i] = true;
317 }
318 
320 {
321  s[0] = x;
322  flags[0] = true;
323 }
324 
326 {
327  s[1] = y;
328  flags[1] = true;
329 }
330 
331 void vpFeatureEllipse::set_xy(double x, double y)
332 {
333  s[0] = x;
334  s[1] = y;
335  for (int i = 0; i < 2; i++)
336  flags[i] = true;
337 }
338 
339 void vpFeatureEllipse::setABC(double a, double b, double c)
340 {
341  this->A = a;
342  this->B = b;
343  this->C = c;
344  for (unsigned int i = 5; i < nbParameters; i++)
345  flags[i] = true;
346 }
347 
348 void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02)
349 {
350 
351  s[2] = mu20;
352  s[3] = mu11;
353  s[4] = mu02;
354  for (int i = 2; i < 5; i++)
355  flags[i] = true;
356 }
357 
368  unsigned int thickness) const
369 {
370  try {
371  double x = s[0];
372  double y = s[1];
373 
374  double mu20 = s[2];
375  double mu11 = s[3];
376  double mu02 = s[4];
377 
378  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness);
379 
380  } catch (...) {
381  vpERROR_TRACE("Error caught");
382  throw;
383  }
384 }
385 
395 void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
396  unsigned int thickness) const
397 {
398  try {
399  double x = s[0];
400  double y = s[1];
401 
402  double mu20 = s[2];
403  double mu11 = s[3];
404  double mu02 = s[4];
405 
406  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness);
407 
408  } catch (...) {
409  vpERROR_TRACE("Error caught");
410  throw;
411  }
412 }
413 
416 {
417  vpFeatureEllipse *feature = new vpFeatureEllipse;
418  return feature;
419 }
420 
421 unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
422 unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
423 unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
424 unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
425 unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
void buildFrom(double x, double y, double mu20, double mu11, double mu02)
void init()
Default initialization.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
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
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4620
vpMatrix interaction(unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
unsigned int dim_s
Dimension of the visual feature.
static unsigned int selectY()
static unsigned int selectMu11()
vpFeatureEllipse()
Default constructor.
void set_y(double y)
class that defines what is a visual feature
#define vpTRACE
Definition: vpDebug.h:416
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
static double sqr(double x)
Definition: vpMath.h:116
void set_xy(double x, double y)
static unsigned int selectMu20()
void setMu(double mu20, double mu11, double mu02)
Generic class defining intrinsic camera parameters.
void setABC(double A, double B, double C)
void print(unsigned int select=FEATURE_ALL) const
print the name of the feature
static const unsigned int FEATURE_LINE[32]
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_x(double x)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpFeatureEllipse * duplicate() const
Feature duplication.
static unsigned int selectX()
Class that defines 2D ellipse visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
static unsigned int selectMu02()