Optimize findRouteSegment

Avoid object creation and insert for every candidate. Do it only when
necesary.