Fix bugs in BFSTree and BFS

Now, one-to-one matching works correctly with the test road networks.