49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeatureLine.h>
53 #include <visp/vpException.h>
54 #include <visp/vpMatrixException.h>
55 #include <visp/vpFeatureException.h>
58 #include <visp/vpDebug.h>
61 #include <visp/vpMath.h>
66 #include <visp/vpCameraParameters.h>
69 #include <visp/vpColor.h>
70 #include <visp/vpImage.h>
74 #include <visp/vpFeatureDisplay.h>
109 A = B = C = D = 0.0 ;
133 for(
int i = 0; i < 2; i++)
flags[i] =
true;
149 const double C,
const double D)
218 if (
flags[i] ==
false)
222 vpTRACE(
"Warning !!! The interaction matrix is computed but rho was not set yet");
225 vpTRACE(
"Warning !!! The interaction matrix is computed but theta was not set yet");
228 vpTRACE(
"Warning !!! The interaction matrix is computed but A was not set yet");
231 vpTRACE(
"Warning !!! The interaction matrix is computed but B was not set yet");
234 vpTRACE(
"Warning !!! The interaction matrix is computed but C was not set yet");
237 vpTRACE(
"Warning !!! The interaction matrix is computed but D was not set yet");
240 vpTRACE(
"Problem during the reading of the variable flags");
247 double theta =
s[1] ;
250 double co = cos(theta);
251 double si = sin(theta);
255 vpERROR_TRACE(
"Incorrect plane coordinates D is null, D = %f",D) ;
258 "Incorrect plane coordinates D")) ;
261 double lambda_theta =( A*si - B*co) /D;
262 double lambda_rho = (C + rho*A*co + rho*B*si)/D;
269 Lrho[0][0]= co*lambda_rho;
270 Lrho[0][1]= si*lambda_rho;
271 Lrho[0][2]= -rho*lambda_rho;
272 Lrho[0][3]= si*(1.0 + rho*rho);
273 Lrho[0][4]= -co*(1.0 + rho*rho);
283 Ltheta[0][0] = co*lambda_theta;
284 Ltheta[0][1] = si*lambda_theta;
285 Ltheta[0][2] = -rho*lambda_theta;
286 Ltheta[0][3] = -rho*co;
287 Ltheta[0][4] = -rho*si;
336 const unsigned int select)
344 erho[0] =
s[0] - s_star[0] ;
354 double err =
s[1] - s_star[1] ;
355 while (err < -M_PI) err += 2*M_PI ;
356 while (err > M_PI) err -= 2*M_PI ;
366 std::cout <<std::endl << me << std::endl ;
372 std::cout <<std::endl << me << std::endl ;
406 std::cout <<
"Line:\t " << A <<
"X+" << B <<
"Y+" << C <<
"Z +" << D <<
"=0" <<std::endl ;;
408 std::cout <<
" \trho=" <<
s[0] ;
410 std::cout <<
" \ttheta=" << s[1] ;
411 std::cout <<std::endl ;
434 for(
int i = 0; i < 2; i++)
flags[i] =
true;
467 const double A,
const double B,
468 const double C,
const double D)
512 unsigned int thickness)
const
543 unsigned int thickness)
const