reserve the size of the relevant obstacles vector to avoid excessive memory allocations
reserve the size of the relevant obstacles vector to avoid excessive memory allocations

This change becomes more significant as the amount of obstacles increases

rst-tu-dortmund/teb_local_planner,rst-tu-dortmund/teb_local_planner