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.