 |
Visual Servoing Platform
version 3.2.0
|
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeatureLine.h>
48 #include <visp3/core/vpException.h>
49 #include <visp3/visual_features/vpFeatureException.h>
52 #include <visp3/core/vpDebug.h>
55 #include <visp3/core/vpMath.h>
60 #include <visp3/core/vpCameraParameters.h>
63 #include <visp3/core/vpColor.h>
64 #include <visp3/core/vpImage.h>
66 #include <visp3/core/vpFeatureDisplay.h>
119 for (
int i = 0; i < 2; i++)
204 if (
flags[i] ==
false) {
207 vpTRACE(
"Warning !!! The interaction matrix is computed but rho "
211 vpTRACE(
"Warning !!! The interaction matrix is computed but theta "
215 vpTRACE(
"Warning !!! The interaction matrix is computed but A was "
219 vpTRACE(
"Warning !!! The interaction matrix is computed but B was "
223 vpTRACE(
"Warning !!! The interaction matrix is computed but C was "
227 vpTRACE(
"Warning !!! The interaction matrix is computed but D was "
231 vpTRACE(
"Problem during the reading of the variable flags");
240 double co = cos(theta);
241 double si = sin(theta);
243 if (fabs(D) < 1e-6) {
244 vpERROR_TRACE(
"Incorrect plane coordinates D is null, D = %f", D);
249 double lambda_theta = (A * si - B * co) / D;
250 double lambda_rho = (C + rho * A * co + rho * B * si) / D;
255 Lrho[0][0] = co * lambda_rho;
256 Lrho[0][1] = si * lambda_rho;
257 Lrho[0][2] = -rho * lambda_rho;
258 Lrho[0][3] = si * (1.0 + rho * rho);
259 Lrho[0][4] = -co * (1.0 + rho * rho);
268 Ltheta[0][0] = co * lambda_theta;
269 Ltheta[0][1] = si * lambda_theta;
270 Ltheta[0][2] = -rho * lambda_theta;
271 Ltheta[0][3] = -rho * co;
272 Ltheta[0][4] = -rho * si;
325 erho[0] =
s[0] - s_star[0];
332 double err =
s[1] - s_star[1];
372 std::cout <<
"Line:\t " << A <<
"X+" << B <<
"Y+" << C <<
"Z +" << D <<
"=0" << std::endl;
375 std::cout <<
" \trho=" <<
s[0];
377 std::cout <<
" \ttheta=" <<
s[1];
378 std::cout << std::endl;
399 for (
int i = 0; i < 2; i++)
431 void vpFeatureLine::buildFrom(
const double rho,
const double theta,
const double A_,
const double B_,
const double C_,
471 unsigned int thickness)
const
497 unsigned int thickness)
const
void resize(const unsigned int i, const bool flagNullify=true)
vpFeatureLine * duplicate() const
void print(const unsigned int select=FEATURE_ALL) const
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Generic class defining intrinsic camera parameters.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
static unsigned int selectTheta()
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Implementation of column vector and the associated operations.
void init()
Default initialization.
Implementation of a matrix and operations on matrices.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void buildFrom(const double rho, const double theta)
void setABCD(const double A, const double B, const double C, const double D)
static unsigned int selectRho()
Error that can be emited by the vpBasicFeature class and its derivates.
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
vpColVector s
State of the visual feature.
vpBasicFeatureDeallocatorType deallocate
static const unsigned int FEATURE_LINE[32]
Class to define colors available for display functionnalities.
void setRhoTheta(const double rho, const double theta)
class that defines what is a visual feature