Reconstruction of complex environments by robust pre-aligned ICP