I am trying to implement an algorithm called the 'rapidly exploring random belief tree'. The aim of this algorithm is to come up with a path for a robot, which, instead of connecting start and goal with a least distance metric or something similar, drives the robot into areas where it can get high accuracy measurements, and only then moves to the goal.
In the implementation of the algorithm, I start off by building a graph through random sampling of a given space. Every time a new point is sampled, a new edge is added to the graph. Initially, the error in the position of the robot will be high, as it won't be getting good measurements, but once the tree I am exploring through reaches the 'good' area, the robot's error suddenly drops, and with this knowledge of where the 'good' area is, I traverse backwards through the graph, pruning previously connected edges and connecting those vertices towards the good area. Given enough samples, in an optimal scenario, the graph would be propagating outward from the good area, hence connecting any two vertices would guarantee that you would pass through these points.
Here's where I have a problem. Once I start the traversal to prune and update my edges, the algorithm shouldn't prune the edges that connect the good area to the starting point, which would result in an infinite loop. An example is shown in this picture:
The algorithm finds vertices 1, 2, 3 and 4. The connections are 1 -> 2, 2 -> 3, 2 -> 4.
Now we find #5, which is a 'good' area for the robot. Visiting this vertex will magically enhance its accuracy in the future. I find neighboring vertices from 5: I find #3. I update the parent of #3 to #5 instead of #2. Now if there were more vertices surrounding #3, I would traverse those too, and update their parents to #3 etc., thus creating a path from #5 to all of those. (For more information, the traversal essentially computes the covariance of the robot if it were to move from #5 to #3 and whether that's more desirable than moving from #2 to #3).
- But in no circumstances should I update the parent of #2 to #3: because the only way the robot got to #5 in the first place was through #2.
To avoid this, a simple (but highly inefficient?) thing I did was to compute the shortest path to the vertex from where I began my traversal, and if any vertex I come across during traversal is part of this path, I don't update its parent. This works reasonably well, at the expense of a lot of computational cost because I need to do this every time I have to traverse backwards with an update, so I was wondering if there was a better, efficient way of doing this.