Still in debug mode. Slightly refactored to give routines better na es that reflect task.
Problem? I cannot get plan displayed properly on 2D RGB image.