Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpFeaturePointPolar.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeaturePointPolar.h>
43 
44 // Exception
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
51 // math
52 #include <visp3/core/vpMath.h>
53 
54 #include <visp3/core/vpFeatureDisplay.h>
55 
56 /*
57 
58  attributes and members directly related to the vpBasicFeature needs
59  other functionalities ar useful but not mandatory
60 
61 */
62 
73 {
74  // feature dimension
75  dim_s = 2;
76  nbParameters = 3;
77 
78  // memory allocation
79  s.resize(dim_s);
80  if (flags == nullptr)
81  flags = new bool[nbParameters];
82  for (unsigned int i = 0; i < nbParameters; i++)
83  flags[i] = false;
84 
85  // default value Z (1 meters)
86  Z = 1;
87 }
88 
100 
107 {
108  s[0] = rho;
109  flags[0] = true;
110 }
117 {
118  s[1] = theta;
119  flags[1] = true;
120 }
121 
127 {
128  this->Z = Z_;
129  flags[2] = true;
130 }
131 
142 void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, double Z_)
143 {
144  set_rho(rho);
145  set_theta(theta);
146  set_Z(Z_);
147 
148  for (unsigned int i = 0; i < nbParameters; i++)
149  flags[i] = true;
150 }
151 
157 double vpFeaturePointPolar::get_rho() const { return s[0]; }
158 
164 double vpFeaturePointPolar::get_theta() const { return s[1]; }
169 double vpFeaturePointPolar::get_Z() const { return this->Z; }
170 
246 {
247  vpMatrix L;
248 
249  L.resize(0, 6);
250 
252  for (unsigned int i = 0; i < nbParameters; i++) {
253  if (flags[i] == false) {
254  switch (i) {
255  case 0:
256  vpTRACE("Warning !!! The interaction matrix is computed but rho "
257  "was not set yet");
258  break;
259  case 1:
260  vpTRACE("Warning !!! The interaction matrix is computed but theta "
261  "was not set yet");
262  break;
263  case 2:
264  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
265  "not set yet");
266  break;
267  default:
268  vpTRACE("Problem during the reading of the variable flags");
269  }
270  }
271  }
272  resetFlags();
273  }
274 
275  double rho = get_rho();
276  double theta = get_theta();
277  double Z_ = get_Z();
278 
279  double c_ = cos(theta);
280  double s_ = sin(theta);
281 
282  double rho2 = rho * rho;
283 
284  if (fabs(rho) < 1e-6) {
285  vpERROR_TRACE("rho polar coordinate of the point is null");
286  std::cout << "rho = " << rho << std::endl;
287 
288  throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
289  }
290 
291  if (Z_ < 0) {
292  vpERROR_TRACE("Point is behind the camera ");
293  std::cout << "Z = " << Z_ << std::endl;
294 
295  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
296  }
297 
298  if (fabs(Z_) < 1e-6) {
299  vpERROR_TRACE("Point Z coordinates is null ");
300  std::cout << "Z = " << Z_ << std::endl;
301 
302  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
303  }
304 
305  if (vpFeaturePointPolar::selectRho() & select) {
306  vpMatrix Lrho(1, 6);
307  Lrho = 0;
308 
309  Lrho[0][0] = -c_ / Z_;
310  Lrho[0][1] = -s_ / Z_;
311  Lrho[0][2] = rho / Z_;
312  Lrho[0][3] = (1 + rho2) * s_;
313  Lrho[0][4] = -(1 + rho2) * c_;
314  Lrho[0][5] = 0;
315 
316  // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
317  // std::cout << "Lrho: " << Lrho << std::endl;
318 
319  L = vpMatrix::stack(L, Lrho);
320  }
321 
322  if (vpFeaturePointPolar::selectTheta() & select) {
323  vpMatrix Ltheta(1, 6);
324  Ltheta = 0;
325 
326  Ltheta[0][0] = s_ / (rho * Z_);
327  Ltheta[0][1] = -c_ / (rho * Z_);
328  Ltheta[0][2] = 0;
329  Ltheta[0][3] = c_ / rho;
330  Ltheta[0][4] = s_ / rho;
331  Ltheta[0][5] = -1;
332 
333  // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
334  // std::cout << "Ltheta: " << Ltheta << std::endl;
335  L = vpMatrix::stack(L, Ltheta);
336  }
337  return L;
338 }
339 
382 vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, unsigned int select)
383 {
384  vpColVector e(0);
385 
386  try {
387  if (vpFeaturePointPolar::selectRho() & select) {
388  vpColVector erho(1);
389  erho[0] = s[0] - s_star[0];
390 
391  e = vpColVector::stack(e, erho);
392  }
393 
394  if (vpFeaturePointPolar::selectTheta() & select) {
395 
396  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
397  // s_star[1]);
398  double err = s[1] - s_star[1];
399 
400  // printf("Error: %f ", err );
401  while (err < -M_PI)
402  err += 2 * M_PI;
403  while (err > M_PI)
404  err -= 2 * M_PI;
405  // printf(" modif %f \n", err );
406 
407  vpColVector etheta(1);
408  etheta[0] = err;
409  e = vpColVector::stack(e, etheta);
410  }
411  } catch (...) {
412  throw;
413  }
414 
415  return e;
416 }
417 
439 void vpFeaturePointPolar::print(unsigned int select) const
440 {
441 
442  std::cout << "Point: Z=" << get_Z();
443  if (vpFeaturePointPolar::selectRho() & select)
444  std::cout << " rho=" << get_rho();
445  if (vpFeaturePointPolar::selectTheta() & select)
446  std::cout << " theta=" << get_theta();
447  std::cout << std::endl;
448 }
449 
467 void vpFeaturePointPolar::buildFrom(double rho, double theta, double Z_)
468 {
469 
470  s[0] = rho;
471  s[1] = theta;
472 
473  this->Z = Z_;
474 
475  if (Z < 0) {
476  vpERROR_TRACE("Point is behind the camera ");
477  std::cout << "Z = " << Z << std::endl;
478 
479  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
480  }
481 
482  if (fabs(Z) < 1e-6) {
483  vpERROR_TRACE("Point Z coordinates is null ");
484  std::cout << "Z = " << Z << std::endl;
485 
486  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
487  }
488 
489  for (unsigned int i = 0; i < nbParameters; i++)
490  flags[i] = true;
491 }
492 
504  unsigned int thickness) const
505 {
506  try {
507  double rho, theta;
508  rho = get_rho();
509  theta = get_theta();
510 
511  double x, y;
512  x = rho * cos(theta);
513  y = rho * sin(theta);
514 
515  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
516  } catch (...) {
517  vpERROR_TRACE("Error caught");
518  throw;
519  }
520 }
521 
533  unsigned int thickness) const
534 {
535  try {
536  double rho, theta;
537  rho = get_rho();
538  theta = get_theta();
539 
540  double x, y;
541  x = rho * cos(theta);
542  y = rho * sin(theta);
543 
544  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
545 
546  } catch (...) {
547  vpERROR_TRACE("Error caught");
548  throw;
549  }
550 }
551 
564 {
566  return feature;
567 }
568 
592 unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
593 
616 unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1]; }
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines 2D image point visual feature with polar coordinates described in .
void buildFrom(double rho, double theta, double Z)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
void print(unsigned int select=FEATURE_ALL) const vp_override
static unsigned int selectTheta()
void set_theta(double theta)
vpFeaturePointPolar * duplicate() const vp_override
static unsigned int selectRho()
void init() vp_override
void set_rhoThetaZ(double rho, double theta, double Z)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5865
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382