yarp::sig::Matrix localSE3inv(const yarp::sig::Matrix &H, unsigned int verbose)
{
if ((H.rows()<4) || (H.cols()<4))
{
if (verbose)
fprintf(stderr,"localSE3inv() failed\n");
return yarp::sig::Matrix(0,0);
}
yarp::sig::Matrix invH(4,4);
yarp::sig::Vector p(3);
yarp::sig::Matrix Rt=H.submatrix(0,2,0,2).transposed();
p[0]=H(0,3);
p[1]=H(1,3);
p[2]=H(2,3);
p=Rt*p;
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
invH(i,j)=Rt(i,j);
invH(0,3)=-p[0];
invH(1,3)=-p[1];
invH(2,3)=-p[2];
invH(3,3)=1.0;
return invH;
}
bool idynMatrix2kdlFrame(const yarp::sig::Matrix & idynMatrix, KDL::Frame & kdlFrame)
{
if( idynMatrix.cols() != 4 || idynMatrix.rows() != 4 ) return false;
KDL::Rotation kdlRotation;
KDL::Vector kdlVector;
idynMatrix2kdlRotation(idynMatrix.submatrix(0,2,0,2),kdlRotation);
idynVector2kdlVector(idynMatrix.subcol(0,3,3),kdlVector);
kdlFrame = KDL::Frame(kdlRotation,kdlVector);
return true;
}