FMM replanning + heuristic breaks at new obstacles

[roncapat]

At step 2 of WALL test, the straight path from start to goal is broken by the obstacle. The above situation happens with 4-connected FMM.

Applying multistencil (8-connected, orthogonal and diagonal stencil) causes the following (maybe not its fault):

The graph disconnects, maybe since the heuristic causes the nodes of the disconnected region to consolidate before the others.

[roncapat]

Disabling heuristic solves the issue. But why? This is not the right solution.

[roncapat]

It appears this can happen also with no Multistencil FMM !

[roncapat]

A pathological case has been identified. Field D* passes the ad-hoc test. FMM (both single and dual stencil) fail.

[roncapat]

From Philippsen and Siegwart

There is a fundamental difference between Raise-Event propagation in E ∗ and RAISE state calculations in D∗. The latter does not set the node’s path cost to infinity, but calculates the usual path cost propagation. This is consistent with the single backpointers used in D ∗ , but interpolation and the multiple backpointers needed to trace cell dependencies raise the following problem in E ∗ : Suppose a cell receives a raise event from one of its neighbors. If we now recalculate the interpolation, this would likely result in a change of backpointers because the previously good propagator is now at a higher path cost. However, the backpointers are needed to propagate a path cost increase to all descendants of a location. Changing a backpointer while this chain has not been completed violates the upwind property. These issues are addressed by setting a cell’s value to infinity when a raise event is propagated, and passing on the raise wake to the unmodified backpointers. Now that the cell is at infinity, the backpointers become useless (the cell cannot be raised further in any case), which is why they are set to null to avoid creating spurious raise events that would result in no change of the navigation function. In any case, after a raise event, a cell will be subject to a retry event which applies the interpolation and sets new backpointers.

[roncapat]

Retry to introduce heuristic with the actual refined implementation