Visual Servoing Platform  version 3.6.1 under development (2024-10-14)
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 
63 BEGIN_VISP_NAMESPACE
74 {
75  // feature dimension
76  dim_s = 2;
77  nbParameters = 3;
78 
79  // memory allocation
80  s.resize(dim_s);
81  if (flags == nullptr)
82  flags = new bool[nbParameters];
83  for (unsigned int i = 0; i < nbParameters; i++)
84  flags[i] = false;
85 
86  // default value Z (1 meters)
87  Z = 1;
88 }
89 
101 
108 {
109  s[0] = rho;
110  flags[0] = true;
111 }
118 {
119  s[1] = theta;
120  flags[1] = true;
121 }
122 
128 {
129  this->Z = Z_;
130  flags[2] = true;
131 }
132 
143 void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, double Z_)
144 {
145  set_rho(rho);
146  set_theta(theta);
147  set_Z(Z_);
148 
149  for (unsigned int i = 0; i < nbParameters; i++)
150  flags[i] = true;
151 }
152 
158 double vpFeaturePointPolar::get_rho() const { return s[0]; }
159 
165 double vpFeaturePointPolar::get_theta() const { return s[1]; }
170 double vpFeaturePointPolar::get_Z() const { return this->Z; }
171 
247 {
248  vpMatrix L;
249 
250  L.resize(0, 6);
251 
253  for (unsigned int i = 0; i < nbParameters; i++) {
254  if (flags[i] == false) {
255  switch (i) {
256  case 0:
257  vpTRACE("Warning !!! The interaction matrix is computed but rho "
258  "was not set yet");
259  break;
260  case 1:
261  vpTRACE("Warning !!! The interaction matrix is computed but theta "
262  "was not set yet");
263  break;
264  case 2:
265  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
266  "not set yet");
267  break;
268  default:
269  vpTRACE("Problem during the reading of the variable flags");
270  }
271  }
272  }
273  resetFlags();
274  }
275 
276  double rho = get_rho();
277  double theta = get_theta();
278  double Z_ = get_Z();
279 
280  double c_ = cos(theta);
281  double s_ = sin(theta);
282 
283  double rho2 = rho * rho;
284 
285  if (fabs(rho) < 1e-6) {
286  vpERROR_TRACE("rho polar coordinate of the point is null");
287  std::cout << "rho = " << rho << std::endl;
288 
289  throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
290  }
291 
292  if (Z_ < 0) {
293  vpERROR_TRACE("Point is behind the camera ");
294  std::cout << "Z = " << Z_ << std::endl;
295 
296  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
297  }
298 
299  if (fabs(Z_) < 1e-6) {
300  vpERROR_TRACE("Point Z coordinates is null ");
301  std::cout << "Z = " << Z_ << std::endl;
302 
303  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
304  }
305 
306  if (vpFeaturePointPolar::selectRho() & select) {
307  vpMatrix Lrho(1, 6);
308  Lrho = 0;
309 
310  Lrho[0][0] = -c_ / Z_;
311  Lrho[0][1] = -s_ / Z_;
312  Lrho[0][2] = rho / Z_;
313  Lrho[0][3] = (1 + rho2) * s_;
314  Lrho[0][4] = -(1 + rho2) * c_;
315  Lrho[0][5] = 0;
316 
317  // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
318  // std::cout << "Lrho: " << Lrho << std::endl;
319 
320  L = vpMatrix::stack(L, Lrho);
321  }
322 
323  if (vpFeaturePointPolar::selectTheta() & select) {
324  vpMatrix Ltheta(1, 6);
325  Ltheta = 0;
326 
327  Ltheta[0][0] = s_ / (rho * Z_);
328  Ltheta[0][1] = -c_ / (rho * Z_);
329  Ltheta[0][2] = 0;
330  Ltheta[0][3] = c_ / rho;
331  Ltheta[0][4] = s_ / rho;
332  Ltheta[0][5] = -1;
333 
334  // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
335  // std::cout << "Ltheta: " << Ltheta << std::endl;
336  L = vpMatrix::stack(L, Ltheta);
337  }
338  return L;
339 }
340 
383 vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, unsigned int select)
384 {
385  vpColVector e(0);
386 
387  try {
388  if (vpFeaturePointPolar::selectRho() & select) {
389  vpColVector erho(1);
390  erho[0] = s[0] - s_star[0];
391 
392  e = vpColVector::stack(e, erho);
393  }
394 
395  if (vpFeaturePointPolar::selectTheta() & select) {
396 
397  // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
398  // s_star[1]);
399  double err = s[1] - s_star[1];
400 
401  // printf("Error: %f ", err );
402  while (err < -M_PI)
403  err += 2 * M_PI;
404  while (err > M_PI)
405  err -= 2 * M_PI;
406  // printf(" modif %f \n", err );
407 
408  vpColVector etheta(1);
409  etheta[0] = err;
410  e = vpColVector::stack(e, etheta);
411  }
412  }
413  catch (...) {
414  throw;
415  }
416 
417  return e;
418 }
419 
441 void vpFeaturePointPolar::print(unsigned int select) const
442 {
443 
444  std::cout << "Point: Z=" << get_Z();
445  if (vpFeaturePointPolar::selectRho() & select)
446  std::cout << " rho=" << get_rho();
447  if (vpFeaturePointPolar::selectTheta() & select)
448  std::cout << " theta=" << get_theta();
449  std::cout << std::endl;
450 }
451 
452 vpFeaturePointPolar &vpFeaturePointPolar::buildFrom(const double &rho, const double &theta, const double &Z_)
453 {
454 
455  s[0] = rho;
456  s[1] = theta;
457 
458  this->Z = Z_;
459 
460  if (Z < 0) {
461  vpERROR_TRACE("Point is behind the camera ");
462  std::cout << "Z = " << Z << std::endl;
463 
464  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
465  }
466 
467  if (fabs(Z) < 1e-6) {
468  vpERROR_TRACE("Point Z coordinates is null ");
469  std::cout << "Z = " << Z << std::endl;
470 
471  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
472  }
473 
474  for (unsigned int i = 0; i < nbParameters; ++i) {
475  flags[i] = true;
476  }
477  return *this;
478 }
479 
491  unsigned int thickness) const
492 {
493  try {
494  double rho, theta;
495  rho = get_rho();
496  theta = get_theta();
497 
498  double x, y;
499  x = rho * cos(theta);
500  y = rho * sin(theta);
501 
502  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
503  }
504  catch (...) {
505  vpERROR_TRACE("Error caught");
506  throw;
507  }
508 }
509 
521  unsigned int thickness) const
522 {
523  try {
524  double rho, theta;
525  rho = get_rho();
526  theta = get_theta();
527 
528  double x, y;
529  x = rho * cos(theta);
530  y = rho * sin(theta);
531 
532  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
533 
534  }
535  catch (...) {
536  vpERROR_TRACE("Error caught");
537  throw;
538  }
539 }
540 
553 {
555  return feature;
556 }
557 
581 unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
582 
605 unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1]; }
606 END_VISP_NAMESPACE
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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 print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
vpFeaturePointPolar & buildFrom(const double &rho, const double &theta, const double &Z)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const VP_OVERRIDE
static unsigned int selectTheta()
void set_theta(double theta)
void init() VP_OVERRIDE
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
vpFeaturePointPolar * duplicate() const VP_OVERRIDE
static unsigned int selectRho()
void set_rhoThetaZ(double rho, double theta, double Z)
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)