C++ software that fixes anomalous Lidar points