# LPA* implementation keeps looping

### Short story

I am currently trying to implement LPA* in an existing navigation system and find the algorithm seems to loop forever, expanding the same vertices over and over again. I am wondering what is causing this to happen, and what I can do to rectify this.

### Long story

The navigation system uses Dijkstra so far. As I am extending it to react to changes in the traffic situation (and thus edge costs) by changing the already-calculated route, I decided to go for LPA*, as it is essentially an evolution of Dijkstra.

The existing Dijkstra implementation diverts from the canonical one in a few ways:

• The cost of a vertex is the cost to reach the destination from that vertex. Cost thus descreases as we travel along the route, the destination (theoretically) has a cost of zero. This way, the route graph remains valid as the vehicle position changes, as long as the destination remains the same.
• If the destination is off-road, it is not directly represented by a vertex. Instead, a handful of nearby vertices are given a cost slightly above zero and inserted in the priority queue. (That initial cost can still be lowered—it is as if each of these nodes were directly connected to the destination with a vertex whose cost is equal to the cost of the node.)
• Current position works in a similar way: it may be off-road or in the middle of a vertex, so we simply add a penalty to the nearest vertex.
• The algorithm terminates after the last vertex in the graph has been visited, even if all vertices around the current position have already been expanded. That allows us to keep the route graph even if the vehicle takes a wrong turn and ends up in a place from which the destination is much more expensive to reach.
• Each vertex maintains a pointer to the next edge along the cheapest path from that vertex to the destination. Where ambiguities exist, the pointer may point to any acceptable edge (the decision is probably stable across multiple runs, but not guaranteed to be).
• Instead of infinity, we use $2^{31} - 1$, or 0x7FFFFFFF (largest positive signed 32-bit integer) as pseudo-infinity. Since cost represents travel time in tenths of seconds, the maximum is in the order of magnitude of several years, and any numbers likely to be encountered in practice are several orders of magnitude below it.
• Most edges are traversable in both directions (and, technically, if they aren’t, their cost is simply pseudo-infinity). Therefore, most successors of any given vertex are also predecessors of it, and vice versa.

I have maintained these diversions and kept the existing data structures, adding just a rhs member to the vertex data structure. Further deviations from canonical LPA* I introduced:

• For the moment, Dijkstra is still used for the original route (I am planning to move everything to LPA* in a later step). When a vertex is visited, I set its rhs member to its value so it will appear locally consistent to a subsequent LPA* run.
• I use a fixed heuristic, $h = 0$. This is valid for A* (effectively turning it into Dijkstra) and thus also for LPA*, and makes it easier for Dijkstra and LPA* to coexist.
• Because $h = 0$, there is no need for the two-dimensional keys in the priority queue (as both elements if each key would always be identical). That permits me to keep using the Fibonacci heap for the priority queue in the same manner as with Dijkstra.
• Rather than a simple addition of edge and route costs, I use a function with some overflow protection. If one of the two operands is 0x7FFFFFFF (pseudo-infinity), this is also returned as a result.
• Similar to the Dijkstra implementation, I do not terminate until the entire route graph is flooded.

However, if I change some edge costs in an already-flooded route graph and then run my LPA* implementation on it, it keeps looping. Debug output shows me that it is expanding the same vertices over and over again, with their costs increasing. What appears to happen here:

• A vertex $v$ is updated.
• All vertices in $Succ(v)$ (essentially all neighbors of $v$) are updated.
• For each $u \in Succ(v)$, all vertices in $Succ(u)$ are updated. Since (in most cases) $v \in Succ(u)$, $v$ is updated again and the cycle starts over.

What I have tried:

• After updating $v$, specifically exclude its shortest-path predecessor (i.e. the next vertex towards the destination) from being updated. That causes the LPA* implementation to terminate in a time simila to Dijkstra, but leaves me with loops in the route graph (i.e. two adjacent vertices both pointing to the edge connecting them).
• If, in addition to the above, while updating a vertex, I skip other vertices which would create a loop condition, that leaves me with the part of the route graph “upstream” from the changed segments having maximum cost.

Where is the error, and how can I fix it?

### Full story

https://github.com/mvglasow/navit/tree/traffic. The commit which added the actual LPA* implementation is 61d9a1a; some preparation work was also done in the previous ones.

• I suggest using standard debugging techniques. Try to find a MCVE, specifically, a minimal-size graph that causes infinite looping. Once you've done that, look at the sequence of updates that get made to understand why a loop is happening. You can also try to write a proof that the algorithm should terminate. Then, if you're able to do that, check what invariants you found yourself using in that proof, and add asserts to your code to check that all of those invariants are true. Can you show concise pseudocode for LPA*? – D.W. May 31 '18 at 18:33
• For concise pseudocode of generic LPA* see LPA* on Wikipedia. I authored the article, based on the paper cited, then based my implementation on it. Therefore, if I introduced any errors in the pseudocode, they would likely have propagated to the actual code. MCVE is difficult, as it is hard to separate the code in question from the rest of the application. – user149408 May 31 '18 at 20:04
• In this case I'm talking about a minimal graph (a minimal input), not minimal subset of code. – D.W. May 31 '18 at 20:26
• @D.W. Thanks, that did indeed reveal that, at least in some cases, the function to calculate edge costs still returns the old values, not the new ones. Apparently I overlooked that the route graph is built from all configured maps, and if the map changes (traffic conditions are also map items), this information needs to be updated. Though that may not be the whole story, I need to fix this first. – user149408 Jun 2 '18 at 1:47

I have solved the issue, and my LPA* implementation now gives me a valid route that is identical to the one I get when I run Dijkstra from scratch on the updated route graph for the test cases that I have tried.

Fist off: The two things that I tried above would break LPA*, so I reverted them.

The first fault was simply a sign error when calling the function to determine edge costs, leading the program to use costs for the wrong direction (though only in one place, not in another).

The other fault was in the call to the function which determines edge costs. Arguments passed to it are a vehicle profile (which determines which edges are usable and what their costs are), the edge, a direction (i.e. first to second vertex or vice versa) and, optionally, a vertex.

The vertex argument was misleadingly named from. In reality, when specified, it is the vertex at which we leave the edge and move on to the next when following the route to the destination. If that vertex has the edge pointer (see above) set to the edge being examined, the function will report the cost of the edge from to be pseudo-infinity (as traversing the edge in that direction will move us further away from the destination). Documentation was scarce and I had to dig through the source code to infer what that argument really was supposed to do.

I found I passed the wrong vertex to the function; fixing this now-obvious error made some of the simpler test cases work.

I also now clear the edge pointer at the beginning of updateVertex() (it is set again in the function as we examine each edge); though this may not be relevant to the issue here.

About predecessors and successors, we can simply use all immediate neighbors (any vertex directly connected to the current one by an edge). If an edge cannot be traversed in a particular direction, we assume its cost in that direction to be infinity.

With these fixes, I get a correct route for simpler test cases. For the more complex ones, LPA* would terminate in a timely manner, but when iterating through the route graph, I would encounter a loop.

Further investigation revealed that this was due to “loop” edges in the graph, which connect a vertex with itself. If the edge pointer of the vertex pointed to that faulty edge, the iteration would loop forever upon hitting one of these points. Dijkstra apparently never picked up such edges as part of the cheapest path (I didn’t investigate this further), but LPA* did.

Due to the way the implementation works (position and destination need not correspond to points on the route graph), such loops can be legit (think of a circular road with only one single access road connecting to it), and parts of it can even become part of the cheapest path when the current position or destination is located on such an edge. However, there is no legit reason to have such a segment appear in the middle of the cheapest path.

I resolved this by checking for this in the cost function: if the segment is a loop and it is not connected to the position or destination, its cost is assumed to be pseudo-infinity to prevent it from becoming part of the cheapest path.