void convertVispHMatToTF(const vpHomogeneousMatrix &HMatrix,
tf::Transform &TFtransform)
{
vpTranslationVector Trans;
HMatrix.extract(Trans);
vpThetaUVector ThetaU;
HMatrix.extract(ThetaU);
double vpAngle;
vpColVector vpAxis;
ThetaU.extract(vpAngle, vpAxis);
btVector3 btAxis;
btScalar btAngle = vpAngle;
if(fabs(vpAngle) < 1.0e-15) // the case of no rotation, prevents nan on btQuaternion
{
btAngle = 0.0;
btAxis.setValue(1.0, 0.0, 0.0);
}
else
{
btAxis.setValue(vpAxis[0], vpAxis[1], vpAxis[2]);
}
btQuaternion Quat(btAxis, btAngle);
TFtransform.setOrigin( tf::Vector3(Trans[0], Trans[1], Trans[2] ) );
TFtransform.setRotation(Quat);
}
void convertVispHMatToGMPose(const vpHomogeneousMatrix &HMatrix,
geometry_msgs::Pose &pose)
{
vpTranslationVector Trans;
HMatrix.extract(Trans);
vpThetaUVector ThetaU;
HMatrix.extract(ThetaU);
double vpAngle;
vpColVector vpAxis;
ThetaU.extract(vpAngle, vpAxis);
btVector3 btAxis;
btScalar btAngle = vpAngle;
if(fabs(vpAngle) < 1.0e-15) // the case of no rotation, prevents nan on btQuaternion
{
btAngle = 0.0;
btAxis.setValue(1.0, 0.0, 0.0);
}
else
{ btAxis.setValue(vpAxis[0], vpAxis[1], vpAxis[2]); }
btQuaternion Quat(btAxis, btAngle);
pose.position.x = Trans[0];
pose.position.y = Trans[1];
pose.position.z = Trans[2];
pose.orientation.w = Quat.w();
pose.orientation.x = Quat.x();
pose.orientation.y = Quat.y();
pose.orientation.z = Quat.z();
}
void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &corners,
const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
vpPose pose;
double x=0, y=0;
for (unsigned int i=0; i < point.size(); i ++) {
vpPixelMeterConversion::convertPoint(cam, corners[i], x, y);
point[i].set_x(x);
point[i].set_y(y);
pose.addPoint(point[i]);
}
if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
else pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
vpTranslationVector t;
cMo.extract(t);
vpRotationMatrix R;
cMo.extract(R);
vpThetaUVector tu(R);
std::cout << "cMo: ";
for (unsigned int i=0; i < 3; i++)
std::cout << t[i] << " ";
for (unsigned int i=0; i < 3; i++)
std::cout << vpMath::deg(tu[i]) << " ";
std::cout << std::endl;
}
/*!
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 tv ; M.extract(tv) ;
buildFrom(tv,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) ;
}
/*!
Set the position and the orientation of a SceneNode.
\param name : Name of the SceneNode to rotate.
\param wMo : The homogeneous matrix representing the rotation and
translation to apply.
*/
void vpAROgre::setPosition(const std::string &name,
const vpHomogeneousMatrix &wMo)
{
// Extract the position and orientation data
vpRotationMatrix rotations;
vpTranslationVector translation;
wMo.extract(rotations);
wMo.extract(translation);
// Apply them to the node
setPosition(name, translation);
setRotation(name, rotations);
}
void printPose(const std::string &text, const vpHomogeneousMatrix &cMo)
{
vpTranslationVector t;
cMo.extract(t);
vpRotationMatrix R;
cMo.extract(R);
vpThetaUVector tu(R);
std::cout << text;
for (unsigned int i=0; i < 3; i++)
std::cout << t[i] << " ";
for (unsigned int i=0; i < 3; i++)
std::cout << vpMath::deg(tu[i]) << " ";
std::cout << std::endl;
}
void
vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
const vpHomogeneousMatrix &cextMo,
const vpHomogeneousMatrix &cMo,
const vpCameraParameters &cam)
{
vpHomogeneousMatrix c1Mc ;
c1Mc = cextMo*cMo.inverse() ;
o.track(c1Mc) ;
x.track(c1Mc) ;
y.track(c1Mc) ;
z.track(c1Mc) ;
vpImagePoint ipo;
vpImagePoint ipx;
vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::green) ;
vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue) ;
vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::red) ;
}
int HRP2ComputeControlLawProcess::loadcMh(vpHomogeneousMatrix& cMh)
{
//TODO : this file name shouldn't be written here
ifstream file;
string name="./data/ViSP/hrp2CamParam/cMh.3";
file.open (name.c_str());
try
{
cMh.load(file);
}
catch(vpException a) // file doesn't exist
{
cout << "---- Open Exception Default---------------"<<endl;
cout<<"----Wide camera extrinsic parameters ---- " <<endl;
vpCTRACE << endl <<a;
cout << "------------------ " <<endl;
// fill a defaut matrix
cMh[0][0]=0.016937505; cMh[0][1]=-0.9997821632; cMh[0][2]= -0.01217327283; cMh[0][3]=-0.05195444004;
cMh[1][0]=-0.1698427574; cMh[1][1]=0.009121740254; cMh[1][2]= -0.9854286431; cMh[1][3]= 0.1036017168 ;
cMh[2][0]=0.9853256992 ; cMh[2][1]=0.0187590039; cMh[2][2]= -0.1696511946; cMh[2][3]=-0.04729590458 ;
cMh[3][0]=0; cMh[3][1]=0;cMh[3][2]= 0; cMh[3][3]=1 ;
return -1;
}
file.close();
return 0;
}
void
vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
const vpHomogeneousMatrix &cextMo,
const vpHomogeneousMatrix &cMo,
const vpCameraParameters &cam,
const unsigned int thickness)
{
vpHomogeneousMatrix c1Mc ;
c1Mc = cextMo*cMo.inverse() ;
o.track(c1Mc) ;
if(o.get_Z() < 0) // do not print camera if behind the external camera
return;
x.track(c1Mc) ;
y.track(c1Mc) ;
z.track(c1Mc) ;
vpImagePoint ipo;
vpImagePoint ipx;
vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;
vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;
vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
}
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
their corresponding 2D coordinates \e dot. The pose is computed using a Lowe
non linear method.
\param point : 3D coordinates of the points.
\param dot : 2D coordinates of the points.
\param ndot : Number of points or dots used for the pose estimation.
\param cam : Intrinsic camera parameters.
\param cMo : Homogeneous matrix in output describing the transformation
between the camera and object frame.
\param cto : Translation in ouput extracted from \e cMo.
\param cro : Rotation in ouput extracted from \e cMo.
\param init : Indicates if the we have to estimate an initial pose with
Lagrange or Dementhon methods.
*/
void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
vpCameraParameters cam,
vpHomogeneousMatrix &cMo,
vpTranslationVector &cto,
vpRxyzVector &cro, bool init)
{
vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpRotationMatrix cRo;
vpPose pose;
vpImagePoint cog;
for (int i=0; i < ndot; i ++) {
double x=0, y=0;
cog = dot[i].getCog();
vpPixelMeterConversion::convertPoint(cam,
cog,
x, y) ; //pixel to meter conversion
point[i].set_x(x) ;//projection perspective p
point[i].set_y(y) ;
pose.addPoint(point[i]) ;
}
if (init == true) {
pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
// Compute and return the residual expressed in meter for the pose matrix
// 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
}
else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
pose.computePose(vpPose::LOWE, cMo) ;
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
}
/*!
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) ;
}
void readConfigVar_PoseRxyz(const string &s, vpHomogeneousMatrix &M)
{
vpColVector v(6);
vpIoTools::readConfigVar(s, v);
vpTranslationVector t(v[0], v[1], v[2]);
vpRxyzVector r(v[3], v[4], v[5]);
vpRotationMatrix R(r);
M.buildFrom(t, R);
}
vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
vpMomentObject &src, vpMomentObject &dst)
{
vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired
vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
//A,B,C parameters of source and destination plane
double A; double B; double C;
double Ad; double Bd; double Cd;
//init main object: using moments up to order 6
//Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
pl.setABCD(0,0,1.0,0);
pl.changeFrame(cMo);
planeToABC(pl,A,B,C);
pl.setABCD(0,0,1.0,0);
pl.changeFrame(cdMo);
planeToABC(pl,Ad,Bd,Cd);
//extracting initial position (actually we only care about Zdst)
vpTranslationVector vec;
cdMo.extract(vec);
///////////////////////////// initializing moments and features /////////////////////////////////
//don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
vpMomentCommon moments (vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
//same thing with common features
vpFeatureMomentCommon featureMoments(moments);
vpFeatureMomentCommon featureMomentsDes(momentsDes);
moments.updateAll(src);
momentsDes.updateAll(dst);
featureMoments.updateAll(A,B,C);
featureMomentsDes.updateAll(Ad,Bd,Cd);
//setup the interaction type
task.setInteractionMatrixType(interaction_type) ;
//////////////////////////////////add useful features to task//////////////////////////////
task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
//the moments are different in case of a symmetric object
task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
task.setLambda(0.4) ;
task.computeControlLaw();
vpMatrix mat = task.computeInteractionMatrix();
task.kill();
return mat;
}
/*!
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 ;
}
/*!
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) ;
}
/*!
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) ;
}
/*!
compute the homography using a displacement matrix.
the homography is given by:
\f$ {}^cH_{c_0} = {}^cR_{c_0} + \frac{{}^cT_{c_0} . {}^tN}{d_0} \f$
Several internal variables are computed (dt, cRc0_0n)
\param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
\param _cHc0 : the homography of the plane
*/
void
vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
{
vpRotationMatrix cRc0;
vpTranslationVector ctransc0;
_cTc0.extract(cRc0);
_cTc0.extract(ctransc0);
vpMatrix cHc0 = _cHc0.convert();
// vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
cHc0 /= cHc0[2][2];
H = cHc0;
// vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
// vpQuaternionVector RotQuat(cRc0);
// vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
// vpQuaternionVector partial = RotQuat * NQuat;
// vpQuaternionVector resQuat = (partial * RotQuatConj);
//
// cRc0_0n = vpColVector(3);
// cRc0_0n[0] = resQuat.x();
// cRc0_0n[1] = resQuat.y();
// cRc0_0n[2] = resQuat.z();
cRc0_0n = cRc0*N;
// vpPlane p(corners[0], corners[1], corners[2]);
// vpColVector Ncur = p.getNormal();
// Ncur.normalize();
N_cur = cRc0_0n;
dt = 0.0;
for (unsigned int i = 0; i < 3; i += 1){
dt += ctransc0[i] * (N_cur[i]);
}
}
vpMatrix kdlJacobianToVispJacobian(const KDL::Jacobian& kdlJacobian,
const vpHomogeneousMatrix& bMe)
{
vpMatrix vpJacobian(kdlJacobian.rows(), kdlJacobian.columns());
KDL::Jacobian kdlJacobianCopy = kdlJacobian;
// Fix reference point so that it is in the base frame
vpTranslationVector bOe; //origin of end frame in base frame
bMe.extract(bOe);
kdlJacobianCopy.changeRefPoint(KDL::Vector(-bOe[0], -bOe[1], -bOe[2]));
pal::convertKDLJacToVispMat(kdlJacobianCopy, vpJacobian);// KDL data --> Visp data
return vpJacobian;
}
void convertGMPoseToVispHMat(const geometry_msgs::Pose &pose,
vpHomogeneousMatrix &HMatrix)
{
tf::Quaternion q;
tf::quaternionMsgToTF(pose.orientation, q);
double angle = q.getAngle();
HMatrix.buildFrom(pose.position.x,
pose.position.y,
pose.position.z,
angle*q.getAxis().x(),
angle*q.getAxis().y(),
angle*q.getAxis().z()
);
}
void convertTFtoVispHMat(const tf::StampedTransform &TFtransform,
vpHomogeneousMatrix &HMatrix)
{
double Angle; // Theta U angle of the transform
Angle = TFtransform.getRotation().getAngle();
HMatrix.buildFrom(TFtransform.getOrigin().x(),
TFtransform.getOrigin().y(),
TFtransform.getOrigin().z(),
Angle*TFtransform.getRotation().getAxis().x(),
Angle*TFtransform.getRotation().getAxis().y(),
Angle*TFtransform.getRotation().getAxis().z()
);
}
/*!
Allow homogeneous matrix multiplication.
\code
#include <visp/vpHomogeneousMatrix.h>
int main()
{
vpHomogeneousMatrix aMb, bMc;
// Initialize aMb and bMc...
// Compute aMc * bMc
vpHomogeneousMatrix aMc = aMb * bMc;
}
\endcode
*/
vpHomogeneousMatrix
vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
{
vpHomogeneousMatrix p,p1 ;
vpRotationMatrix R1, R2, R ;
vpTranslationVector T1, T2 , T;
extract(T1) ;
M.extract(T2) ;
extract (R1) ;
M.extract (R2) ;
R = R1*R2 ;
T = R1*T2 + T1 ;
p.insert(T) ;
p.insert(R) ;
return p;
}
geometry_msgs::Transform toGeometryMsgsTransform(const vpHomogeneousMatrix& mat){
geometry_msgs::Transform trans;
vpQuaternionVector q;
mat.extract(q);
trans.rotation.x = q.x();
trans.rotation.y = q.y();
trans.rotation.z = q.z();
trans.rotation.w = q.w();
trans.translation.x = mat[0][3];
trans.translation.y = mat[1][3];
trans.translation.z = mat[2][3];
return trans;
}
/*!
\brief Compute the pose using the Lowe non linear approach
it consider the minimization of a residual using
the levenberg marquartd approach.
The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
*/
void
vpPose::poseLowe(vpHomogeneousMatrix & cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
#endif
int n, m; /* nombre d'elements dans la matrice jac */
int lwa; /* taille du vecteur wa */
int ldfjac; /* taille maximum d'une ligne de jac */
int info, ipvt[NBR_PAR];
int tst_lmder;
double f[2 * NBPTMAX], sol[NBR_PAR];
double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
// double u[3]; /* vecteur de rotation */
// double rd[3][3]; /* matrice de rotation */
n = NBR_PAR; /* nombres d'inconnues */
m = (int)(2 * npt); /* nombres d'equations */
lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
// c = cam ;
// for (i=0;i<3;i++)
// for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
// mat_rot(rd,u);
vpRotationMatrix cRo ;
cMo.extract(cRo) ;
vpThetaUVector u(cRo) ;
for (unsigned int i=0;i<3;i++)
{
sol[i] = cMo[i][3];
sol[i+3] = u[i];
}
vpPoint P ;
unsigned int i_=0;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
XI[i_] = P.get_x();//*cam.px + cam.xc ;
YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
XO[i_] = P.get_oX();
YO[i_] = P.get_oY();
ZO[i_] = P.get_oZ();
++i_;
}
tst_lmder = lmder1 (&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
if (tst_lmder == -1)
{
std::cout << " in CCalculPose::PoseLowe(...) : " ;
std::cout << "pb de minimisation, returns FATAL_ERROR";
// return FATAL_ERROR ;
}
for (unsigned int i = 0; i < 3; i++)
u[i] = sol[i + 3];
for (unsigned int i=0;i<3;i++)
{
cMo[i][3] = sol[i];
u[i] = sol[i+3];
}
vpRotationMatrix rd(u) ;
cMo.insert(rd) ;
// rot_mat(u,rd);
// for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
#endif
// return OK ;
}
double
vpHomography::computeDisplacement(unsigned int nbpoint,
vpPoint *c1P,
vpPoint *c2P,
vpPlane *oN,
vpHomogeneousMatrix &c2Mc1,
vpHomogeneousMatrix &c1Mo,
int userobust
)
{
vpColVector e(2) ;
double r_1 = -1 ;
vpColVector p2(3) ;
vpColVector p1(3) ;
vpColVector Hp2(3) ;
vpColVector Hp1(3) ;
vpMatrix H2(2,6) ;
vpColVector e2(2) ;
vpMatrix H1(2,6) ;
vpColVector e1(2) ;
int only_1 = 1 ;
int only_2 = 0 ;
int iter = 0 ;
unsigned int i ;
unsigned int n=0 ;
n = nbpoint ;
if ((only_1==1) || (only_2==1)) ; else n *=2 ;
vpRobust robust(n);
vpColVector res(n) ;
vpColVector w(n) ;
w =1 ;
robust.setThreshold(0.0000) ;
vpMatrix W(2*n,2*n) ;
W = 0 ;
vpColVector N1(3), N2(3) ;
double d1, d2 ;
double r =1e10 ;
iter =0 ;
while (vpMath::equal(r_1,r,threshold_displacement) == false )
{
r_1 =r ;
// compute current position
//Change frame (current)
vpHomogeneousMatrix c1Mc2, c2Mo ;
vpRotationMatrix c1Rc2, c2Rc1 ;
vpTranslationVector c1Tc2, c2Tc1 ;
c1Mc2 = c2Mc1.inverse() ;
c2Mc1.extract(c2Rc1) ;
c2Mc1.extract(c2Tc1) ;
c2Mc1.extract(c1Rc2) ;
c1Mc2.extract(c1Tc2) ;
c2Mo = c2Mc1*c1Mo ;
vpMatrix L(2,3), Lp ;
int k =0 ;
for (i=0 ; i < nbpoint ; i++)
{
getPlaneInfo(oN[i], c1Mo, N1, d1) ;
getPlaneInfo(oN[i], c2Mo, N2, d2) ;
p2[0] = c2P[i].get_x() ;
p2[1] = c2P[i].get_y() ;
p2[2] = 1.0 ;
p1[0] = c1P[i].get_x() ;
p1[1] = c1P[i].get_y() ;
p1[2] = 1.0 ;
vpMatrix H(3,3) ;
Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
Hp2 /= Hp2[2] ; // normalisation
Hp1 /= Hp1[2] ;
// set up the interaction matrix
double x = Hp2[0] ;
double y = Hp2[1] ;
double Z1 ;
Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
H2 *=-1 ;
vpMatrix c1CFc2(6,6) ;
{
vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
for (unsigned int k=0 ; k < 3 ; k++)
for (unsigned int l=0 ; l<3 ; l++)
{
c1CFc2[k][l] = c1Rc2[k][l] ;
c1CFc2[k+3][l+3] = c1Rc2[k][l] ;
c1CFc2[k][l+3] = sTR[k][l] ;
}
}
H2 = H2*c1CFc2 ;
// Set up the error vector
e2[0] = Hp2[0] - c1P[i].get_x() ;
e2[1] = Hp2[1] - c1P[i].get_y() ;
x = Hp1[0] ;
y = Hp1[1] ;
Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
// Set up the error vector
e1[0] = Hp1[0] - c2P[i].get_x() ;
e1[1] = Hp1[1] - c2P[i].get_y() ;
if (only_2==1)
{
if (k == 0) { L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
}
else
if (only_1==1)
{
if (k == 0) { L = H1 ; e= e1 ; }
else
{
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
}
else
{
if (k == 0) {L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
k++ ;
}
if (userobust)
{
robust.setIteration(0);
for (unsigned int k=0 ; k < n ; k++)
{
res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
}
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
for (unsigned int k=0 ; k < n ; k++)
{
W[2*k][2*k] = w[k] ;
W[2*k+1][2*k+1] = w[k] ;
}
}
else
{
for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
}
(W*L).pseudoInverse(Lp, 1e-16) ;
// Compute the camera velocity
vpColVector c2Tcc1 ;
c2Tcc1 = -1*Lp*W*e ;
// only for simulation
c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
// UpdatePose2(c2Tcc1, c2Mc1) ;
r =(W*e).sumSquare() ;
if (r < 1e-15) {break ; }
if (iter>1000){break ; }
if (r>r_1) { break ; }
iter++ ;
}
return (W*e).sumSquare() ;
}
/*!
Compute an instantaneous velocity from an homogeneous matrix. The inverse
function is the exponential map, see direct().
\param M : An homogeneous matrix corresponding to a displacement.
\param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
displacement is applied.
\return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
vector: \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
\sa direct(const vpColVector &, const float &)
*/
vpColVector
vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t)
{
vpColVector v(6);
unsigned int i;
double theta,si,co,sinc,mcosc,msinc,det;
vpThetaUVector u ;
vpRotationMatrix Rd,a;
vpTranslationVector dt ;
M.extract(Rd);
u.buildFrom(Rd);
for (i=0;i<3;i++) v[3+i] = u[i];
theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
si = sin(theta);
co = cos(theta);
sinc = vpMath::sinc(si,theta);
mcosc = vpMath::mcosc(co,theta);
msinc = vpMath::msinc(si,theta);
// a below is not a pure rotation matrix, even if not so far from
// the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
// with V = t.U
a[0][0] = sinc + u[0]*u[0]*msinc;
a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
a[1][1] = sinc + u[1]*u[1]*msinc;
a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
a[2][2] = sinc + u[2]*u[2]*msinc;
det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
+ a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
- a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
if (fabs(det) > 1.e-5)
{
v[0] = (M[0][3]*a[1][1]*a[2][2]
+ M[1][3]*a[2][1]*a[0][2]
+ M[2][3]*a[0][1]*a[1][2]
- M[2][3]*a[1][1]*a[0][2]
- M[1][3]*a[0][1]*a[2][2]
- M[0][3]*a[2][1]*a[1][2])/det;
v[1] = (a[0][0]*M[1][3]*a[2][2]
+ a[1][0]*M[2][3]*a[0][2]
+ M[0][3]*a[1][2]*a[2][0]
- a[2][0]*M[1][3]*a[0][2]
- a[1][0]*M[0][3]*a[2][2]
- a[0][0]*M[2][3]*a[1][2])/det;
v[2] = (a[0][0]*a[1][1]*M[2][3]
+ a[1][0]*a[2][1]*M[0][3]
+ a[0][1]*M[1][3]*a[2][0]
- a[2][0]*a[1][1]*M[0][3]
- a[1][0]*a[0][1]*M[2][3]
- a[0][0]*a[2][1]*M[1][3])/det;
}
else
{
v[0] = M[0][3];
v[1] = M[1][3];
v[2] = M[2][3];
}
// Apply the sampling time to the computed velocity
v /= delta_t;
return(v);
}
