Robust weighted scan matching with quadtrees