The D*-Lite algorithm

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 edg开发者_开发百科e 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();

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".

Read LPA* paper you will know what they are. Basically, in LPA*, the search begin at start position. So the successors would be the nodes around the u.Pop node. This mean that, they are the nodes you will go to from the current node. And Pred, it is simply a mother node. This means that Pred of successors is u.Pop.

In DLite, everything goes conversely. The search begin at the Goal position. So, it is a little confused for you. Successor of DLite is Pred in LPA*. So, successor = U.pop. Pred of DLite is successor in LPA. So, Pred is the node you will go to from an successor.

Hope you understand my poor English.