double
AmclNode::getYaw(tf::Pose& t)
{
double yaw, pitch, roll;
t.getBasis().getEulerYPR(yaw,pitch,roll);
return yaw;
}
void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
{
for (unsigned int i = 0; i < 3; ++i)
k.p[i] = t.getOrigin()[i];
for (unsigned int i = 0; i < 9; ++i)
k.M.data[i] = t.getBasis()[i/3][i%3];
}
double
AmclNode::getYaw(tf::Pose& t)
{
double yaw, pitch, roll;
btMatrix3x3 mat = t.getBasis();
mat.getEulerYPR(yaw,pitch,roll);
return yaw;
}
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]);
}
double AMCLocalizer::getYaw(tf::Pose& t) {
double yaw, pitch, roll;
t.getBasis().getEulerYPR(yaw,pitch,roll);
return yaw;
}
