The D*-Lite algorithm

Question

I'm trying to implement the D*-Lite pathfinding algorithm, as described in the 2002 article by Koenig and Likhachev, for Boost::Graph. I think I've gotten a decent grasp on the basic ideas and theory behind it, but I'm having a problem understanding when the Pred and Succ sets are updated.

I'm guessing it happens in the Move to sstart step in Main, but then the first call to ComputeShortestPath will be rather pointless? And is the Succ set supposed to be inserted into at the same time as Pred only? Then Pred and Succ could be implented as doubly linked lists?

I've inserted the pseudocode of the algorithm below. The Pred and Succ sets are predecessors and successors respectivly. g, h, rhs and c are different costs and weights. U is a priority queue of vertices to visit.

procedure CalculateKey(s)
{01’} return [min(g(s), rhs(s)) + h(sstart, s) + km; min(g(s), rhs(s))];

procedure Initialize()
{02’} U = ∅;
{03’} km = 0;
{04’} for all s ∈ S rhs(s) = g(s) = ∞;
{05’} rhs(sgoal) = 0;
{06’} U.Insert(sgoal, CalculateKey(sgoal));

procedure UpdateVertex(u)
{07’} if (u ≠ sgoal) rhs(u) = min s'∈Succ(u)(c(u, s') + g(s'));
{08’} if (u ∈ U) U.Remove(u);
{09’} if (g(u) ≠ rhs(u)) U.Insert(u, CalculateKey(u));

procedure ComputeShortestPath()
{10’} while (U.TopKey() < CalculateKey(sstart) OR rhs(sstart) ≠ g(sstart))
{11’}   kold = U.TopKey();
{12’}   u = U.Pop();
{13’}   if (kold ˙<CalculateKey(u))
{14’}     U.Insert(u, CalculateKey(u));
{15’}   else if (g(u) > rhs(u))
{16’}     g(u) = rhs(u);
{17’}     for all s ∈ Pred(u) UpdateVertex(s);
{18’}   else
{19’}     g(u) = ∞;
{20’}     for all s ∈ Pred(u) ∪ {u} UpdateVertex(s);

procedure Main()
{21’} slast = sstart;
{22’} Initialize();
{23’} ComputeShortestPath();
{24’} while (sstart ≠ sgoal)
{25’}   /* if (g(sstart) = ∞) then there is no known path */
{26’}   sstart = argmin s'∈Succ(sstart)(c(sstart, s') + g(s'));
{27’}   Move to sstart;
{28’}   Scan graph for changed edge costs;
{29’}   if any edge costs changed
{30’}     km = km + h(slast, sstart);
{31’}     slast = sstart;
{32’}     for all directed edges (u, v) with changed edge costs
{33’}       Update the edge cost c(u, v);
{34’}       UpdateVertex(u);
{35’}     ComputeShortestPath();

Answer 1

It turns out I didn't have a decent grasp on the basic ideas and theory... I misunderstood the meaning of "successor" and "predecessor", since I assumed it was meant "in path order" so that in a path v0->v1->v2, v0 would be the predecessor of v1, and v2 the successor.

What was meant however was simply the neighbours. The predecessor set was the set of all vertices with an "in-edge" to the given vertex, and the successors had "out-edges".