本文整理汇总了C++中tf::Pose::setBasis方法的典型用法代码示例。如果您正苦于以下问题：C++ Pose::setBasis方法的具体用法？C++ Pose::setBasis怎么用？C++ Pose::setBasis使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf::Pose的用法示例。
在下文中一共展示了Pose::setBasis方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: poseKDLToTF
void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
{
t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
k.M.data[3], k.M.data[4], k.M.data[5],
k.M.data[6], k.M.data[7], k.M.data[8]));
}示例2: mirrorPoseOnXPlane
void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
{
mirror.setBasis(orig.getBasis());
tf::Matrix3x3& basis = mirror.getBasis();
basis[0][1] = -basis[0][1];
basis[1][0] = -basis[1][0];
basis[2][1] = -basis[2][1];
mirror.setOrigin(orig.getOrigin());
tf::Vector3& origin = mirror.getOrigin();
origin[1] = -origin[1];
// double r, p, y;
// tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
// mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));
// tf::Vector3 v = orig.getOrigin();
// v.setY(-v.getY());
// mirror.setOrigin(v);
// tfScalar m[16];
// ROS_INFO("------------------");
// mirror.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// orig.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
// ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
}