vpHomography::vpHomography(const vpThetaUVector &tu,
const vpTranslationVector &atb,
const vpPlane &p)
: vpArray2D<double>(3,3), aMb(), bP()
{
buildFrom(tu, atb, p) ;
}
vpHomography::vpHomography(const vpThetaUVector &tu,
const vpTranslationVector &atb,
const vpPlane &_bP) : vpMatrix()
{
init() ;
buildFrom(tu,atb,_bP) ;
}
vpHomography::vpHomography(const vpRotationMatrix &aRb,
const vpTranslationVector &atb,
const vpPlane &p)
: vpArray2D<double>(3,3), aMb(), bP()
{
buildFrom(aRb, atb, p) ;
}
vpHomography::vpHomography(const vpPoseVector &arb,
const vpPlane &_bP) : vpMatrix()
{
init() ;
buildFrom(arb,_bP) ;
}
vpHomography::vpHomography(const vpRotationMatrix &aRb,
const vpTranslationVector &atb,
const vpPlane &_bP) : vpMatrix()
{
init() ;
buildFrom(aRb,atb,_bP) ;
}
/*!
Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
representation vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpRzyxVector& rzyx)
{
vpRotationMatrix R(rzyx) ;
buildFrom(R) ;
return *this ;
}
/*!
Build a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
representation vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpRxyzVector& rxyz)
{
vpRotationMatrix R(rxyz) ;
buildFrom(R) ;
return *this ;
}
/*!
Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$ from
an homogeneous matrix \f$ \bf M \f$.
\param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
\bf t \f$ and \f$\Theta \bf u \f$ vectors are extracted to initialize
the pose vector.
\return The build pose vector.
*/
vpPoseVector
vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
{
vpRotationMatrix R ; M.extract(R) ;
vpTranslationVector t ; M.extract(t) ;
buildFrom(t,R) ;
return *this ;
}
/*!
Constructor that build a 3D visual feature from an homogeneous
matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
\param f2Mf1 [in] : 3D displacement that the camera has to achieve to
move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
\param r : type of feature. It can be vpFeature::cdMc or vpFeature::cMo.
*/
vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, vpFeatureTranslationRepresentationType r) : vpBasicFeature()
{
init() ;
translation = r;
buildFrom(f2Mf1) ;
}
/*!
Build a 3D \f$ \theta u \f$ visual feature from an
homogeneous matrix \f$ M \f$ that represent the displacement that
the camera has to achieve.
\param M [in] : Homogeneous transformation that describe the
movement that the camera has to achieve. Only the rotational part of
this homogeneous transformation is taken into consideration.
Depending on the rotation representation type
(vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
construct this object, the parameter \e M represents either the
rotation that the camera has to achieve to move from the desired
camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
that the camera has to achieve to move from the current camera frame
to the desired one (\f$ ^{c}R_{c^*}\f$).
*/
void
vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
vpRotationMatrix R ;
M.extract(R) ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
/*!
Build a \f$\theta {\bf u}\f$ vector from a quaternion
representation vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpQuaternionVector& q)
{
vpRotationMatrix R(q) ;
buildFrom(R) ;
return *this ;
}
vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu,
vpFeatureThetaURotationRepresentationType r)
: rotation(r)
{
init() ;
buildFrom(tu) ;
}
/*!
Initialize a force/torque twist transformation matrix from a translation vector
\f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
\param tx,ty,tz : Translation vector in meters.
\param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz,
const double tux, const double tuy, const double tuz)
: vpArray2D<double>(6,6)
{
vpTranslationVector T(tx,ty,tz) ;
vpThetaUVector tu(tux,tuy,tuz) ;
buildFrom(T,tu) ;
}
vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
vpFeatureThetaURotationRepresentationType r)
: rotation(r)
{
init() ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
/*!
Initialize a force/torque twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
\param tv : Translation vector.
\param thetau : \f$\theta {\bf u}\f$ rotation vector.
*/
vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
const vpThetaUVector &thetau)
{
vpRotationMatrix R ;
R.buildFrom(thetau) ;
buildFrom(tv,R) ;
return (*this) ;
}
/*!
Initialise the triangle thanks to the collection of 2D points (in meter). The
fields \e x and \e y are used to compute the corresponding coordinates in
pixel thanks to the camera parameters \e cam.
\warning the corners must be ordered (either clockwise or counter clockwise).
\param corners : The corners of the polyon.
\param cam : The camera parameters used to convert the coordinates from meter
to pixel.
*/
void
vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
{
std::vector<vpImagePoint> ipCorners(corners.size());
for(unsigned int i=0; i<corners.size(); ++i){
vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
}
buildFrom(ipCorners);
}
/*!
Initialize a velocity twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
\param t : Translation vector.
\param thetau : \f$\theta {\bf u}\f$ rotation vector.
*/
vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
const vpThetaUVector &thetau)
{
vpRotationMatrix R ;
R.buildFrom(thetau) ;
buildFrom(t,R) ;
return (*this) ;
}
/*!
Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
Euler angles vector.
\param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
input.
\return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
vpRzyzVector
vpRzyzVector::buildFrom(const vpThetaUVector& tu)
{
vpRotationMatrix R ;
R.buildFrom(tu) ;
buildFrom(R) ;
return *this ;
}
/*!
Constructor that build a 3D \f$ \theta u \f$ visual feature from an
homogeneous matrix \f$ M \f$ that represent the displacement that
the camera has to achieve.
\param M [in] : Homogeneous transformation that describe the
movement that the camera has to achieve. Only the rotational part of
this homogeneous transformation is taken into
consideration. Depending on r the rotation represent either the
rotation that the camera has to achieve to move from the desired
camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
that the camera has to achieve to move from the current camera frame
to the desired one (\f$ ^{c}R_{c^*}\f$).
\param r [in] : The rotation representation of M.
*/
vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
vpFeatureThetaURotationRepresentationType r)
: rotation(r)
{
init() ;
vpRotationMatrix R ;
M.extract(R) ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
const double ty,
const double tz,
const double tux,
const double tuy,
const double tuz) : vpMatrix()
{
init() ;
buildFrom(tx, ty, tz,tux, tuy, tuz) ;
}
/*!
Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$
from a translation vector \f$ \bf t \f$ and a rotation matrix \f$
\bf R \f$.
\param t : Translation vector \f$ \bf t \f$.
\param R : Rotation matrix \f$ \bf R \f$ from which \f$\Theta \bf
u\f$ vector is extracted to initialise the pose vector.
\return The build pose vector.
*/
vpPoseVector
vpPoseVector::buildFrom(const vpTranslationVector& t,
const vpRotationMatrix& R)
{
vpThetaUVector tu ;
tu.buildFrom(R) ;
buildFrom(t,tu) ;
return *this ;
}
/*!
Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
{
vpRotationMatrix R;
M.extract(R);
buildFrom(R);
return *this ;
}
/*!
Initialize a velocity twist transformation matrix from an homogeneous matrix
\f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
\\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
\param M : Homogeneous matrix \f$M\f$ used to initialize the twist
transformation matrix.
*/
vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
vpTranslationVector tv ;
vpRotationMatrix R ;
M.extract(R) ;
M.extract(tv) ;
buildFrom(tv, R) ;
return (*this) ;
}
/*!
Initialize a force/torque twist transformation matrix from an homogeneous matrix
\f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
\\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
\param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
transformation matrix.
*/
vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
vpTranslationVector t ;
vpRotationMatrix R ;
M.extract(R) ;
M.extract(t) ;
buildFrom(t, R) ;
return (*this) ;
}
vpHomography::vpHomography(const vpHomogeneousMatrix &aMb,
const vpPlane &_bP) : vpMatrix()
{
init() ;
buildFrom(aMb,_bP) ;
}
/*!
Initialize a force/torque twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
\param tx,ty,tz : Translation vector in meters.
\param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
const double ty,
const double tz,
const double tux,
const double tuy,
const double tuz) : vpMatrix()
{
init() ;
vpTranslationVector T(tx,ty,tz) ;
vpThetaUVector tu(tux,tuy,tuz) ;
buildFrom(T,tu) ;
}
/*!
Convert a rotation matrix into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
angles vector.
\param R : Rotation matrix used as input.
\return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
vpRxyzVector
vpRxyzVector::buildFrom(const vpRotationMatrix& R)
{
double COEF_MIN_ROT = 1e-6;
double phi ;
if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
else phi = atan2(-R[1][2], R[2][2]) ;
double si = sin(phi) ;
double co = cos(phi) ;
double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);
buildFrom(phi, theta, psi);
return *this ;
}
/*!
Convert a rotation matrix into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
angles vector.
\param R : Rotation matrix used as input.
\return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
vpRzyzVector
vpRzyzVector::buildFrom(const vpRotationMatrix& R)
{
double phi ;
if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
phi = 0 ;
else
phi = atan2(R[1][2],R[0][2]) ;
double cphi = cos(phi) ;
double sphi = sin(phi) ;
double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);
double psi = atan2(-sphi*R[0][0]+cphi*R[1][0],-sphi*R[0][1]+cphi*R[1][1]) ;
buildFrom(phi, theta, psi);
return *this ;
}
/*!
Initialise the polygon by (left-)clicking to add a corners to the polygon.
A right click is used to stop the addition of new corner.
\param I : The image where to click to initialise the corners.
*/
void
vpPolygon::initClick(const vpImage<unsigned char>& I)
{
vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
vpImagePoint ip;
std::vector<vpImagePoint> cornersClick;
while(button == vpMouseButton::button1){
vpDisplay::getClick(I, ip, button, true);
if(button == vpMouseButton::button1){
vpDisplay::displayCross(I, ip, 5, vpColor::red);
cornersClick.push_back(ip);
vpDisplay::flush(I);
}
}
buildFrom(cornersClick);
}
/*!
Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
angles vector from a \f$\theta {\bf u}\f$ vector.
\param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
input to initialize the Euler angles.
*/
vpRzyzVector::vpRzyzVector(const vpThetaUVector& tu)
: vpRotationVector (3)
{
buildFrom(tu) ;
}
