Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeaturePointPolar.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * 2D point visual feature.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 
45 #include <visp3/visual_features/vpBasicFeature.h>
46 #include <visp3/visual_features/vpFeaturePointPolar.h>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 #include <visp3/visual_features/vpFeatureException.h>
51 
52 // Debug trace
53 #include <visp3/core/vpDebug.h>
54 
55 // math
56 #include <visp3/core/vpMath.h>
57 
58 #include <visp3/core/vpFeatureDisplay.h>
59 
60 
61 
62 /*
63 
64  attributes and members directly related to the vpBasicFeature needs
65  other functionalities ar useful but not mandatory
66 
67 */
68 
78 void
80 {
81  //feature dimension
82  dim_s = 2 ;
83  nbParameters = 3;
84 
85  // memory allocation
86  s.resize(dim_s) ;
87  if (flags == NULL)
88  flags = new bool[nbParameters];
89  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
90 
91  //default value Z (1 meters)
92  Z = 1;
93 
94 }
95 
107 {
108  init() ;
109 }
110 
111 
117 void
119 {
120  s[0] = rho ;
121  flags[0] = true;
122 }
128 void
130 {
131  s[1] = theta ;
132  flags[1] = true;
133 }
134 
139 void
141 {
142  this->Z = Z_ ;
143  flags[2] = true;
144 }
145 
156 void
158  const double theta,
159  const double Z_)
160 {
161  set_rho(rho) ;
162  set_theta(theta) ;
163  set_Z(Z_) ;
164 
165  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
166 }
167 
173 double
175 {
176  return s[0] ;
177 }
178 
184 double
186 {
187  return s[1] ;
188 }
193 double
195 {
196  return this->Z ;
197 }
198 
199 
200 
276 vpMatrix
277 vpFeaturePointPolar::interaction(const unsigned int select)
278 {
279  vpMatrix L ;
280 
281  L.resize(0,6) ;
282 
284  {
285  for (unsigned int i = 0; i < nbParameters; i++)
286  {
287  if (flags[i] == false)
288  {
289  switch(i){
290  case 0:
291  vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
292  break;
293  case 1:
294  vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
295  break;
296  case 2:
297  vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
298  break;
299  default:
300  vpTRACE("Problem during the reading of the variable flags");
301  }
302  }
303  }
304  resetFlags();
305  }
306 
307  double rho = get_rho() ;
308  double theta = get_theta() ;
309  double Z_ = get_Z() ;
310 
311  double c_ = cos(theta);
312  double s_ = sin(theta);
313 
314  double rho2 = rho*rho;
315 
316  if (fabs(rho) < 1e-6) {
317  vpERROR_TRACE("rho polar coordinate of the point is null") ;
318  std::cout <<"rho = " << rho << std::endl ;
319 
321  "rho polar coordinate of the point is null")) ;
322  }
323 
324  if (Z_ < 0)
325  {
326  vpERROR_TRACE("Point is behind the camera ") ;
327  std::cout <<"Z = " << Z_ << std::endl ;
328 
330  "Point is behind the camera ")) ;
331  }
332 
333  if (fabs(Z_) < 1e-6)
334  {
335  vpERROR_TRACE("Point Z coordinates is null ") ;
336  std::cout <<"Z = " << Z_ << std::endl ;
337 
339  "Point Z coordinates is null")) ;
340  }
341 
342  if (vpFeaturePointPolar::selectRho() & select )
343  {
344  vpMatrix Lrho(1,6) ; Lrho = 0;
345 
346  Lrho[0][0] = -c_/Z_ ;
347  Lrho[0][1] = -s_/Z_ ;
348  Lrho[0][2] = rho/Z_ ;
349  Lrho[0][3] = (1+rho2)*s_ ;
350  Lrho[0][4] = -(1+rho2)*c_ ;
351  Lrho[0][5] = 0 ;
352 
353 // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
354 // std::cout << "Lrho: " << Lrho << std::endl;
355 
356  L = vpMatrix::stack(L,Lrho) ;
357  }
358 
359  if (vpFeaturePointPolar::selectTheta() & select )
360  {
361  vpMatrix Ltheta(1,6) ; Ltheta = 0;
362 
363  Ltheta[0][0] = s_/(rho*Z_) ;
364  Ltheta[0][1] = -c_/(rho*Z_) ;
365  Ltheta[0][2] = 0 ;
366  Ltheta[0][3] = c_/rho ;
367  Ltheta[0][4] = s_/rho ;
368  Ltheta[0][5] = -1 ;
369 
370 // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
371 // std::cout << "Ltheta: " << Ltheta << std::endl;
372  L = vpMatrix::stack(L,Ltheta) ;
373  }
374  return L ;
375 }
376 
421  const unsigned int select)
422 {
423  vpColVector e(0) ;
424 
425  try{
426  if (vpFeaturePointPolar::selectRho() & select )
427  {
428  vpColVector erho(1) ;
429  erho[0] = s[0] - s_star[0] ;
430 
431  e = vpColVector::stack(e,erho) ;
432  }
433 
434  if (vpFeaturePointPolar::selectTheta() & select )
435  {
436 
437  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
438  double err = s[1] - s_star[1] ;
439 
440  // printf("Error: %f ", err );
441  while (err < -M_PI) err += 2*M_PI ;
442  while (err > M_PI) err -= 2*M_PI ;
443  // printf(" modif %f \n", err );
444 
445  vpColVector etheta(1) ;
446  etheta[0] = err ;
447  e = vpColVector::stack(e,etheta) ;
448  }
449  }
450  catch(...) {
451  throw ;
452  }
453 
454 
455  return e ;
456 
457 }
458 
459 
481 void
482 vpFeaturePointPolar::print(const unsigned int select ) const
483 {
484 
485  std::cout <<"Point: Z=" << get_Z() ;
486  if (vpFeaturePointPolar::selectRho() & select )
487  std::cout << " rho=" << get_rho() ;
488  if (vpFeaturePointPolar::selectTheta() & select )
489  std::cout << " theta=" << get_theta() ;
490  std::cout <<std::endl ;
491 }
492 
493 
511 void
512 vpFeaturePointPolar::buildFrom(const double rho, const double theta,
513  const double Z_)
514 {
515 
516  s[0] = rho ;
517  s[1] = theta ;
518 
519  this->Z = Z_ ;
520 
521  if (Z < 0)
522  {
523  vpERROR_TRACE("Point is behind the camera ") ;
524  std::cout <<"Z = " << Z << std::endl ;
525 
527  "Point is behind the camera ")) ;
528  }
529 
530  if (fabs(Z) < 1e-6)
531  {
532  vpERROR_TRACE("Point Z coordinates is null ") ;
533  std::cout <<"Z = " << Z << std::endl ;
534 
536  "Point Z coordinates is null")) ;
537  }
538 
539  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
540 }
541 
542 
553 void
555  const vpImage<unsigned char> &I,
556  const vpColor &color,
557  unsigned int thickness) const
558 {
559  try {
560  double rho,theta;
561  rho = get_rho();
562  theta = get_theta();
563 
564  double x,y;
565  x = rho*cos(theta);
566  y = rho*sin(theta);
567 
568  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
569  }
570  catch(...) {
571  vpERROR_TRACE("Error caught") ;
572  throw ;
573  }
574 }
575 
586 void
588  const vpImage<vpRGBa> &I,
589  const vpColor &color,
590  unsigned int thickness) const
591 {
592  try {
593  double rho,theta;
594  rho = get_rho();
595  theta = get_theta();
596 
597  double x,y;
598  x = rho*cos(theta);
599  y = rho*sin(theta);
600 
601  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
602 
603  }
604  catch(...) {
605  vpERROR_TRACE("Error caught") ;
606  throw ;
607  }
608 }
609 
622 {
624  return feature ;
625 }
626 
650 unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0] ; }
651 
652 
676 unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpFeaturePointPolar * duplicate() const
void print(const unsigned int select=FEATURE_ALL) const
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
unsigned int dim_s
Dimension of the visual feature.
void set_Z(const double Z)
Class that defines 2D image point visual feature with polar coordinates described in ...
static unsigned int selectTheta()
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()
#define vpTRACE
Definition: vpDebug.h:414
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_theta(const double theta)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
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:225