ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
3

Proper way to transform a point cloud

asked 2016-10-06 06:07:53 -0500

OMC gravatar image

updated 2016-10-06 10:48:27 -0500

Hello,

I am using V-REP to simulate a vehicle with a Velodyne. To simulate the behavior of the real Velodyne, I transform at each simulation pass the captured points to the /odom frame. This is to gather 360º data and correct the delay between samples due to the car velocity (this is what I asked in this question).

Now, other members of my team, need the point cloud to be given relative to the velodyne frame (base_laser), so I just need to transform the point cloud relative to /odom to the desired target frame. My tf tree is /odom->/base_link->/base_laser and is working fine. I use Ubuntu 14.04 and ROS Indigo.

The best solution I found was using the pcl_ros package and I tried doing

sensor_msgs::PointCloud2 buffer_local;
tf::TransformListener listener;

pcl_ros::transformPointCloud("/base_laser",_buffer, buffer_local, listener);
_pubVelodyne.publish(buffer_local);

being _buffer the previous pointcloud relative to /odom.

I am getting the following error:

[ERROR] [1475751606.660104363]: "base_laser" passed to lookupTransform argument target_frame does not exist.

It looks like transformPointCloud calls the lookupTransform function but is unable to find the "base_laser" frame. My tf tree is correct (I've checked it with rosrun tf view_frames) and if instead of transforming it to "base_laser" I transform it to "/odom" (itself), I get the original pointcloud without errors, as expected.

Where can be the error? Is the use of the TransformListener right? My pointcloud uses the timestamp of ROS, so it should be right. Another option would be using the transformPointCloud funtion with the transform directly retrieved from tf by me.

Any ideas?

UPDATE 1:

I was using the TransformListener just after creating it, that's why I got that error. Now I create a listener as a class member and I get another error:

[ERROR] [1475759993.562920093]: Lookup would require extrapolation into the future. Requested time 1475759993.562353881 but the latest data is at time 1475759993.468326569, when looking up transform from frame [odom] to frame [base_laser]

I understand I get this error because transformPointCloud wants to use a later transform, so I should use waitForTransform before calling transformPointCloud, right?

So, if I want to use the latest transform available, I would have to manually retrieve it using

listener.lookupTransform("/odom", "/base_laser", ros::Time(0), transform);

and then use

transformPointCloud(original_PC, transformed_PC, transform_matrix)

(after converting the StampedTransform to a Transform object).

UPDATE 2:

Simply use this pcl_ros::transformPointCloud definition.

UPDATE 3:

The previous only works for PointCloud data. For PointCloud2 the available definitions of pcl_ros::transformPointCloud are these.

In order to get the latest available transform, it must be done as suggested in my first Update of this question.

Regards

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2016-10-06 08:07:41 -0500

Thomas D gravatar image

updated 2016-10-06 09:32:17 -0500

The TransformListener needs time to store some transforms so that interpolation can be done. I usually try to create transform listeners as member variables of my class so that they are persistent during runtime. If you are declaring listener as a local variable in a function and then immediately trying to use it then you will get those types of errors.

Edit: If you want to use the latest transform I suggest using the pcl_ros::transformPointCloud that accepts a target_frame and a tf::TransformListener as opposed to manually converting transforms yourself.

edit flag offensive delete link more

Comments

Thank you, you are right. I have edited my original question, I can't mark it as solved yet because I still get an error.

OMC gravatar image OMC  ( 2016-10-06 09:05:36 -0500 )edit

For the updated question you can take a look at tf and Time tutorial. Have you tried adding waitForTransform? The tutorial addresses the specific error you see in your update. Did you get any new errors?

Thomas D gravatar image Thomas D  ( 2016-10-06 09:13:34 -0500 )edit

Yes, I tried it and everything works fine with an appropriate delay. But in my case, I think it would be more accurate to work with the latest available transform. Is the only way of doing that by manually retrieving it, as I suggest in my updated question?

OMC gravatar image OMC  ( 2016-10-06 09:23:04 -0500 )edit

Using the pcl_ros::transformPointCloud is what I was originally doing, and according to the errors I got, It doesn't use the latest transform. I have to wait for a new one, otherwise, I'll get the error "Lookup would require extrapolation at time [...]".

OMC gravatar image OMC  ( 2016-10-06 09:37:16 -0500 )edit

If you really want to use the latest transform then you will have to do some conversions. However, the function I linked to uses the timestamp of the original point cloud get the transform. To me that seems like a better time to use than the most recent time else you might get rotation errors.

Thomas D gravatar image Thomas D  ( 2016-10-06 09:53:44 -0500 )edit

Note that there is also a pcl_ros::transformPointCloud that accepts a target_frame, a tf::listener, and a target_time that might work better for what you are trying to do.

Thomas D gravatar image Thomas D  ( 2016-10-06 09:55:04 -0500 )edit

I have just realised of that definition of the function (my bad!!!). That will very likely do what I intend to. Thank you for your time.

OMC gravatar image OMC  ( 2016-10-06 10:11:11 -0500 )edit

I'm glad it's working for you.

Thomas D gravatar image Thomas D  ( 2016-10-06 10:23:08 -0500 )edit

I spoke too soon... that definition is for PointCloud data types, not for PointCloud2. Only 3 definitions of pcl_ros::transformPointCloud take PC2 as arguments: link. None uses ros::Time as input.

OMC gravatar image OMC  ( 2016-10-06 10:45:57 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2016-10-06 06:07:53 -0500

Seen: 16,569 times

Last updated: Oct 06 '16