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 .
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
Implementation of a matrix and operations on matrices.
void stack(const vpMatrix &A)