Improved buildmap to handle point clouds with initial guess poses.  Improved trajectory visualisation. Removed unnecessary np.vectorize in occupiedlist.py.   Significantly accelerated buildmap by decoupling the map resolution from the volumetric sample resolution as a result map building tests run much faster.  Added graph and save results to speed_test.py.