41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeaturePointPolar.h>
45#include <visp3/core/vpException.h>
46#include <visp3/visual_features/vpFeatureException.h>
49#include <visp3/core/vpDebug.h>
52#include <visp3/core/vpMath.h>
54#include <visp3/core/vpFeatureDisplay.h>
253 if (
flags[i] ==
false) {
256 vpTRACE(
"Warning !!! The interaction matrix is computed but rho "
260 vpTRACE(
"Warning !!! The interaction matrix is computed but theta "
264 vpTRACE(
"Warning !!! The interaction matrix is computed but Z was "
268 vpTRACE(
"Problem during the reading of the variable flags");
279 double c_ = cos(theta);
280 double s_ = sin(theta);
282 double rho2 = rho * rho;
284 if (fabs(rho) < 1e-6) {
286 std::cout <<
"rho = " << rho << std::endl;
293 std::cout <<
"Z = " << Z_ << std::endl;
298 if (fabs(Z_) < 1e-6) {
300 std::cout <<
"Z = " << Z_ << std::endl;
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_;
326 Ltheta[0][0] = s_ / (rho * Z_);
327 Ltheta[0][1] = -c_ / (rho * Z_);
329 Ltheta[0][3] = c_ / rho;
330 Ltheta[0][4] = s_ / rho;
389 erho[0] =
s[0] - s_star[0];
398 double err =
s[1] - s_star[1];
442 std::cout <<
"Point: Z=" <<
get_Z();
444 std::cout <<
" rho=" <<
get_rho();
447 std::cout << std::endl;
477 std::cout <<
"Z = " << Z << std::endl;
482 if (fabs(Z) < 1e-6) {
484 std::cout <<
"Z = " << Z << std::endl;
504 unsigned int thickness)
const
512 x = rho * cos(theta);
513 y = rho * sin(theta);
533 unsigned int thickness)
const
541 x = rho * cos(theta);
542 y = rho * sin(theta);
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.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
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 Corke09a.
void buildFrom(double rho, double theta, double Z)
static unsigned int selectTheta()
void set_theta(double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
static unsigned int selectRho()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_rhoThetaZ(double rho, double theta, double Z)
vpFeaturePointPolar * duplicate() const
void print(unsigned int select=FEATURE_ALL) const
Definition of the vpImage class member functions.
Implementation of a matrix and operations on matrices.
void stack(const vpMatrix &A)