Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpFeaturePointPolar.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * 2D point visual feature.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeaturePointPolar.h>
46 
47 // Exception
48 #include <visp3/core/vpException.h>
49 #include <visp3/visual_features/vpFeatureException.h>
50 
51 // Debug trace
52 #include <visp3/core/vpDebug.h>
53 
54 // math
55 #include <visp3/core/vpMath.h>
56 
57 #include <visp3/core/vpFeatureDisplay.h>
58 
59 /*
60 
61  attributes and members directly related to the vpBasicFeature needs
62  other functionalities ar useful but not mandatory
63 
64 */
65 
76 {
77  // feature dimension
78  dim_s = 2;
79  nbParameters = 3;
80 
81  // memory allocation
82  s.resize(dim_s);
83  if (flags == NULL)
84  flags = new bool[nbParameters];
85  for (unsigned int i = 0; i < nbParameters; i++)
86  flags[i] = false;
87 
88  // default value Z (1 meters)
89  Z = 1;
90 }
91 
103 
109 void vpFeaturePointPolar::set_rho(const double rho)
110 {
111  s[0] = rho;
112  flags[0] = true;
113 }
119 void vpFeaturePointPolar::set_theta(const double theta)
120 {
121  s[1] = theta;
122  flags[1] = true;
123 }
124 
129 void vpFeaturePointPolar::set_Z(const double Z_)
130 {
131  this->Z = Z_;
132  flags[2] = true;
133 }
134 
145 void vpFeaturePointPolar::set_rhoThetaZ(const double rho, const double theta, const double Z_)
146 {
147  set_rho(rho);
148  set_theta(theta);
149  set_Z(Z_);
150 
151  for (unsigned int i = 0; i < nbParameters; i++)
152  flags[i] = true;
153 }
154 
160 double vpFeaturePointPolar::get_rho() const { return s[0]; }
161 
167 double vpFeaturePointPolar::get_theta() const { return s[1]; }
172 double vpFeaturePointPolar::get_Z() const { return this->Z; }
173 
248 vpMatrix vpFeaturePointPolar::interaction(const unsigned int select)
249 {
250  vpMatrix L;
251 
252  L.resize(0, 6);
253 
255  for (unsigned int i = 0; i < nbParameters; i++) {
256  if (flags[i] == false) {
257  switch (i) {
258  case 0:
259  vpTRACE("Warning !!! The interaction matrix is computed but rho "
260  "was not set yet");
261  break;
262  case 1:
263  vpTRACE("Warning !!! The interaction matrix is computed but theta "
264  "was not set yet");
265  break;
266  case 2:
267  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
268  "not set yet");
269  break;
270  default:
271  vpTRACE("Problem during the reading of the variable flags");
272  }
273  }
274  }
275  resetFlags();
276  }
277 
278  double rho = get_rho();
279  double theta = get_theta();
280  double Z_ = get_Z();
281 
282  double c_ = cos(theta);
283  double s_ = sin(theta);
284 
285  double rho2 = rho * rho;
286 
287  if (fabs(rho) < 1e-6) {
288  vpERROR_TRACE("rho polar coordinate of the point is null");
289  std::cout << "rho = " << rho << std::endl;
290 
291  throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
292  }
293 
294  if (Z_ < 0) {
295  vpERROR_TRACE("Point is behind the camera ");
296  std::cout << "Z = " << Z_ << std::endl;
297 
298  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
299  }
300 
301  if (fabs(Z_) < 1e-6) {
302  vpERROR_TRACE("Point Z coordinates is null ");
303  std::cout << "Z = " << Z_ << std::endl;
304 
305  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
306  }
307 
308  if (vpFeaturePointPolar::selectRho() & select) {
309  vpMatrix Lrho(1, 6);
310  Lrho = 0;
311 
312  Lrho[0][0] = -c_ / Z_;
313  Lrho[0][1] = -s_ / Z_;
314  Lrho[0][2] = rho / Z_;
315  Lrho[0][3] = (1 + rho2) * s_;
316  Lrho[0][4] = -(1 + rho2) * c_;
317  Lrho[0][5] = 0;
318 
319  // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
320  // std::cout << "Lrho: " << Lrho << std::endl;
321 
322  L = vpMatrix::stack(L, Lrho);
323  }
324 
325  if (vpFeaturePointPolar::selectTheta() & select) {
326  vpMatrix Ltheta(1, 6);
327  Ltheta = 0;
328 
329  Ltheta[0][0] = s_ / (rho * Z_);
330  Ltheta[0][1] = -c_ / (rho * Z_);
331  Ltheta[0][2] = 0;
332  Ltheta[0][3] = c_ / rho;
333  Ltheta[0][4] = s_ / rho;
334  Ltheta[0][5] = -1;
335 
336  // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
337  // std::cout << "Ltheta: " << Ltheta << std::endl;
338  L = vpMatrix::stack(L, Ltheta);
339  }
340  return L;
341 }
342 
385 vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, const unsigned int select)
386 {
387  vpColVector e(0);
388 
389  try {
390  if (vpFeaturePointPolar::selectRho() & select) {
391  vpColVector erho(1);
392  erho[0] = s[0] - s_star[0];
393 
394  e = vpColVector::stack(e, erho);
395  }
396 
397  if (vpFeaturePointPolar::selectTheta() & select) {
398 
399  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
400  // s_star[1]);
401  double err = s[1] - s_star[1];
402 
403  // printf("Error: %f ", err );
404  while (err < -M_PI)
405  err += 2 * M_PI;
406  while (err > M_PI)
407  err -= 2 * M_PI;
408  // printf(" modif %f \n", err );
409 
410  vpColVector etheta(1);
411  etheta[0] = err;
412  e = vpColVector::stack(e, etheta);
413  }
414  } catch (...) {
415  throw;
416  }
417 
418  return e;
419 }
420 
442 void vpFeaturePointPolar::print(const unsigned int select) const
443 {
444 
445  std::cout << "Point: Z=" << get_Z();
446  if (vpFeaturePointPolar::selectRho() & select)
447  std::cout << " rho=" << get_rho();
448  if (vpFeaturePointPolar::selectTheta() & select)
449  std::cout << " theta=" << get_theta();
450  std::cout << std::endl;
451 }
452 
470 void vpFeaturePointPolar::buildFrom(const double rho, const double theta, const double Z_)
471 {
472 
473  s[0] = rho;
474  s[1] = theta;
475 
476  this->Z = Z_;
477 
478  if (Z < 0) {
479  vpERROR_TRACE("Point is behind the camera ");
480  std::cout << "Z = " << Z << std::endl;
481 
482  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
483  }
484 
485  if (fabs(Z) < 1e-6) {
486  vpERROR_TRACE("Point Z coordinates is null ");
487  std::cout << "Z = " << Z << std::endl;
488 
489  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
490  }
491 
492  for (unsigned int i = 0; i < nbParameters; i++)
493  flags[i] = true;
494 }
495 
507  unsigned int thickness) const
508 {
509  try {
510  double rho, theta;
511  rho = get_rho();
512  theta = get_theta();
513 
514  double x, y;
515  x = rho * cos(theta);
516  y = rho * sin(theta);
517 
518  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
519  } catch (...) {
520  vpERROR_TRACE("Error caught");
521  throw;
522  }
523 }
524 
536  unsigned int thickness) const
537 {
538  try {
539  double rho, theta;
540  rho = get_rho();
541  theta = get_theta();
542 
543  double x, y;
544  x = rho * cos(theta);
545  y = rho * sin(theta);
546 
547  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
548 
549  } catch (...) {
550  vpERROR_TRACE("Error caught");
551  throw;
552  }
553 }
554 
567 {
569  return feature;
570 }
571 
595 unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
596 
619 unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpFeaturePointPolar * duplicate() const
void print(const unsigned int select=FEATURE_ALL) const
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4551
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:416
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:130
void stack(double d)
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:310