3D mosaicing for environment reconstruction