ViSP  2.8.0
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  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
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:174
vpFeaturePointPolar * duplicate() const
#define vpERROR_TRACE
Definition: vpDebug.h:379
void print(const unsigned int select=FEATURE_ALL) const
#define vpTRACE
Definition: vpDebug.h:401
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:75
void set_Z(const double Z)
Class that defines 2D image point visual feature with polar coordinates described in ...
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void buildFrom(const double rho, const double theta, const double Z)
void set_rhoThetaZ(const double rho, const double theta, const double Z)
class that defines what is a visual feature
static unsigned int selectRho()
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
static unsigned int selectTheta()
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
vpBasicFeatureDeallocatorType deallocate
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void set_theta(const double theta)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
error that can be emited by the vpMatrix class and its derivates
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpMatrix interaction(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 set_rho(const double rho)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94