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