ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpFeaturePointPolar.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeaturePointPolar.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 point visual feature.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeaturePointPolar.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 #include <visp/vpFeatureDisplay.h>
64 
65 
66 
67 /*
68 
69  attributes and members directly related to the vpBasicFeature needs
70  other functionalities ar useful but not mandatory
71 
72 */
73 
83 void
85 {
86  //feature dimension
87  dim_s = 2 ;
88  nbParameters = 3;
89 
90  // memory allocation
91  s.resize(dim_s) ;
92  if (flags == NULL)
93  flags = new bool[nbParameters];
94  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
95 
96  //default value Z (1 meters)
97  Z = 1;
98 
99 }
100 
112 {
113  init() ;
114 }
115 
116 
122 void
124 {
125  s[0] = rho ;
126  flags[0] = true;
127 }
133 void
135 {
136  s[1] = theta ;
137  flags[1] = true;
138 }
139 
144 void
146 {
147  this->Z = Z ;
148  flags[2] = true;
149 }
150 
161 void
163  const double theta,
164  const double Z)
165 {
166  set_rho(rho) ;
167  set_theta(theta) ;
168  set_Z(Z) ;
169 
170  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
171 }
172 
178 double
180 {
181  return s[0] ;
182 }
183 
189 double
191 {
192  return s[1] ;
193 }
198 double
200 {
201  return this->Z ;
202 }
203 
204 
205 
281 vpMatrix
282 vpFeaturePointPolar::interaction(const unsigned int select)
283 {
284  vpMatrix L ;
285 
286  L.resize(0,6) ;
287 
289  {
290  for (unsigned int i = 0; i < nbParameters; i++)
291  {
292  if (flags[i] == false)
293  {
294  switch(i){
295  case 0:
296  vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
297  break;
298  case 1:
299  vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
300  break;
301  case 2:
302  vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
303  break;
304  default:
305  vpTRACE("Problem during the reading of the variable flags");
306  }
307  }
308  }
309  resetFlags();
310  }
311 
312  double rho = get_rho() ;
313  double theta = get_theta() ;
314  double Z = get_Z() ;
315 
316  double c = cos(theta);
317  double s = sin(theta);
318 
319  double rho2 = rho*rho;
320 
321  if (fabs(rho) < 1e-6) {
322  vpERROR_TRACE("rho polar coordinate of the point is null") ;
323  std::cout <<"rho = " << rho << std::endl ;
324 
326  "rho polar coordinate of the point is null")) ;
327  }
328 
329 
330  if (Z < 0)
331  {
332  vpERROR_TRACE("Point is behind the camera ") ;
333  std::cout <<"Z = " << Z << std::endl ;
334 
336  "Point is behind the camera ")) ;
337  }
338 
339  if (fabs(Z) < 1e-6)
340  {
341  vpERROR_TRACE("Point Z coordinates is null ") ;
342  std::cout <<"Z = " << Z << std::endl ;
343 
345  "Point Z coordinates is null")) ;
346  }
347 
348  if (vpFeaturePointPolar::selectRho() & select )
349  {
350  vpMatrix Lrho(1,6) ; Lrho = 0;
351 
352  Lrho[0][0] = -c/Z ;
353  Lrho[0][1] = -s/Z ;
354  Lrho[0][2] = rho/Z ;
355  Lrho[0][3] = (1+rho2)*s ;
356  Lrho[0][4] = -(1+rho2)*c ;
357  Lrho[0][5] = 0 ;
358 
359 // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
360 // std::cout << "Lrho: " << Lrho << std::endl;
361 
362  L = vpMatrix::stackMatrices(L,Lrho) ;
363  }
364 
365  if (vpFeaturePointPolar::selectTheta() & select )
366  {
367  vpMatrix Ltheta(1,6) ; Ltheta = 0;
368 
369  Ltheta[0][0] = s/(rho*Z) ;
370  Ltheta[0][1] = -c/(rho*Z) ;
371  Ltheta[0][2] = 0 ;
372  Ltheta[0][3] = c/rho ;
373  Ltheta[0][4] = s/rho ;
374  Ltheta[0][5] = -1 ;
375 
376 // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
377 // std::cout << "Ltheta: " << Ltheta << std::endl;
378  L = vpMatrix::stackMatrices(L,Ltheta) ;
379  }
380  return L ;
381 }
382 
427  const unsigned int select)
428 {
429  vpColVector e(0) ;
430 
431  try{
432  if (vpFeaturePointPolar::selectRho() & select )
433  {
434  vpColVector erho(1) ;
435  erho[0] = s[0] - s_star[0] ;
436 
437  e = vpMatrix::stackMatrices(e,erho) ;
438  }
439 
440  if (vpFeaturePointPolar::selectTheta() & select )
441  {
442 
443  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
444  double err = s[1] - s_star[1] ;
445 
446  // printf("Error: %f ", err );
447  while (err < -M_PI) err += 2*M_PI ;
448  while (err > M_PI) err -= 2*M_PI ;
449  // printf(" modif %f \n", err );
450 
451  vpColVector etheta(1) ;
452  etheta[0] = err ;
453  e = vpMatrix::stackMatrices(e,etheta) ;
454  }
455  }
456  catch(vpMatrixException me)
457  {
458  vpERROR_TRACE("caught a Matrix related error") ;
459  std::cout <<std::endl << me << std::endl ;
460  throw(me) ;
461  }
462  catch(vpException me)
463  {
464  vpERROR_TRACE("caught another error") ;
465  std::cout <<std::endl << me << std::endl ;
466  throw(me) ;
467  }
468 
469 
470  return e ;
471 
472 }
473 
474 
496 void
497 vpFeaturePointPolar::print(const unsigned int select ) const
498 {
499 
500  std::cout <<"Point: Z=" << get_Z() ;
501  if (vpFeaturePointPolar::selectRho() & select )
502  std::cout << " rho=" << get_rho() ;
503  if (vpFeaturePointPolar::selectTheta() & select )
504  std::cout << " theta=" << get_theta() ;
505  std::cout <<std::endl ;
506 }
507 
508 
526 void
527 vpFeaturePointPolar::buildFrom(const double rho, const double theta,
528  const double Z)
529 {
530 
531  s[0] = rho ;
532  s[1] = theta ;
533 
534  this->Z = Z ;
535 
536  if (Z < 0)
537  {
538  vpERROR_TRACE("Point is behind the camera ") ;
539  std::cout <<"Z = " << Z << std::endl ;
540 
542  "Point is behind the camera ")) ;
543  }
544 
545  if (fabs(Z) < 1e-6)
546  {
547  vpERROR_TRACE("Point Z coordinates is null ") ;
548  std::cout <<"Z = " << Z << std::endl ;
549 
551  "Point Z coordinates is null")) ;
552  }
553 
554  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
555 }
556 
557 
568 void
570  const vpImage<unsigned char> &I,
571  const vpColor &color,
572  unsigned int thickness) const
573 {
574  try {
575  double rho,theta;
576  rho = get_rho();
577  theta = get_theta();
578 
579  double x,y;
580  x = rho*cos(theta);
581  y = rho*sin(theta);
582 
583  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
584  }
585  catch(...) {
586  vpERROR_TRACE("Error caught") ;
587  throw ;
588  }
589 }
590 
601 void
603  const vpImage<vpRGBa> &I,
604  const vpColor &color,
605  unsigned int thickness) const
606 {
607  try {
608  double rho,theta;
609  rho = get_rho();
610  theta = get_theta();
611 
612  double x,y;
613  x = rho*cos(theta);
614  y = rho*sin(theta);
615 
616  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
617 
618  }
619  catch(...) {
620  vpERROR_TRACE("Error caught") ;
621  throw ;
622  }
623 }
624 
637 {
639  return feature ;
640 }
641 
642 
643 /*
644  * Local variables:
645  * c-basic-offset: 2
646  * End:
647  */