ViSP  2.9.0
vpFeaturePointPolar.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeaturePointPolar.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 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  if (Z_ < 0)
330  {
331  vpERROR_TRACE("Point is behind the camera ") ;
332  std::cout <<"Z = " << Z_ << std::endl ;
333 
335  "Point is behind the camera ")) ;
336  }
337 
338  if (fabs(Z_) < 1e-6)
339  {
340  vpERROR_TRACE("Point Z coordinates is null ") ;
341  std::cout <<"Z = " << Z_ << std::endl ;
342 
344  "Point Z coordinates is null")) ;
345  }
346 
347  if (vpFeaturePointPolar::selectRho() & select )
348  {
349  vpMatrix Lrho(1,6) ; Lrho = 0;
350 
351  Lrho[0][0] = -c_/Z_ ;
352  Lrho[0][1] = -s_/Z_ ;
353  Lrho[0][2] = rho/Z_ ;
354  Lrho[0][3] = (1+rho2)*s_ ;
355  Lrho[0][4] = -(1+rho2)*c_ ;
356  Lrho[0][5] = 0 ;
357 
358 // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
359 // std::cout << "Lrho: " << Lrho << std::endl;
360 
361  L = vpMatrix::stackMatrices(L,Lrho) ;
362  }
363 
364  if (vpFeaturePointPolar::selectTheta() & select )
365  {
366  vpMatrix Ltheta(1,6) ; Ltheta = 0;
367 
368  Ltheta[0][0] = s_/(rho*Z_) ;
369  Ltheta[0][1] = -c_/(rho*Z_) ;
370  Ltheta[0][2] = 0 ;
371  Ltheta[0][3] = c_/rho ;
372  Ltheta[0][4] = s_/rho ;
373  Ltheta[0][5] = -1 ;
374 
375 // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
376 // std::cout << "Ltheta: " << Ltheta << std::endl;
377  L = vpMatrix::stackMatrices(L,Ltheta) ;
378  }
379  return L ;
380 }
381 
426  const unsigned int select)
427 {
428  vpColVector e(0) ;
429 
430  try{
431  if (vpFeaturePointPolar::selectRho() & select )
432  {
433  vpColVector erho(1) ;
434  erho[0] = s[0] - s_star[0] ;
435 
436  e = vpMatrix::stackMatrices(e,erho) ;
437  }
438 
439  if (vpFeaturePointPolar::selectTheta() & select )
440  {
441 
442  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
443  double err = s[1] - s_star[1] ;
444 
445  // printf("Error: %f ", err );
446  while (err < -M_PI) err += 2*M_PI ;
447  while (err > M_PI) err -= 2*M_PI ;
448  // printf(" modif %f \n", err );
449 
450  vpColVector etheta(1) ;
451  etheta[0] = err ;
452  e = vpMatrix::stackMatrices(e,etheta) ;
453  }
454  }
455  catch(vpMatrixException me)
456  {
457  vpERROR_TRACE("caught a Matrix related error") ;
458  std::cout <<std::endl << me << std::endl ;
459  throw(me) ;
460  }
461  catch(vpException me)
462  {
463  vpERROR_TRACE("caught another error") ;
464  std::cout <<std::endl << me << std::endl ;
465  throw(me) ;
466  }
467 
468 
469  return e ;
470 
471 }
472 
473 
495 void
496 vpFeaturePointPolar::print(const unsigned int select ) const
497 {
498 
499  std::cout <<"Point: Z=" << get_Z() ;
500  if (vpFeaturePointPolar::selectRho() & select )
501  std::cout << " rho=" << get_rho() ;
502  if (vpFeaturePointPolar::selectTheta() & select )
503  std::cout << " theta=" << get_theta() ;
504  std::cout <<std::endl ;
505 }
506 
507 
525 void
526 vpFeaturePointPolar::buildFrom(const double rho, const double theta,
527  const double Z_)
528 {
529 
530  s[0] = rho ;
531  s[1] = theta ;
532 
533  this->Z = Z_ ;
534 
535  if (Z < 0)
536  {
537  vpERROR_TRACE("Point is behind the camera ") ;
538  std::cout <<"Z = " << Z << std::endl ;
539 
541  "Point is behind the camera ")) ;
542  }
543 
544  if (fabs(Z) < 1e-6)
545  {
546  vpERROR_TRACE("Point Z coordinates is null ") ;
547  std::cout <<"Z = " << Z << std::endl ;
548 
550  "Point Z coordinates is null")) ;
551  }
552 
553  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
554 }
555 
556 
567 void
569  const vpImage<unsigned char> &I,
570  const vpColor &color,
571  unsigned int thickness) const
572 {
573  try {
574  double rho,theta;
575  rho = get_rho();
576  theta = get_theta();
577 
578  double x,y;
579  x = rho*cos(theta);
580  y = rho*sin(theta);
581 
582  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
583  }
584  catch(...) {
585  vpERROR_TRACE("Error caught") ;
586  throw ;
587  }
588 }
589 
600 void
602  const vpImage<vpRGBa> &I,
603  const vpColor &color,
604  unsigned int thickness) const
605 {
606  try {
607  double rho,theta;
608  rho = get_rho();
609  theta = get_theta();
610 
611  double x,y;
612  x = rho*cos(theta);
613  y = rho*sin(theta);
614 
615  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
616 
617  }
618  catch(...) {
619  vpERROR_TRACE("Error caught") ;
620  throw ;
621  }
622 }
623 
636 {
638  return feature ;
639 }
640 
641 
642 /*
643  * Local variables:
644  * c-basic-offset: 2
645  * End:
646  */
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:183
vpFeaturePointPolar * duplicate() const
#define vpERROR_TRACE
Definition: vpDebug.h:395
void print(const unsigned int select=FEATURE_ALL) const
#define vpTRACE
Definition: vpDebug.h:418
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:76
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.
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3003
static unsigned int selectTheta()
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