Visual Servoing Platform  version 3.5.1 under development (2023-02-04)
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 attributes and members directly related to the vpBasicFeature needs
62 other functionalities ar useful but not mandatory
63 
64 */
65 
67 {
68  // feature dimension
69  dim_s = 5;
70  nbParameters = 8;
71 
72  // memory allocation
73  s.resize(dim_s);
74  if (flags == NULL)
75  flags = new bool[nbParameters];
76  for (unsigned int i = 0; i < nbParameters; i++)
77  flags[i] = false;
78 
79  // default depth values
80  A = B = 0;
81  C = 1;
82 }
83 
84 vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
85 
88 {
89  vpMatrix L;
90 
91  L.resize(0, 6);
92 
94  for (unsigned int i = 0; i < nbParameters; i++) {
95  if (flags[i] == false) {
96  switch (i) {
97  case 0:
98  vpTRACE("Warning !!! The interaction matrix is computed but x was "
99  "not set yet");
100  break;
101  case 1:
102  vpTRACE("Warning !!! The interaction matrix is computed but y was "
103  "not set yet");
104  break;
105  case 2:
106  vpTRACE("Warning !!! The interaction matrix is computed but n20 "
107  "was not set yet");
108  break;
109  case 3:
110  vpTRACE("Warning !!! The interaction matrix is computed but n11 "
111  "was not set yet");
112  break;
113  case 4:
114  vpTRACE("Warning !!! The interaction matrix is computed but n02 "
115  "was not set yet");
116  break;
117  case 5:
118  vpTRACE("Warning !!! The interaction matrix is computed but A was "
119  "not set yet");
120  break;
121  case 6:
122  vpTRACE("Warning !!! The interaction matrix is computed but B was "
123  "not set yet");
124  break;
125  case 7:
126  vpTRACE("Warning !!! The interaction matrix is computed but C was "
127  "not set yet");
128  break;
129  default:
130  vpTRACE("Problem during the reading of the variable flags");
131  }
132  }
133  }
134  resetFlags();
135  }
136 
137  double xc = s[0];
138  double yc = s[1];
139  double n20 = s[2];
140  double n11 = s[3];
141  double n02 = s[4];
142 
143  double Zinv = A * xc + B * yc + C;
144 
145  if (vpFeatureEllipse::selectX() & select) {
146  vpMatrix H(1, 6);
147  H = 0;
148  // Eq (14) of Chaumette 2004 TRO paper on moments
149  H[0][0] = -Zinv;
150  H[0][1] = 0;
151  H[0][2] = xc * Zinv + 4.0 * (A * n20 + B * n11);
152  H[0][3] = xc * yc + 4.0 * n11;
153  H[0][4] = -1 - vpMath::sqr(xc) - 4.0 * n20;
154  H[0][5] = yc;
155 
156  L = vpMatrix::stack(L, H);
157  }
158 
159  if (vpFeatureEllipse::selectY() & select) {
160  vpMatrix H(1, 6);
161  H = 0;
162  // Eq (14) of Chaumette 2004 TRO paper on moments
163  H[0][0] = 0;
164  H[0][1] = -Zinv;
165  H[0][2] = yc * Zinv + 4.0 * (A * n11 + B * n02);
166  H[0][3] = 1 + vpMath::sqr(yc) + 4.0 * n02;
167  H[0][4] = -xc * yc - 4.0 * n11;
168  H[0][5] = -xc;
169 
170  L = vpMatrix::stack(L, H);
171  }
172 
173  if (vpFeatureEllipse::select_n20() & select) {
174  vpMatrix H(1, 6);
175  H = 0;
176  // Eq (26) of Chaumette 2004 TRO paper on moments
177  H[0][0] = -2.0 * (A * n20 + B * n11);
178  H[0][1] = 0;
179  H[0][2] = 2 * ((Zinv + A * xc) * n20 + B * xc * n11);
180  H[0][3] = 2 * (yc * n20 + xc * n11);
181  H[0][4] = -4 * n20 * xc;
182  H[0][5] = 2 * n11;
183 
184  L = vpMatrix::stack(L, H);
185  }
186 
187  if (vpFeatureEllipse::select_n11() & select) {
188  vpMatrix H(1, 6);
189  H = 0;
190  // Eq (26) of Chaumette 2004 TRO paper on moments
191  H[0][0] = -A * n11 - B * n02;
192  H[0][1] = -A * n20 - B * n11;
193  H[0][2] = A * yc * n20 + (3 * Zinv - C) * n11 + B * xc * n02;
194  H[0][3] = 3 * yc * n11 + xc * n02;
195  H[0][4] = -yc * n20 - 3 * xc * n11;
196  H[0][5] = n02 - n20;
197 
198  L = vpMatrix::stack(L, H);
199  }
200 
201  if (vpFeatureEllipse::select_n02() & select) {
202  vpMatrix H(1, 6);
203  H = 0;
204  // Eq (26) of Chaumette 2004 TRO paper on moments
205  H[0][0] = 0;
206  H[0][1] = -2 * (A * n11 + B * n02);
207  H[0][2] = 2 * ((Zinv + B * yc) * n02 + A * yc * n11);
208  H[0][3] = 4 * yc * n02;
209  H[0][4] = -2 * (yc * n11 + xc * n02);
210  H[0][5] = -2 * n11;
211  L = vpMatrix::stack(L, H);
212  }
213 
214  return L;
215 }
216 
219 vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, unsigned int select)
220 {
221  vpColVector e(0);
222 
223  try {
224  if (vpFeatureEllipse::selectX() & select) {
225  vpColVector ex(1);
226  ex[0] = s[0] - s_star[0];
227 
228  e = vpColVector::stack(e, ex);
229  }
230 
231  if (vpFeatureEllipse::selectY() & select) {
232  vpColVector ey(1);
233  ey[0] = s[1] - s_star[1];
234  e = vpColVector::stack(e, ey);
235  }
236 
237  if (vpFeatureEllipse::select_n20() & select) {
238  vpColVector ex(1);
239  ex[0] = s[2] - s_star[2];
240 
241  e = vpColVector::stack(e, ex);
242  }
243 
244  if (vpFeatureEllipse::select_n11() & select) {
245  vpColVector ey(1);
246  ey[0] = s[3] - s_star[3];
247  e = vpColVector::stack(e, ey);
248  }
249 
250  if (vpFeatureEllipse::select_n02() & select) {
251  vpColVector ey(1);
252  ey[0] = s[4] - s_star[4];
253  e = vpColVector::stack(e, ey);
254  }
255 
256  } catch (...) {
257  throw;
258  }
259 
260  return e;
261 }
262 
263 void vpFeatureEllipse::print(unsigned int select) const
264 {
265 
266  std::cout << "Ellipse: " << std::endl;
267  if (vpFeatureEllipse::selectX() & select)
268  std::cout << " x=" << s[0] << std::endl;
269  ;
270  if (vpFeatureEllipse::selectY() & select)
271  std::cout << " y=" << s[1] << std::endl;
272  if (vpFeatureEllipse::select_n20() & select)
273  std::cout << " n20=" << s[2] << std::endl;
274  if (vpFeatureEllipse::select_n11() & select)
275  std::cout << " n11=" << s[3] << std::endl;
276  if (vpFeatureEllipse::select_n02() & select)
277  std::cout << " n02=" << s[4] << std::endl;
278  std::cout << "A = " << A << " B = " << B << " C = " << C << std::endl;
279 }
280 
281 void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11, double n02)
282 {
283  s[0] = x;
284  s[1] = y;
285  s[2] = n20;
286  s[3] = n11;
287  s[4] = n02;
288 
289  for (int i = 0; i < 5; i++)
290  flags[i] = true;
291 }
292 
293 void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11, double n02, double a, double b, double c)
294 {
295  s[0] = x;
296  s[1] = y;
297  s[2] = n20;
298  s[3] = n11;
299  s[4] = n02;
300 
301  this->A = a;
302  this->B = b;
303  this->C = c;
304 
305  for (unsigned int i = 0; i < nbParameters; i++)
306  flags[i] = true;
307 }
308 
310 {
311  s[0] = x;
312  flags[0] = true;
313 }
314 
316 {
317  s[1] = y;
318  flags[1] = true;
319 }
320 
321 void vpFeatureEllipse::set_xy(double x, double y)
322 {
323  s[0] = x;
324  s[1] = y;
325  for (int i = 0; i < 2; i++)
326  flags[i] = true;
327 }
328 
329 void vpFeatureEllipse::setABC(double a, double b, double c)
330 {
331  this->A = a;
332  this->B = b;
333  this->C = c;
334  for (unsigned int i = 5; i < nbParameters; i++)
335  flags[i] = true;
336 }
337 
344 void vpFeatureEllipse::setMoments(double n20, double n11, double n02)
345 {
346  s[2] = n20;
347  s[3] = n11;
348  s[4] = n02;
349  for (int i = 2; i < 5; i++)
350  flags[i] = true;
351 }
352 
353 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
360 void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02) { setMoments(mu20, mu11, mu02); }
361 #endif
362 
373  unsigned int thickness) const
374 {
375  double x = s[0];
376  double y = s[1];
377 
378  double n20 = s[2];
379  double n11 = s[3];
380  double n02 = s[4];
381 
382  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
383 }
384 
394 void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
395  unsigned int thickness) const
396 {
397  double x = s[0];
398  double y = s[1];
399 
400  double n20 = s[2];
401  double n11 = s[3];
402  double n02 = s[4];
403 
404  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
405 }
406 
409 {
410  vpFeatureEllipse *feature = new vpFeatureEllipse;
411  return feature;
412 }
413 
417 unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
421 unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
422 
427 unsigned int vpFeatureEllipse::select_n20() { return FEATURE_LINE[2]; }
432 unsigned int vpFeatureEllipse::select_n11() { return FEATURE_LINE[3]; }
437 unsigned int vpFeatureEllipse::select_n02() { return FEATURE_LINE[4]; }
438 
439 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
446 vp_deprecated unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
453 vp_deprecated unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
460 vp_deprecated unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
461 #endif
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static void displayEllipse(double x, double y, double n20, double n11, double n02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Class that defines 2D ellipse visual feature.
void print(unsigned int select=FEATURE_ALL) const
print the name of the feature
void set_x(double x)
vpFeatureEllipse * duplicate() const
Feature duplication.
static unsigned int selectX()
void init()
Default initialization.
void set_y(double y)
vpFeatureEllipse()
Default constructor.
static unsigned int selectY()
void setABC(double A, double B, double C)
vp_deprecated void setMu(double mu20, double mu11, double mu02)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
static unsigned int select_n20()
void setMoments(double n20, double n11, double n02)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
static unsigned int select_n02()
static vp_deprecated unsigned int selectMu20()
static unsigned int select_n11()
void set_xy(double x, double y)
static vp_deprecated unsigned int selectMu11()
static vp_deprecated unsigned int selectMu02()
void buildFrom(double x, double y, double n20, double n11, double n02)
static double sqr(double x)
Definition: vpMath.h:127
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5861
#define vpTRACE
Definition: vpDebug.h:416