PLEASE NOTE: This code is depreciated and will not be maintained or developed further. This publication is now out of print and no longer available. Code depreciated: this code will not be update for ROS Kinetic or later.