Visual Servoing Platform  version 3.5.1 under development (2022-05-28)
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)
354 
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)
440 
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
void init()
Default initialization.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void buildFrom(double x, double y, double n20, double n11, double n02)
static vp_deprecated unsigned int selectMu11()
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
static unsigned int select_n02()
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5871
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()
vpFeatureEllipse()
Default constructor.
void set_y(double y)
static unsigned int select_n11()
class that defines what is a visual feature
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)
static unsigned int select_n20()
#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:123
void set_xy(double x, double y)
vp_deprecated void setMu(double mu20, double mu11, double mu02)
Generic class defining intrinsic camera parameters.
void setMoments(double n20, double n11, double n02)
void setABC(double A, double B, double C)
void print(unsigned int select=FEATURE_ALL) const
print the name of the feature
static vp_deprecated unsigned int selectMu20()
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
static vp_deprecated unsigned int selectMu02()
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.