Point Cloud Data - In the map frame?

[nalinraut]

Is the point cloud data with respect to the map frame or is it with respect to the camera coordinate frame?

[Myzhar]

The frame of the pointcloud is the left camera.

[nalinraut]

@Myzhar So we need to transform it to the map frame? No way we can obtain it from the ZED SDK or ZED node?

[Myzhar]

You can use the TF to transform the pointcloud if you need it in map frame. The node cannot do it automatically because it should work even in the case were map frame does not exist

[NeilNie]

@Myzhar I have a very similar question. I would like to transform the point cloud using pcl. I am not very familiar with tf. I receive this error "map" passed to lookupTransform argument target_frame does not exist.

when I run this code. Thanks in advance!

void cloud_callback (const sensor_msgs::PointCloud2ConstPtr& input)
{
    // Create a container for the data.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

    tf::TransformListener transform_listener;
    pcl_ros::transformPointCloud("/map", *temp_cloud, *cloud_transformed, transform_listener);

    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*cloud_transformed,cloud_publish);
    cloud_publish.header = input->header;

    pub.publish(cloud_publish);
}

When I run tf view_frame, it seems like the frames are working. I am not sure what is the proper target frames in this case. frames.pdf

[Myzhar]

Have you tried to remove the / in front of map in the function transformPointCloud?

The target frame is correctly map, it is available only if the zed_nodenis publishing the map -> odom transform

[NeilNie]

@Myzhar Thanks for your reply. I was able to solve this problem by doing the following:

I realized that I shouldn't create the listener in the callback. Instead, I should listen for the transformation in the main loop, and then apply the transformation in the point cloud call back.

#include <ros/ros.h>
#include <ros/console.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_ros/transforms.h>

ros::Publisher pub;
tf::StampedTransform transform;

void cloud_callback (const sensor_msgs::PointCloud2ConstPtr& input)
{
    // Create a container for the data.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

    pcl_ros::transformPointCloud(*temp_cloud, *cloud_transformed, transform);

    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*cloud_transformed,cloud_publish);
    cloud_publish.header = input->header;

    pub.publish(cloud_publish);
}

int main (int argc, char** argv){

    // Initialize ROS
    ROS_INFO("Node started");

    ros::init (argc, argv, "point_cloud_transform");
    ros::NodeHandle nh;

    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe ("/zed/point_cloud/cloud_registered", 1, cloud_callback);

    // Create a ROS publisher for the output point cloud
    pub = nh.advertise<sensor_msgs::PointCloud2> ("/point_cloud/cloud_transformed", 1);

    tf::TransformListener listener;
    ros::Rate rate(30.0);
    while (nh.ok()){

        try{
            listener.lookupTransform("map", "zed_camera_center", ros::Time(), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
        }

        ros::spinOnce();
        rate.sleep();
    }

    ros::spin();
    return 0;

}

[Myzhar]

Yes, that's correct.

It is even better if you use a tf2::Buffer like we do in the ZED wrapper:

https://github.com/stereolabs/zed-ros-wrapper/blob/master/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp#L342

https://github.com/stereolabs/zed-ros-wrapper/blob/master/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp#L267

[nalinraut]

@Myzhar @NeilNie Is there specific documentation for tf ? I desperately need to transform point-cloud from camera frame to map frame.

[Myzhar]

@nalinraur the code that @NeilNie has shared does exactly what you are searching for.

You can adapt it to your needings