Added support for outputting debug images for cage detection and ground
estimation