Small bugs in velocity map saturation and heuristics

[Magnus-Noren]

Hello,

Thank you for making this public. I find the library very useful in my current research.

I found a couple of small bugs when testing the code.

1. I found that the velocity map wasn't saturated at the correct distance when setting maxDistance != -1. This has two causes. First, the velocity shouldn't first be normalized (by dividing it by maxValue), when doing the saturation step. Secondly, the division by getLeafSize() shouldn't be there, since the leaf size is already accounted for in the Eikonal solver. With these changes, you get the saturation at the correct distance in real units, regardless of leaf size.
2. Also related to leaf size, when using heuristics, the setHeuristicTime forgets to take the leaf size into account, which leads to strange behaviour when leafSize != 1. I think the least intrusive fix is to multiply the return value by getLeafSize() in getPrecomputedDistance: return distances_[idx_dist] * grid_->getLeafSize();.

I made these changes on my end, so I'm not asking you to do anything, I just thought you'd might like to know. Once again, thanks!