ViSP  2.10.0
vpFeatureEllipse.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureEllipse.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * 2D ellipse visual feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeatureEllipse.h>
51 
52 // Exception
53 #include <visp/vpException.h>
54 #include <visp/vpMatrixException.h>
55 #include <visp/vpFeatureException.h>
56 
57 // Debug trace
58 #include <visp/vpDebug.h>
59 
60 // math
61 #include <visp/vpMath.h>
62 
63 
64 
65 #include <visp/vpFeatureDisplay.h>
66 
67 
68 /*
69 
70 
71 
72 attributes and members directly related to the vpBasicFeature needs
73 other functionalities ar useful but not mandatory
74 
75 
76 
77 
78 
79 */
80 
81 void
83 {
84  //feature dimension
85  dim_s = 5 ;
86  nbParameters = 8;
87 
88  // memory allocation
89  s.resize(dim_s) ;
90  if (flags == NULL)
91  flags = new bool[nbParameters];
92  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
93 
94  //default depth values
95  A = B = 0;
96  C =1 ;
97 
98 }
99 
101 {
102  init() ;
103 }
104 
105 
106 
108 vpMatrix
109 vpFeatureEllipse::interaction(const unsigned int select)
110 {
111  vpMatrix L ;
112 
113  L.resize(0,6) ;
114 
116  {
117  for (unsigned int i = 0; i < nbParameters; i++)
118  {
119  if (flags[i] == false)
120  {
121  switch(i){
122  case 0:
123  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
124  break;
125  case 1:
126  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
127  break;
128  case 2:
129  vpTRACE("Warning !!! The interaction matrix is computed but mu20 was not set yet");
130  break;
131  case 3:
132  vpTRACE("Warning !!! The interaction matrix is computed but mu11 was not set yet");
133  break;
134  case 4:
135  vpTRACE("Warning !!! The interaction matrix is computed but mu02 was not set yet");
136  break;
137  case 5:
138  vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
139  break;
140  case 6:
141  vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
142  break;
143  case 7:
144  vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
145  break;
146  default:
147  vpTRACE("Problem during the reading of the variable flags");
148  }
149  }
150  }
151  resetFlags();
152  }
153 
154  double xc = s[0] ;
155  double yc = s[1] ;
156  double mu20 = s[2] ;
157  double mu11 = s[3] ;
158  double mu02 = s[4] ;
159 
160  //eq 39
161  double Z = 1/(A*xc + B*yc + C) ;
162 
163 
164 
165  if (vpFeatureEllipse::selectX() & select )
166  {
167  vpMatrix H(1,6) ; H = 0;
168 
169 
170  H[0][0] = -1/Z;
171  H[0][1] = 0 ;
172  H[0][2] = xc/Z + A*mu20 + B*mu11;
173  H[0][3] = xc*yc + mu11;
174  H[0][4] = -1-vpMath::sqr(xc)-mu20;
175  H[0][5] = yc;
176 
177 
178  L = vpMatrix::stackMatrices(L,H) ;
179  }
180 
181  if (vpFeatureEllipse::selectY() & select )
182  {
183  vpMatrix H(1,6) ; H = 0;
184 
185 
186  H[0][0] = 0 ;
187  H[0][1] = -1/Z;
188  H[0][2] = yc/Z + A*mu11 + B*mu02;
189  H[0][3] = 1+vpMath::sqr(yc)+mu02;
190  H[0][4] = -xc*yc - mu11;
191  H[0][5] = -xc;
192 
193  L = vpMatrix::stackMatrices(L,H) ;
194  }
195 
196  if (vpFeatureEllipse::selectMu20() & select )
197  {
198  vpMatrix H(1,6) ; H = 0;
199 
200  H[0][0] = -2*(A*mu20+B*mu11);
201  H[0][1] = 0 ;
202  H[0][2] = 2*((1/Z+A*xc)*mu20+B*xc*mu11) ;
203  H[0][3] = 2*(yc*mu20+xc*mu11);
204  H[0][4] = -4*mu20*xc;
205  H[0][5] = 2*mu11;
206 
207  L = vpMatrix::stackMatrices(L,H) ;
208  }
209 
210  if (vpFeatureEllipse::selectMu11() & select )
211  {
212  vpMatrix H(1,6) ; H = 0;
213 
214  H[0][0] = -A*mu11-B*mu02;
215  H[0][1] = -A*mu20-B*mu11;
216  H[0][2] = A*yc*mu20+(3/Z-C)*mu11+B*xc*mu02;
217  H[0][3] = 3*yc*mu11+xc*mu02;
218  H[0][4] = -yc*mu20-3*xc*mu11;
219  H[0][5] = mu02-mu20;
220 
221  L = vpMatrix::stackMatrices(L,H) ;
222  }
223 
224  if (vpFeatureEllipse::selectMu02() & select )
225  {
226  vpMatrix H(1,6) ; H = 0;
227 
228  H[0][0] = 0 ;
229  H[0][1] = -2*(A*mu11+B*mu02);
230  H[0][2] = 2*((1/Z+B*yc)*mu02+A*yc*mu11);
231  H[0][3] = 4*yc*mu02;
232  H[0][4] = -2*(yc*mu11 +xc*mu02) ;
233  H[0][5] = -2*mu11 ;
234  L = vpMatrix::stackMatrices(L,H) ;
235  }
236 
237 
238  return L ;
239 }
240 
245  const unsigned int select)
246 {
247  vpColVector e(0) ;
248 
249  try{
250  if (vpFeatureEllipse::selectX() & select )
251  {
252  vpColVector ex(1) ;
253  ex[0] = s[0] - s_star[0] ;
254 
255  e = vpMatrix::stackMatrices(e,ex) ;
256  }
257 
258  if (vpFeatureEllipse::selectY() & select )
259  {
260  vpColVector ey(1) ;
261  ey[0] = s[1] - s_star[1] ;
262  e = vpMatrix::stackMatrices(e,ey) ;
263  }
264 
265  if (vpFeatureEllipse::selectMu20() & select )
266  {
267  vpColVector ex(1) ;
268  ex[0] = s[2] - s_star[2] ;
269 
270  e = vpMatrix::stackMatrices(e,ex) ;
271  }
272 
273  if (vpFeatureEllipse::selectMu11() & select )
274  {
275  vpColVector ey(1) ;
276  ey[0] = s[3] - s_star[3] ;
277  e = vpMatrix::stackMatrices(e,ey) ;
278  }
279 
280  if (vpFeatureEllipse::selectMu02() & select )
281  {
282  vpColVector ey(1) ;
283  ey[0] = s[4] - s_star[4] ;
284  e = vpMatrix::stackMatrices(e,ey) ;
285  }
286 
287  }
288  catch(vpMatrixException me)
289  {
290  vpERROR_TRACE("caught a Matrix related error") ;
291  std::cout <<std::endl << me << std::endl ;
292  throw(me) ;
293  }
294  catch(vpException me)
295  {
296  vpERROR_TRACE("caught another error") ;
297  std::cout <<std::endl << me << std::endl ;
298  throw(me) ;
299  }
300 
301 
302  return e ;
303 
304 }
305 
306 
307 void
308 vpFeatureEllipse::print(const unsigned int select ) const
309 {
310 
311  std::cout <<"Ellipse: " << std::endl ;
312  if (vpFeatureEllipse::selectX() & select )
313  std::cout << " x=" << s[0] <<std::endl ;;
314  if (vpFeatureEllipse::selectY() & select )
315  std::cout << " y=" << s[1] <<std::endl ;
316  if (vpFeatureEllipse::selectMu20() & select )
317  std::cout << " mu20=" << s[2] <<std::endl ;
318  if (vpFeatureEllipse::selectMu11() & select )
319  std::cout << " mu11=" << s[3] <<std::endl ;
320  if (vpFeatureEllipse::selectMu02() & select )
321  std::cout << " mu02=" << s[4] <<std::endl ;
322  std::cout << "A = "<<A <<" B = "<<B <<" C = "<<C << std::endl ;
323 }
324 
325 
326 void
327 vpFeatureEllipse::buildFrom(const double x, const double y,
328  const double mu20, const double mu11,
329  const double mu02)
330 {
331 
332  s[0] = x ;
333  s[1] = y ;
334  s[2] = mu20 ;
335  s[3] = mu11 ;
336  s[4] = mu02 ;
337 
338  for( int i = 0; i < 5; i++) flags[i] = true;
339 
340 }
341 
342 void
343 vpFeatureEllipse::buildFrom(const double x, const double y,
344  const double mu20, const double mu11,
345  const double mu02,
346  const double a, const double b, const double c)
347 {
348 
349  s[0] = x ;
350  s[1] = y ;
351  s[2] = mu20 ;
352  s[3] = mu11 ;
353  s[4] = mu02 ;
354 
355  this->A = a ;
356  this->B = b ;
357  this->C = c ;
358 
359  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
360 }
361 
362 void
363 vpFeatureEllipse::set_x(const double x)
364 {
365  s[0] = x ;
366  flags[0] = true;
367 }
368 
369 void
370 vpFeatureEllipse::set_y(const double y)
371 {
372  s[1] = y ;
373  flags[1] = true;
374 }
375 
376 void
377 vpFeatureEllipse::set_xy(const double x,const double y)
378 {
379  s[0] = x ;
380  s[1] = y ;
381  for( int i = 0; i < 2; i++) flags[i] = true;
382 }
383 
384 void
385 vpFeatureEllipse::setABC(const double a, const double b, const double c)
386 {
387  this->A = a ;
388  this->B = b ;
389  this->C = c ;
390  for( unsigned int i = 5; i < nbParameters; i++) flags[i] = true;
391 }
392 
393 
394 void
395 vpFeatureEllipse::setMu(const double mu20, const double mu11,
396  const double mu02)
397 {
398 
399  s[2] = mu20 ;
400  s[3] = mu11 ;
401  s[4] = mu02 ;
402  for( int i = 2; i < 5; i++) flags[i] = true;
403 
404 }
405 
406 
407 
417 void
419  const vpImage<unsigned char> &I,
420  const vpColor &color,
421  unsigned int thickness ) const
422 {
423  try{
424  double x = s[0] ;
425  double y = s[1] ;
426 
427  double mu20 = s[2] ;
428  double mu11 = s[3] ;
429  double mu02 = s[4] ;
430 
431  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
432  cam, I, color, thickness) ;
433 
434  }
435  catch(...)
436  {
437  vpERROR_TRACE("Error caught") ;
438  throw ;
439  }
440 }
441 
451 void
453  const vpImage<vpRGBa> &I,
454  const vpColor &color,
455  unsigned int thickness ) const
456 {
457  try{
458  double x = s[0] ;
459  double y = s[1] ;
460 
461  double mu20 = s[2] ;
462  double mu11 = s[3] ;
463  double mu02 = s[4] ;
464 
465  vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02,
466  cam, I, color, thickness) ;
467 
468  }
469  catch(...)
470  {
471  vpERROR_TRACE("Error caught") ;
472  throw ;
473  }
474 }
475 
476 
479 {
480  vpFeatureEllipse *feature = new vpFeatureEllipse ;
481  return feature ;
482 }
483 
484 
485 /*
486  * Local variables:
487  * c-basic-offset: 2
488  * End:
489  */
void init()
Default initialization.
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
void set_x(const double x)
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)
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void setMu(const double mu20, const double mu11, const double mu02)
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:76
static unsigned int selectY()
static unsigned int selectMu02()
vpFeatureEllipse()
Default constructor.
class that defines what is a visual feature
void set_xy(const double x, const double y)
static unsigned int selectX()
vpFeatureEllipse * duplicate() const
feature duplication
static unsigned int selectMu11()
static double sqr(double x)
Definition: vpMath.h:106
void print(const unsigned int select=FEATURE_ALL) const
print the name of the feature
Generic class defining intrinsic camera parameters.
void setABC(const double A, const double B, const double C)
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3272
vpBasicFeatureDeallocatorType deallocate
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_y(const double y)
static unsigned int selectMu20()
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
error that can be emited by the vpMatrix class and its derivates
void buildFrom(const double x, const double y, const double mu20, const double mu11, const double mu02)
Class that defines 2D ellipse visual feature.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98