Fix issue with tolerance size

[rjoomen]

With the new toleranced waypoints, the tolerance variable type was introduced as a fixed Matrix (effectively Vector6d), always requiring 6 values. Here and here, however, a resize was still attempted, leading to silent errors, except for debug builds, where Eigen asserted an error.

This PR changes the types to a VectorXd, bringing the behavior in line with the coefs, allowing either 1 or 6 values, and fixing the resize() assert.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 84.47%. Comparing base (f141bdd) to head (4d4f3b7).

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #535      +/-   ##
==========================================
- Coverage   84.49%   84.47%   -0.02%     
==========================================
  Files         233      233              
  Lines       15864    15870       +6     
==========================================
+ Hits        13404    13406       +2     
- Misses       2460     2464       +4     

see 2 files with indirect coverage changes

[Levi-Armstrong]

@rjoomen What about just fixing the xml parsing code to check if the token size is 1 then create fixed size vector of 6 and if not 1 it must be of length 6 otherwise throw an exceptions? @marip8 Interested to hear your thoughts on this change?