ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
You have to publish that tf::Transform from transformation matrix you found from ICP. There is no tf between those frames unless you publish it. And also, you need to publish clouds after you publish tf::Transform.
// includes
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
// code
Eigen::Matrix4f transformation = icp.getFinalTransformation();
Eigen::Transform<float, 3, Eigen::Affine> tROTA(transformation);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(tROTA, x, y, z, roll, pitch, yaw);
transform_tf.setOrigin( tf::Vector3(x,y,z) );
tf::Quaternion q;
q.setRPY(roll,pitch,yaw);
transform_tf.setRotation(q);
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform_tf, ros::Time::now(), "map", "scene"));
map_pub.publish(map_cloud);
scene_pub.publish(scene_cloud);
Correct me if I am wrong.
2 | No.2 Revision |
You have to publish that tf::Transform from transformation matrix you found from ICP. There is no tf between those frames unless you publish it. And also, you need to publish clouds after you publish tf::Transform.
// includes
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
// code
Eigen::Matrix4f transformation = icp.getFinalTransformation();
Eigen::Transform<float, 3, Eigen::Affine> tROTA(transformation);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(tROTA, x, y, z, roll, pitch, yaw);
tf::Transform transform_tf;
transform_tf.setOrigin( tf::Vector3(x,y,z) );
tf::Quaternion q;
q.setRPY(roll,pitch,yaw);
transform_tf.setRotation(q);
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform_tf, ros::Time::now(), "map", "scene"));
map_pub.publish(map_cloud);
scene_pub.publish(scene_cloud);
Correct me if I am wrong.