Path finding algorithm, small improvements for obj recognition