Dear Wim, dear Stu,
I finally started to get something working using KDL and its rotation
objects. Thanks!
Still, there are a few cases when the value of yaw pitch and roll cannot be
guessed.
Anyway, all I wanted to do was to get the Z rotation value of one observed
frame. If feels strange to have gone all the way with using KDL for that.
Here is the code (not optimized) I came up with. Do you see a more efficient
way to do this?
//compute roll pitch yaw using KDL
tf::Vector3 axis=quat.getAxis();
float anglekd = quat.getAngle();
KDL::Vector vectkd = KDL::Vector(axis.x(), axis.y(), axis.z());
KDL::Rotation rot = KDL::Rotation();
rot = rot.Rot(vectkd, anglekd);
double rx, ry, rz;
rot.GetEulerZYX(rz, ry, rx);
//broadcast the Kinect frame
tf::StampedTransform transform2;
transform2.setRotation(tf::Quaternion(-rz , 0, -halfPI));
transform2.setOrigin( tf::Vector3(1.0, 0.0 , 0.0 ));
br.sendTransform(tf::StampedTransform(transform2, ros::Time::now(),
"world", "kinect_rgb"));
Thanks for your inputs and help!
Raph
--
View this message in context:
http://ros-users.122217.n3.nabble.com/tf-and-quaternions-tp2206128p2234112.html
Sent from the ROS-Users mailing list archive at Nabble.com.