Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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,
294  double n02, double a, double b, double c)
295 {
296  s[0] = x;
297  s[1] = y;
298  s[2] = n20;
299  s[3] = n11;
300  s[4] = n02;
301 
302  this->A = a;
303  this->B = b;
304  this->C = c;
305 
306  for (unsigned int i = 0; i < nbParameters; i++)
307  flags[i] = true;
308 }
309 
311 {
312  s[0] = x;
313  flags[0] = true;
314 }
315 
317 {
318  s[1] = y;
319  flags[1] = true;
320 }
321 
322 void vpFeatureEllipse::set_xy(double x, double y)
323 {
324  s[0] = x;
325  s[1] = y;
326  for (int i = 0; i < 2; i++)
327  flags[i] = true;
328 }
329 
330 void vpFeatureEllipse::setABC(double a, double b, double c)
331 {
332  this->A = a;
333  this->B = b;
334  this->C = c;
335  for (unsigned int i = 5; i < nbParameters; i++)
336  flags[i] = true;
337 }
338 
345 void vpFeatureEllipse::setMoments(double n20, double n11, double n02)
346 {
347  s[2] = n20;
348  s[3] = n11;
349  s[4] = n02;
350  for (int i = 2; i < 5; i++)
351  flags[i] = true;
352 }
353 
354 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
355 
361 void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02)
362 {
363  setMoments(mu20, mu11, mu02);
364 }
365 #endif
366 
377  unsigned int thickness) const
378 {
379  double x = s[0];
380  double y = s[1];
381 
382  double n20 = s[2];
383  double n11 = s[3];
384  double n02 = s[4];
385 
386  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
387 }
388 
398 void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
399  unsigned int thickness) const
400 {
401  double x = s[0];
402  double y = s[1];
403 
404  double n20 = s[2];
405  double n11 = s[3];
406  double n02 = s[4];
407 
408  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
409 }
410 
413 {
414  vpFeatureEllipse *feature = new vpFeatureEllipse;
415  return feature;
416 }
417 
421 unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
425 unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
426 
431 unsigned int vpFeatureEllipse::select_n20() { return FEATURE_LINE[2]; }
436 unsigned int vpFeatureEllipse::select_n11() { return FEATURE_LINE[3]; }
441 unsigned int vpFeatureEllipse::select_n02() { return FEATURE_LINE[4]; }
442 
443 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
444 
450 vp_deprecated unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
457 vp_deprecated unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
464 vp_deprecated unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
465 #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:304
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:5879
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:116
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.