Minor changes to lib to not try to free an unallocated pointer.
calcPose.cpp is a ROS node for estimating pose - in early stages so far.