Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
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  // eq 39
144  double Z = 1 / (A * xc + B * yc + C);
145 
146  if (vpFeatureEllipse::selectX() & select) {
147  vpMatrix H(1, 6);
148  H = 0;
149 
150  H[0][0] = -1 / Z;
151  H[0][1] = 0;
152  H[0][2] = xc / Z + A * n20 + B * n11;
153  H[0][3] = xc * yc + n11;
154  H[0][4] = -1 - vpMath::sqr(xc) - n20;
155  H[0][5] = yc;
156 
157  L = vpMatrix::stack(L, H);
158  }
159 
160  if (vpFeatureEllipse::selectY() & select) {
161  vpMatrix H(1, 6);
162  H = 0;
163 
164  H[0][0] = 0;
165  H[0][1] = -1 / Z;
166  H[0][2] = yc / Z + A * n11 + B * n02;
167  H[0][3] = 1 + vpMath::sqr(yc) + n02;
168  H[0][4] = -xc * yc - n11;
169  H[0][5] = -xc;
170 
171  L = vpMatrix::stack(L, H);
172  }
173 
174  if (vpFeatureEllipse::select_n20() & select) {
175  vpMatrix H(1, 6);
176  H = 0;
177 
178  H[0][0] = -2 * (A * n20 + B * n11);
179  H[0][1] = 0;
180  H[0][2] = 2 * ((1 / Z + A * xc) * n20 + B * xc * n11);
181  H[0][3] = 2 * (yc * n20 + xc * n11);
182  H[0][4] = -4 * n20 * xc;
183  H[0][5] = 2 * n11;
184 
185  L = vpMatrix::stack(L, H);
186  }
187 
188  if (vpFeatureEllipse::select_n11() & select) {
189  vpMatrix H(1, 6);
190  H = 0;
191 
192  H[0][0] = -A * n11 - B * n02;
193  H[0][1] = -A * n20 - B * n11;
194  H[0][2] = A * yc * n20 + (3 / Z - C) * n11 + B * xc * n02;
195  H[0][3] = 3 * yc * n11 + xc * n02;
196  H[0][4] = -yc * n20 - 3 * xc * n11;
197  H[0][5] = n02 - n20;
198 
199  L = vpMatrix::stack(L, H);
200  }
201 
202  if (vpFeatureEllipse::select_n02() & select) {
203  vpMatrix H(1, 6);
204  H = 0;
205 
206  H[0][0] = 0;
207  H[0][1] = -2 * (A * n11 + B * n02);
208  H[0][2] = 2 * ((1 / Z + B * yc) * n02 + A * yc * n11);
209  H[0][3] = 4 * yc * n02;
210  H[0][4] = -2 * (yc * n11 + xc * n02);
211  H[0][5] = -2 * n11;
212  L = vpMatrix::stack(L, H);
213  }
214 
215  return L;
216 }
217 
220 vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, unsigned int select)
221 {
222  vpColVector e(0);
223 
224  try {
225  if (vpFeatureEllipse::selectX() & select) {
226  vpColVector ex(1);
227  ex[0] = s[0] - s_star[0];
228 
229  e = vpColVector::stack(e, ex);
230  }
231 
232  if (vpFeatureEllipse::selectY() & select) {
233  vpColVector ey(1);
234  ey[0] = s[1] - s_star[1];
235  e = vpColVector::stack(e, ey);
236  }
237 
238  if (vpFeatureEllipse::select_n20() & select) {
239  vpColVector ex(1);
240  ex[0] = s[2] - s_star[2];
241 
242  e = vpColVector::stack(e, ex);
243  }
244 
245  if (vpFeatureEllipse::select_n11() & select) {
246  vpColVector ey(1);
247  ey[0] = s[3] - s_star[3];
248  e = vpColVector::stack(e, ey);
249  }
250 
251  if (vpFeatureEllipse::select_n02() & select) {
252  vpColVector ey(1);
253  ey[0] = s[4] - s_star[4];
254  e = vpColVector::stack(e, ey);
255  }
256 
257  } catch (...) {
258  throw;
259  }
260 
261  return e;
262 }
263 
264 void vpFeatureEllipse::print(unsigned int select) const
265 {
266 
267  std::cout << "Ellipse: " << std::endl;
268  if (vpFeatureEllipse::selectX() & select)
269  std::cout << " x=" << s[0] << std::endl;
270  ;
271  if (vpFeatureEllipse::selectY() & select)
272  std::cout << " y=" << s[1] << std::endl;
273  if (vpFeatureEllipse::select_n20() & select)
274  std::cout << " n20=" << s[2] << std::endl;
275  if (vpFeatureEllipse::select_n11() & select)
276  std::cout << " n11=" << s[3] << std::endl;
277  if (vpFeatureEllipse::select_n02() & select)
278  std::cout << " n02=" << s[4] << std::endl;
279  std::cout << "A = " << A << " B = " << B << " C = " << C << std::endl;
280 }
281 
282 void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11, double n02)
283 {
284  s[0] = x;
285  s[1] = y;
286  s[2] = n20;
287  s[3] = n11;
288  s[4] = n02;
289 
290  for (int i = 0; i < 5; i++)
291  flags[i] = true;
292 }
293 
294 void vpFeatureEllipse::buildFrom(double x, double y, double n20, double n11,
295  double n02, double a, double b, double c)
296 {
297  s[0] = x;
298  s[1] = y;
299  s[2] = n20;
300  s[3] = n11;
301  s[4] = n02;
302 
303  this->A = a;
304  this->B = b;
305  this->C = c;
306 
307  for (unsigned int i = 0; i < nbParameters; i++)
308  flags[i] = true;
309 }
310 
312 {
313  s[0] = x;
314  flags[0] = true;
315 }
316 
318 {
319  s[1] = y;
320  flags[1] = true;
321 }
322 
323 void vpFeatureEllipse::set_xy(double x, double y)
324 {
325  s[0] = x;
326  s[1] = y;
327  for (int i = 0; i < 2; i++)
328  flags[i] = true;
329 }
330 
331 void vpFeatureEllipse::setABC(double a, double b, double c)
332 {
333  this->A = a;
334  this->B = b;
335  this->C = c;
336  for (unsigned int i = 5; i < nbParameters; i++)
337  flags[i] = true;
338 }
339 
346 void vpFeatureEllipse::setMoments(double n20, double n11, double n02)
347 {
348  s[2] = n20;
349  s[3] = n11;
350  s[4] = n02;
351  for (int i = 2; i < 5; i++)
352  flags[i] = true;
353 }
354 
355 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
356 
362 void vpFeatureEllipse::setMu(double mu20, double mu11, double mu02)
363 {
364  setMoments(mu20, mu11, mu02);
365 }
366 #endif
367 
378  unsigned int thickness) const
379 {
380  double x = s[0];
381  double y = s[1];
382 
383  double n20 = s[2];
384  double n11 = s[3];
385  double n02 = s[4];
386 
387  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
388 }
389 
399 void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
400  unsigned int thickness) const
401 {
402  double x = s[0];
403  double y = s[1];
404 
405  double n20 = s[2];
406  double n11 = s[3];
407  double n02 = s[4];
408 
409  vpFeatureDisplay::displayEllipse(x, y, n20, n11, n02, cam, I, color, thickness);
410 }
411 
414 {
415  vpFeatureEllipse *feature = new vpFeatureEllipse;
416  return feature;
417 }
418 
422 unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
426 unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
427 
432 unsigned int vpFeatureEllipse::select_n20() { return FEATURE_LINE[2]; }
437 unsigned int vpFeatureEllipse::select_n11() { return FEATURE_LINE[3]; }
442 unsigned int vpFeatureEllipse::select_n02() { return FEATURE_LINE[4]; }
443 
444 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
445 
451 vp_deprecated unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
458 vp_deprecated unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
465 vp_deprecated unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
466 #endif
void init()
Default initialization.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
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:305
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: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()
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.