Pointvectors

Based on discussion here: 
https://groups.google.com/group/ros-sig-navigation/browse_thread/thread/c6ec688240f59848