Source: http://robotics.stanford.edu/~ang/papers/icraoss09-ROS.pdf