Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
vpFeatureEllipse.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeatureEllipse.h>
43 
44 // Exception
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
51 // math
52 #include <visp3/core/vpMath.h>
53 
54 #include <visp3/core/vpFeatureDisplay.h>
55 
56 /*
57 
58 attributes and members directly related to the vpBasicFeature needs
59 other functionalities ar useful but not mandatory
60 
61 */
62 
64 {
65  // feature dimension
66  dim_s = 5;
67  nbParameters = 8;
68 
69  // memory allocation
70  s.resize(dim_s);
71  if (flags == nullptr)
72  flags = new bool[nbParameters];
73  for (unsigned int i = 0; i < nbParameters; i++)
74  flags[i] = false;
75 
76  // default depth values
77  A = B = 0;
78  C = 1;
79 }
80 
81 vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
82 vpFeatureEllipse::vpFeatureEllipse(double x, double y, double n20, double n11, double n02) { this->buildFrom(x, y, n20, n11, n02); }
83 
84 
87 {
88  vpMatrix L;
89 
90  L.resize(0, 6);
91 
93  for (unsigned int i = 0; i < nbParameters; i++) {
94  if (flags[i] == false) {
95  switch (i) {
96  case 0:
97  vpTRACE("Warning !!! The interaction matrix is computed but x was "
98  "not set yet");
99  break;
100  case 1:
101  vpTRACE("Warning !!! The interaction matrix is computed but y was "
102  "not set yet");
103  break;
104  case 2:
105  vpTRACE("Warning !!! The interaction matrix is computed but n20 "
106  "was not set yet");
107  break;
108  case 3:
109  vpTRACE("Warning !!! The interaction matrix is computed but n11 "
110  "was not set yet");
111  break;
112  case 4:
113  vpTRACE("Warning !!! The interaction matrix is computed but n02 "
114  "was not set yet");
115  break;
116  case 5:
117  vpTRACE("Warning !!! The interaction matrix is computed but A was "
118  "not set yet");
119  break;
120  case 6:
121  vpTRACE("Warning !!! The interaction matrix is computed but B was "
122  "not set yet");
123  break;
124  case 7:
125  vpTRACE("Warning !!! The interaction matrix is computed but C was "
126  "not set yet");
127  break;
128  default:
129  vpTRACE("Problem during the reading of the variable flags");
130  }
131  }
132  }
133  resetFlags();
134  }
135 
136  double xc = s[0];
137  double yc = s[1];
138  double n20 = s[2];
139  double n11 = s[3];
140  double n02 = s[4];
141 
142  double Zinv = A * xc + B * yc + C;
143 
144  if (vpFeatureEllipse::selectX() & select) {
145  vpMatrix H(1, 6);
146  H = 0;
147  // Eq (14) of Chaumette 2004 TRO paper on moments
148  H[0][0] = -Zinv;
149  H[0][1] = 0;
150  H[0][2] = xc * Zinv + 4.0 * (A * n20 + B * n11);
151  H[0][3] = xc * yc + 4.0 * n11;
152  H[0][4] = -1 - vpMath::sqr(xc) - 4.0 * n20;
153  H[0][5] = yc;
154 
155  L = vpMatrix::stack(L, H);
156  }
157 
158  if (vpFeatureEllipse::selectY() & select) {
159  vpMatrix H(1, 6);
160  H = 0;
161  // Eq (14) of Chaumette 2004 TRO paper on moments
162  H[0][0] = 0;
163  H[0][1] = -Zinv;
164  H[0][2] = yc * Zinv + 4.0 * (A * n11 + B * n02);
165  H[0][3] = 1 + vpMath::sqr(yc) + 4.0 * n02;
166  H[0][4] = -xc * yc - 4.0 * n11;
167  H[0][5] = -xc;
168 
169  L = vpMatrix::stack(L, H);
170  }
171 
172  if (vpFeatureEllipse::select_n20() & select) {
173  vpMatrix H(1, 6);
174  H = 0;
175  // Eq (26) of Chaumette 2004 TRO paper on moments
176  H[0][0] = -2.0 * (A * n20 + B * n11);
177  H[0][1] = 0;
178  H[0][2] = 2 * ((Zinv + A * xc) * n20 + B * xc * n11);
179  H[0][3] = 2 * (yc * n20 + xc * n11);
180  H[0][4] = -4 * n20 * xc;
181  H[0][5] = 2 * n11;
182 
183  L = vpMatrix::stack(L, H);
184  }
185 
186  if (vpFeatureEllipse::select_n11() & select) {
187  vpMatrix H(1, 6);
188  H = 0;
189  // Eq (26) of Chaumette 2004 TRO paper on moments
190  H[0][0] = -A * n11 - B * n02;
191  H[0][1] = -A * n20 - B * n11;
192  H[0][2] = A * yc * n20 + (3 * Zinv - C) * n11 + B * xc * n02;
193  H[0][3] = 3 * yc * n11 + xc * n02;
194  H[0][4] = -yc * n20 - 3 * xc * n11;
195  H[0][5] = n02 - n20;
196 
197  L = vpMatrix::stack(L, H);
198  }
199 
200  if (vpFeatureEllipse::select_n02() & select) {
201  vpMatrix H(1, 6);
202  H = 0;
203  // Eq (26) of Chaumette 2004 TRO paper on moments
204  H[0][0] = 0;
205  H[0][1] = -2 * (A * n11 + B * n02);
206  H[0][2] = 2 * ((Zinv + B * yc) * n02 + A * yc * n11);
207  H[0][3] = 4 * yc * n02;
208  H[0][4] = -2 * (yc * n11 + xc * n02);
209  H[0][5] = -2 * n11;
210  L = vpMatrix::stack(L, H);
211  }
212 
213  return L;
214 }
215 
218 vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, unsigned int select)
219 {
220  vpColVector e(0);
221 
222  try {
223  if (vpFeatureEllipse::selectX() & select) {
224  vpColVector ex(1);
225  ex[0] = s[0] - s_star[0];
226 
227  e = vpColVector::stack(e, ex);
228  }
229 
230  if (vpFeatureEllipse::selectY() & select) {
231  vpColVector ey(1);
232  ey[0] = s[1] - s_star[1];
233  e = vpColVector::stack(e, ey);
234  }
235 
236  if (vpFeatureEllipse::select_n20() & select) {
237  vpColVector ex(1);
238  ex[0] = s[2] - s_star[2];
239 
240  e = vpColVector::stack(e, ex);
241  }
242 
243  if (vpFeatureEllipse::select_n11() & select) {
244  vpColVector ey(1);
245  ey[0] = s[3] - s_star[3];
246  e = vpColVector::stack(e, ey);
247  }
248 
249  if (vpFeatureEllipse::select_n02() & select) {
250  vpColVector ey(1);
251  ey[0] = s[4] - s_star[4];
252  e = vpColVector::stack(e, ey);
253  }
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:163
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
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 init() vp_override
Default initialization.
void set_x(double x)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
compute the interaction matrix from a subset a the possible features
static unsigned int selectX()
void print(unsigned int select=FEATURE_ALL) const vp_override
Print the name of the feature.
vpFeatureEllipse * duplicate() const vp_override
Feature duplication.
void set_y(double y)
vpFeatureEllipse()
Default constructor.
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
static unsigned int selectY()
void setABC(double A, double B, double C)
vp_deprecated void setMu(double mu20, double mu11, double mu02)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
static unsigned int select_n20()
void setMoments(double n20, double n11, double n02)
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:201
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5669
#define vpTRACE
Definition: vpDebug.h:407