Potential issues in the curvature term in smoothing

[SteveMacenski]

Hi,

For reference, I'm working on a hybrid A* implementation for the navigation stack here. I've been able to experimentally show that there's at minimum 4 mistakes in the smoother terms in the paper. Because I'm using a NLP solver, I get some debug information that lets me test and have some knowledge about "wow that messed things up" or "wow that helped". I was suspect of the derivations so I did them myself separately and had differences. I haven't deeply looked at this implementation, but I see them reflected here too. I bring this up for visibility and if anyone can independently verify my math, we can add it here too:

• In all derivatives, the term begins with -1 / || delta x||. That -1 should be a 1. I derived this formulation every which way and that's definitely positive.
• in dk/dx-1 the sign of the second term switches to positive because d (delta X) for xi-1 is going to include a -1 term because its (xi - xi-1).
• The second term denominator should be ||delta x|| ^ 2 not (delta x)^2. They're equal, but just watch out. delta x ^2 implies a vector output.
• In this implementation, it looks like you're using the partials of the same term to create the curvature. If its d k / d xi-1, then that should be used in the xi-1 point, not the xi point. I believe you need to evaluate the derivatives at d ki-1 / dxi+1 such that at the local d k / dxi, Xi-1's xi+1 is this terms' contribution. I am not 100% sure on that, but it seems to reason such that the derivative terms for the local neighborhood can impact their directions.
• In this implementation, I think there's an issue with the curvature derivative combination. I'm not completely sure the rigor behind doing 0.25 * kim1 + 0.5 * ki + 0.25 * kip1. This could be just me missing something. @karlkurzer I would absolutely love your input on that. I see it done here too (which seems to be a straight copy) and I can't reason myself through it. If you can help with that, I can upstream any other issues I find while implementing this.

@facontidavide I see you've been active on this repo too, you run into any of this or just testing it out?

[karlkurzer]

Hey Steve

Thanks for you interest. I think it would be great to have a solid Hybrid A* implementation. Given the time constraints of my thesis I did not manage to finish some of the issues you mentioned. Thus the weight terms in the smoother are adjusted (e.g. here: https://github.com/karlkurzer/path_planner/blob/453864b55202283019380a941be50daacb4a06a0/include/smoother.h#L76 and https://github.com/karlkurzer/path_planner/blob/453864b55202283019380a941be50daacb4a06a0/include/smoother.h#L78)

Unfortunately I haven't had the time to fix these things, however in case you figured it out feel free to create a pull request :-)

[SteveMacenski]

in case you figured it out

In progress of that - wouldn't say I'm quite there yet, but 85%. I feel very confident in my smoothing and cost terms, after finding some of these curvature errors, much more confident in those but I'm still suspicious on how to properly use the partial derivatives without populating a jacobian. Can you talk at all about your decision making (if you can recall) around the gradient = 0.25 * kim1 + 0.5 * ki + 0.25 * kip1? The best I can back trace from that is it could have an integral looking something like (delta ki+1 - delta ki)^2 change in curvature nature to it, but that's not really what the term is with (ki - kmax)^2. Any hints on this would be immensely helpful.

Right now my main focus is on the CG upsampler which there exists little to any actual detail on. I'm also debating about deviating on this one and using bezier curves since the delta X between nodes aren't exactly uniform, but its definitely not randomly sampled in density. Surprisingly super-sampling a path and running CG/GD appears to be hard. If you linearly interpolate, the curvature is 0 in most terms. If you overlay on the original waypoints, the delta X is 0, basically any reasonable first guesses of how to super sample end up in an edge case or local minima. Real fun.

Edit: forgot to mention, I think your smoothing term is wrong, I think https://github.com/karlkurzer/path_planner/blob/master/src/smoother.cpp#L227 the 6 should be an 8. My analog is here unless because you're using a 5 point smoother instead of a 3 there's a couple cancellations.

[SteveMacenski]

Another suspect for error is the ones (https://github.com/karlkurzer/path_planner/blob/master/src/smoother.cpp#L196). The derivative is actually d/dx (||delta xi||) not d/dx(delta x), so its not +1 or -1 depending on the current xi or xi-1, but rather +/- delta x / ||delta x|| (which if you think about it, if the derivative was just identity, I think Thrun would have just said that. This is a pretty simple evaluation too so maybe there's even more to it, but I doubt it.).

I can show in my optimizer that both will converge to the global minimum, but this change will do so faster and more consistently. That makes me think that this contribution is relatively small but 100/100 being in the same 0.1ms evaluation time and the other method varying in the 10s of ms makes me think that this is the case.

[SteveMacenski]

FYI the Navigation2 Hybrid-A* planner has been merged and released. It boasts a fast compute time, well in excess of 10 hz for 85 meter path lengths in ~4,000 m^2 spaces. Super configurable, contains all these fixes, and computes all 4 smoother cost function terms.

https://discourse.ros.org/t/nav2-smacplanner-hybrid-a-2d-a-now-available-reminder-meeting-oct-15-cancelled/16759

[yslenjoy]

Edit: forgot to mention, I think your smoothing term is wrong, I think https://github.com/karlkurzer/path_planner/blob/master/src/smoother.cpp#L227 the 6 should be an 8. My analog is here unless because you're using a 5 point smoother instead of a 3 there's a couple cancellations.

If using 5 points, the 6 is correct, see picture below.