本文整理汇总了C++中yarp::sig::Matrix::setSubcol方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix::setSubcol方法的具体用法？C++ Matrix::setSubcol怎么用？C++ Matrix::setSubcol使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类yarp::sig::Matrix的用法示例。
在下文中一共展示了Matrix::setSubcol方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeFoR
//creates a full transform as given by a DCM matrix at the pos and norm w.r.t. the original frame, from the pos and norm (one axis set arbitrarily)
bool AvoidanceHandlerAbstract::computeFoR(const yarp::sig::Vector &pos, const yarp::sig::Vector &norm, yarp::sig::Matrix &FoR)
{
if (norm == zeros(3))
{
FoR=eye(4);
return false;
}
yarp::sig::Vector x(3,0.0), z(3,0.0), y(3,0.0);
z = norm;
if (z[0] == 0.0)
{
z[0] = 0.00000001; // Avoid the division by 0
}
y[0] = -z[2]/z[0]; //y is in normal plane
y[2] = 1; //this setting is arbitrary
x = -1*(cross(z,y));
// Let's make them unitary vectors:
x = x / yarp::math::norm(x);
y = y / yarp::math::norm(y);
z = z / yarp::math::norm(z);
FoR=eye(4);
FoR.setSubcol(x,0,0);
FoR.setSubcol(y,0,1);
FoR.setSubcol(z,0,2);
FoR.setSubcol(pos,0,3);
return true;
}示例2: KDLtoYarp_position
bool KDLtoYarp_position(const KDL::Frame & kdlFrame, yarp::sig::Matrix & yarpMatrix4_4 )
{
yarp::sig::Matrix R(3,3);
yarp::sig::Vector p(3);
KDLtoYarp(kdlFrame.M,R);
KDLtoYarp(kdlFrame.p,p);
if( yarpMatrix4_4.rows() != 4 || yarpMatrix4_4.cols() != 4 ) { yarpMatrix4_4.resize(4,4); }
yarpMatrix4_4.zero();
yarpMatrix4_4.setSubmatrix(R,0,0);
yarpMatrix4_4.setSubcol(p,0,3);
yarpMatrix4_4(3,3) = 1;
return true;
}