Ask Your Question
1

Unable to convert pcl data

asked 2018-03-27 02:50:21 -0500

arthur0304 gravatar image

updated 2018-03-28 12:35:19 -0500

I am trying to convert pcl data type to ROS message type using command pcl::toROSMsg and publish two topics. However, I cannot receive any tf info and the error shows here:

terminate called after throwing an instance of 'tf2::LookupException'
what():  "map" passed to lookupTransform argument target_frame does not exist.

Below is my code:

#include <iostream>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/registration/icp.h>
#include <Eigen/Dense>

int main(int argc, char **argv)
{
    //Initialize ROS
    ros::init(argc, argv, "pcl");
    ros::NodeHandle nh;

    ros::Publisher map_pub;
    ros::Publisher scene_pub;
    tf::TransformListener listener;
    tf::StampedTransform transform;
    tf::Quaternion q;
    static tf::TransformBroadcaster br;

    //Create a ROS publisher for the output point cloud
    map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100);
    scene_pub = nh.advertise<sensor_msgs::PointCloud2>("/scene", 100);

    //Load the file
    const pcl::PointCloud<pcl::PointXYZ>::Ptr map (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("/home/parallels/Downloads/target.pcd", *map);
    const pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("/home/parallels/Downloads/scene.pcd", *scene);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    //Set the input source and target
    icp.setInputSource(scene);
    icp.setInputTarget(map);

    //Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    icp.setMaxCorrespondenceDistance(0.05);

    //Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations(5000);

    //Set the transformation epsilon (criterion 2)
    icp.setTransformationEpsilon(1e-8);

    //Set the euclidean distance difference epsilon (criterion 3)
    icp.setEuclideanFitnessEpsilon(1);

    //Perform the alignment
    pcl::PointCloud<pcl::PointXYZ> scene_cloud_registered;
    icp.align(scene_cloud_registered);

    //Obtain the transformation that aligned scene_cloud to scene_cloud_registered
    Eigen::Matrix4f transformation = icp.getFinalTransformation();
    std::cout << "Transformation matrix : " << std::endl << transformation;

********************Below is where I believe something goes wrong**********************

    //Convert pcd data type to ROS message type
    sensor_msgs::PointCloud2 map_cloud;
    sensor_msgs::PointCloud2 scene_cloud;
    pcl::toROSMsg(*map, map_cloud);
    pcl::toROSMsg(*scene, scene_cloud);

    //Publish the point cloud message and tf
    map_pub.publish(map_cloud);
    scene_pub.publish(scene_cloud);

    //Calculate tf between /map and /scene
    listener.lookupTransform("/map", "/scene", ros::Time(0), transform);
    br.sendTransform(transform);

    //Spin
    ros::spin();
}

I assume I did something wrong in the step of converting pcl data to ROS message type since I can obtain transformation matrix. I have searched through the Internet but still could not fix this problem. Can someone give me some hint? Thank you guys so much!

edit retag flag offensive close merge delete

Comments

1

Can you post a picture of your TF tree so we know these frames actually exist?

stevejp gravatar imagestevejp ( 2018-03-27 06:40:00 -0500 )edit

Or cut and paste the output of rosrun tf tf_echo map scene for mostly the same information in text form.

lucasw gravatar imagelucasw ( 2018-03-27 15:47:57 -0500 )edit

Hi @stevejp and @lucasw, thank you for your response and sorry for my late response. Yes I did not create those frames since I thought that topic name was also the frame name.

arthur0304 gravatar imagearthur0304 ( 2018-03-27 22:40:16 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-10-23 00:07:28 -0500

Abdulbaki gravatar image

updated 2018-10-23 00:09:42 -0500

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-03-27 02:50:21 -0500

Seen: 281 times

Last updated: Oct 23 '18