MessageFilter with PointCloud + PoseMessage
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