Add diagnostics to organized plane segmentation

![screenshot_from_2014-08-26 22 52 18](https://cloud.githubusercontent.com/assets/40454/4045288/71ebbbec-2d28-11e4-9bc0-0e926e78304f.png)