Robotics StackExchange | Archived questions

Unable to convert pcl data

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!

Asked by arthur0304 on 2018-03-27 02:50:21 UTC

Comments

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

Asked by stevejp on 2018-03-27 06:40:00 UTC

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

Asked by lucasw on 2018-03-27 15:47:57 UTC

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.

Asked by arthur0304 on 2018-03-27 22:40:16 UTC

Answers

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.

Asked by Abdulbaki on 2018-10-23 00:07:28 UTC

Comments