Ask Your Question
0

MessageFilter with PointCloud + PoseMessage

asked 2014-07-03 13:10:47 -0500

mark_vision gravatar image

Hi all, I'm trying to register a map using point clouds that are coming from a Stereo Camera at 20 Hz and a pose estimator that fires at 160 Hz. My first code is something like:

tl.waitForTransform("odom", "bumblebee_optical_frame", time, ros::Duration(1.0));
tl.lookupTransform("odom", "bumblebee_optical_frame", ros::Time::now(), st);
Eigen::Affine3d e;
tf::transformTFToEigen(st, e);
pcl::transformPointCloud(*tmp, buffer, e);

where tl is a TransformListener, odom is the fixed world frame, tmp is the cloud that comes from the message (the piece of code is inside a callback function) and buffer is the output cloud that I will use with something like map += buffer; where map is the global map.

Of course is a naive approach and I don't expect a perfect map, but what I see is that it seems like the every new cloud that is coming is registered, but with a delay so the effect is that I see the same object on the ground repeated many many times, while in Rviz I clearly see that the objects change during the trip.

If I remove the line waitForTransform, I see a very sparse map, but synchronized correctly (I can see one cloud every one or two meters).

Why is this happening? The pose is published with a very high frequency so the chance to have two close messages (cloud and pose) should be high.

Anyway, my question is: MessageFilter should take care of this synchronization, but in the tutorial here is using the TransformListener class directly to transform, but I can't because I need to transform a cloud using the PCL methods. So what should I do to use it?

Something similar to what I did before with lookupTransform?

What is the difference in this case?

I use Ubuntu 12.04 on Hydro

Thanks

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-07-03 13:34:54 -0500

tfoote gravatar image

If you are processing data you should always use the timestamp associated with the data to transform it, never now().

If you use now() your transforms will be off by whatever is the aggregate latency in your data pipeline.

You are right that the MessageFilter is the preferred approach for waiting as it won't block your thread. It will give you the callback when the transform for the data is available and then you can do whatever operation you want inside the callback.

edit flag offensive delete link more

Comments

so, to take the proper TF I should extract the timestamp from the cloud and use it inside the listener with something like: lookupTransform("source","dest",ros::Time(timestamp)) ?

mark_vision gravatar image mark_vision  ( 2014-07-04 03:49:13 -0500 )edit

yes, header.stamp is already a ros::Time object.

tfoote gravatar image tfoote  ( 2014-07-05 17:50:02 -0500 )edit

OK now I did it without using a MessageFilter. What is the difference between this doing the same with MessageFilter?

mark_vision gravatar image mark_vision  ( 2014-07-07 04:34:17 -0500 )edit

A MessageFilter will hold data without blocking. If you have multiple parallel inputs which could be processed instead of blocking it will decrease latency.

tfoote gravatar image tfoote  ( 2014-07-07 19:02:25 -0500 )edit

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: 2014-07-03 13:10:47 -0500

Seen: 335 times

Last updated: Jul 03 '14