Visual Servoing Platform  version 3.3.1 under development (2020-07-03)
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 
110 {
111  s[0] = rho;
112  flags[0] = true;
113 }
120 {
121  s[1] = theta;
122  flags[1] = true;
123 }
124 
130 {
131  this->Z = Z_;
132  flags[2] = true;
133 }
134 
145 void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, 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 
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, 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(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(double rho, double theta, 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:156
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
void buildFrom(double rho, double theta, double Z)
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4620
unsigned int dim_s
Dimension of the visual feature.
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)
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.
void print(unsigned int select=FEATURE_ALL) const
Generic class defining intrinsic camera parameters.
vpFeaturePointPolar * duplicate() const
void set_rhoThetaZ(double rho, double theta, double Z)
static const unsigned int FEATURE_LINE[32]
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpBasicFeatureDeallocatorType deallocate
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_theta(double theta)
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.