本文整理汇总了C++中yarp::sig::Matrix::submatrix方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix::submatrix方法的具体用法？C++ Matrix::submatrix怎么用？C++ Matrix::submatrix使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类yarp::sig::Matrix的用法示例。
在下文中一共展示了Matrix::submatrix方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: fprintf
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;
}示例2: idynMatrix2kdlFrame
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;
}