Cleaning the code. Better figures and code working for our lidar sample data.