perf(motion_utils): faster removeOverlapPoints and calcLateralOffset functions

[maxime-clem]

Description

Make the removeOverlapPoints and calcLateralOffset functions faster, simply by calling reserve on the vector created in removeOverlapPoints.

Tests performed

Benchmark test (disabled by default) was added. It calls the function 10000 times on a trajectory with 1000 points.

• Without the fix: ~250ms.
• After the fix: ~80ms.

To test on you local machine:

./build/motion_utils/test_motion_utils --gtest_filter=trajectory_benchmark.\* --gtest_also_run_disabled_tests

Effects on system behavior

None.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Attention: 20 lines in your changes are missing coverage. Please review.

Comparison is base (69813cb) 14.76% compared to head (5887861) 14.76%.
Report is 6 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5385      +/-   ##
==========================================
- Coverage   14.76%   14.76%   -0.01%     
==========================================
  Files        1662     1663       +1     
  Lines      115457   116135     +678     
  Branches    35639    36018     +379     
==========================================
+ Hits        17047    17142      +95     
- Misses      79181    79695     +514     
- Partials    19229    19298      +69     

Flag	Coverage Δ		*Carryforward flag
differential	18.13% <0.00%> (?)
total	14.76% <ø> (ø)		Carriedforward from 69813cb

*This pull request uses carry forward flags. Click here to find out more.

Files	Coverage Δ
...ils/include/motion_utils/trajectory/trajectory.hpp	65.54% <0.00%> (-0.12%)	⬇️
...est/src/trajectory/benchmark_calcLateralOffset.cpp	0.00% <0.00%> (ø)

... and 9 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.