[ros-users] generic pose kalman filtering and robot_pose_ekf

Radu Bogdan Rusu rusu at willowgarage.com
Fri Sep 3 05:05:09 UTC 2010


Adam, before Wim replies, I can suggest an alternative approach.

The reason why the cylinder segmentation might not be stable is due to noise in normal space. To estimate the cylinders 
the model expects two points with good surface normals. The first thing to check is to see if you can increase the 
normal support size ("k-neighborhood") either by bumping k_search to a higher value, or alternatively by using a bigger 
radius (radius_search).

However, even so, if your data fluctuates a lot between frames (I assume you're using narrow stereo here on the PR2 or 
the tilt laser - correct me if I'm wrong :) ), you can do two things:

* use a median filter to average/cancel noise out between frames and do the segmentation in the filtered cloud
and/or
* leave the cylinder segmentation as it is, run it over N frames, and then run the segmentation again but only in the 
reunion over all inlier indices from the previous N frames.

[hopefully soon we'll have better 3D sensors and all these things will be history :)]

The latter should help stabilize the model quite a bit, as the starting point is close to optimal.

Of course, you can always try playing with the individual thresholds for the segmentation step.


PS. Finally, one of the things that I like best is to filter and smooth out the data before such a segmentation. You can 
either use a VoxelGrid (which Bastian greatly improved today! :) or something more fancy like a MovingLeastSquares 
algorithm.

Cheers,
Radu.

On 09/02/2010 09:35 PM, Adam Leeper wrote:
> Hello-
>
> I am using pcl to estimate the pose of object primitives (in this case,
> a cylinder) from stereo data.
>
> I searched ros.org <http://ros.org> for "kalman" and found the
> robot_pose_ekf package. I dug around in the source for a bit, and it
> seems that it is all hard-coded to only relate "base_footprint" to
> "odom_combined." It seems to me that with some modifications, this could
> be used as a generic 6-dof pose filter. Before I make a copy and go and
> change a bunch of things, am I right in thinking this would work as a
> generic filter to smooth out the (rather jumpy) estimates of the
> cylinder pose? The only incoming estimates would be a visual odometry of
> sorts, that is, a 3D pose from the pcl cylinder segmentation. Would this
> smooth that out even if it is the only sensor source being used?
>
> If I've overlooked some other handy package that will filter for me,
> that would be good to know too...
>
> Thanks!
>
> Adam Leeper
> Stanford University
> aleeper at stanford.edu <mailto:aleeper at stanford.edu>
> 719.358.3804
>
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users

-- 
| Radu Bogdan Rusu | http://rbrusu.com/



More information about the ros-users mailing list