Optimized pseudo code for D*Lite

Question

I'm looking at the optimized version of the DLite:

procedure CalculateKey(s)
{01”} return [min(g(s), rhs(s)) + h(s_start, 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(s_goal) = 0;
{06”} U.Insert(s_goal, [h(s_start, s_goal); 0]);

procedure UpdateVertex(u)
{07”} if (g(u) != rhs(u) AND u ∈ U) U.Update(u,CalculateKey(u));
{08”} else if (g(u) != rhs(u) AND u /∈ U) U.Insert(u,CalculateKey(u));
{09”} else if (g(u) = rhs(u) AND u ∈ U) U.Remove(u);

procedure ComputeShortestPath()
{10”} while (U.TopKey() < CalculateKey(s_start) OR rhs(s_start) > g(s_start))
{11”} u = U.Top();
{12”} k_old = U.TopKey();
{13”} k_new = CalculateKey(u));
{14”} if(k_old < k_new)
{15”}   U.Update(u, k_new);
{16”} else if (g(u) > rhs(u))
{17”}   g(u) = rhs(u);
{18”}   U.Remove(u);
{19”}   for all s ∈ Pred(u)
{20”}   if (s != s_goal) rhs(s) = min(rhs(s), c(s, u) + g(u));
{21”}   UpdateVertex(s);
{22”} else
{23”}   g_old = g(u);
{24”}   g(u) = ∞;
{25”}   for all s ∈ Pred(u) ∪ {u}
{26”}   if (rhs(s) = c(s, u) + g_old)
{27”}     if (s != s_goal) rhs(s) = min s'∈Succ(s)(c(s, s') + g(s'));
{28”}   UpdateVertex(s);

procedure Main()
{29”} s_last = s_start;
{30”} Initialize();
{31”} ComputeShortestPath();
{32”} while (s_start != s_goal)
{33”} /* if (g(s_start) = ∞) then there is no known path */
{34”}   s_start = argmin s'∈Succ(s_start)(c(s_start, s') + g(s'));
{35”}   Move to s_start;
{36”}   Scan graph for changed edge costs;
{37”}   if any edge costs changed
{38”}     km = km + h(s_last, s_start);
{39”}     s_last = s_start;
{40”}     for all directed edges (u, v) with changed edge costs
{41”}       c_old = c(u, v);
{42”}       Update the edge cost c(u, v);
{43”}       if (c_old > c(u, v))
{44”}         if (u != s_goal) rhs(u) = min(rhs(u), c(u, v) + g(v));
{45”}       else if (rhs(u) = c_old + g(v))
{46”}         if (u != s_goal) rhs(u) = min s'∈Succ(u)(c(u, s') + g(s'));
{47”}       UpdateVertex(u);
{48”}     ComputeShortestPath()

I can't guess why at line 44 and 46 is evaluated the same condition (if u ~= s_goal). Wouldn't be possible to evaluate this before entering in the the if/if else that appear in line 43 and 45? Could it be :

if  (u != s_goal)
if (c_old=c(u,v))
  rhs(u)=min(rhs(u),c(u,v)+g(v));
elseif (rhs(u)=c_old + g(v))
  rhs(u)=min_s'(c(u,s')+g(s'))

would it be wrong?

Regards

Answer 1

That looks as though it would work, but if u is s_goal only rarely, then you've added one test to almost every iteration where rhs does not need updating to save a couple instructions overall in space and time.