ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpFeatureEllipse.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureEllipse.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  */