39 #include <visp3/core/vpCylinder.h> 40 #include <visp3/core/vpFeatureDisplay.h> 82 const double Z0,
const double R)
136 const double Z0,
const double R)
201 double co, si, e, x0, y0, z0;
202 double A, B, C, X0, Y0, Z0, R;
203 double s, a, b, c, zero;
212 zero = A * X0 + B * Y0 + C * Z0;
214 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
216 printf(
"The camera is inside the cylinder with s=%f !\n", s);
223 x0 = C * Y0 - B * Z0;
224 y0 = A * Z0 - C * X0;
225 z0 = B * X0 - A * Y0;
230 e = sqrt(co * co + si * si);
231 p_[0] = -(R * c * s - z0) / e;
232 p_[1] = atan2(si, co);
240 e = sqrt(co * co + si * si);
241 p_[2] = -(R * c * s + z0) / e;
242 p_[3] = atan2(si, co);
278 double oA, oB, oC, oX0, oY0, oZ0;
286 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
287 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
288 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
289 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
299 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
300 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
301 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
305 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
306 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
307 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
371 return (B - std::sqrt(B * B - A * C)) / A;
385 const vpColor &color,
const unsigned int thickness)
398 const unsigned int thickness)
vpCylinder * duplicate() const
for memory issue (used by the vpServo class only)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Class to define colors available for display functionnalities.
error that can be emited by ViSP classes.
double computeZ(const double x, const double y) const
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Generic class defining intrinsic camera parameters.
Class that defines what is a cylinder.
Implementation of column vector and the associated operations.
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
void setWorldCoordinates(const vpColVector &oP)
void resize(const unsigned int i, const bool flagNullify=true)