Added function to transform points and poses
Changed EstimateVertical app to use transformPoints function