I need a shortest path algorithm for controlling a real life robot.
Lets say i have map of the environment in the form of a matrix where 1 is an obstacle and 0 is free space. If i use a conventional shortest path algorithm, such as A* then that would give me a Manhattan distance shortest path. So nowhere near a the actual shortest path. This problem arises since i cannot think of a way to penalize movement in such a way that a diagonal line is better than two straight lines. I can make a heuristic that will make A* try euclidean shortest path between two points first, but not actually make euclidean shortest path a better path.
Does anyone know of a method to get the continuous space shortest path? It does not have to be the actual optimal path, but a better one than straight lines and 90 degree angles.
I have one idea:
From the start point make a circle.
Increase the radius of the circle until the one point on the circle is next to a wall, or at the goal. All the points on the edge of the circle are set as children nodes with the penalty of the radius of the circle. All points inside the circle, that are not open, will be closed since there is no reason to test them. Repeat this in an A* way with euclidean shortest path as heuristic, until the goal state is reached.Make the robot go from one point to the next in a straight line.
This should give something closer to what i am looking for. A set of straight lines with various angles. It would of course be better with a continuous curvy line...
I have implemented a continuous space path planning algorithm and blogged about it here. It uses A* to get an initial estimate and finalizes it (and pennalizes for sharp turns and robot's orientation at the destination) by using the simple gradient descent algorithm.
Let's say the discrete path from A* has n "waypoints" (coordinates on the grid). First and last ones cannot be moved but others can, as long as the path doesn't go through blocked grid cells. The function to be minimized is parametrized by n - 2 parameters which move waypoints perpendicular to its current direction.
Related
I'm working on my bachelor thesis (on Computer Science) and right now I'm having a problem about finding shortest path between two points on 3D triangular mesh that is manifold. I already read about MMP, but which computes distance function $d(x)$ between given point and vertex $x$ on mesh.
I got to know that the problem I'm solving is named Geodesics but What I really couldn't find is some good algorithm which uses A* for finding shortest path between two given points on two given vertices.
I 'invented' also algorithm which uses A* by using Euclidian Distance Heuristics and correction after finding new Point on any Edge..
I also have edges saved in half-edge structure.
So my main idea is this:
We will find closest edge by A* algorithm and find on this edge point with minimalizing function $f(x) + g(x)$ where $f$ is our current distance and $g$ is heuristics(euclidean distance)
Everytime we find new edge, we will unfold current mesh and find closest path to our starting point
So now my questions:
Do you know some research paper which talks about this problem ??
Why nobody wrote about algorithm that uses A* ??
What are your opinions about algorithm I proposed ?
Here are some papers and tools related to finding geodesics (or approximations) on a surface mesh:
A Survey of Algorithms for Geodesic Paths and Distances
You Can Find Geodesic Paths in Triangle Meshes by Just Flipping Edges (code)
The Vector Heat Method
(code)
You can find more papers in the survey paper.
I implemented the algorithm you mentionned (MMP) a long time ago and it's quite difficult to get it right and quite time consuming since the number of splits along an edge grows quite fast.
I am no expert in the matter so read with prejudice. Also sorry this is more of a comment than answer...
First You should clarify some things:
the mesh is convex or concave?
are the path always on surface or can fly between faces on the outside (if concave) but never inside?
are the start/end points on edges of faces or can be inside?
Assuming concave, points on edges and only surface paths...
I think the graph A* approach is unusable as there is infinite possible paths between point and any edge of the same face so how you test all of them?
If you really want A* then you can do something similar to raster A*
so resample all your edges to more points
so either n points or use some density like 10 points per average edge length or some detail size.
use graph A* on resampled points (do not handle them as edges anymore)
However this will produce only close to shortest path so in order to improve the accuracy you should recursively resample the edges near used point with higher and higher density until the distance between resampled points get smaller than accuracy.
Another option would be using something similar to CCD (cyclic coordinate descent) so:
create plane that goes through your 2 points and center of your mesh
create path that goes through all intersection of plane and faces between the 2 points (use the shorter on from the 2 options)
iterativelly move intersections back and forward and use greedy approach to get the result
However this might get stuck in local minima... You could use search/fitting approaches instead but those will get very slow with increasing number of faces
I got the feeling you might also do this using RANSAC ...
From my point of view I think the first A* approach is the most promising, you just need linked list of points per each edge and one cost counter per each its point from this you can simply encode even the recursive improvement of accuracy. It can be done even in-place so no reallocation needed in the recursion ... And also the algo is not complicated so you should have no problems implementing it, and the result is guaranteed which is not the case with other approaches I mention... Another pros is that it can be used even if start/endpoint does not belong to edge...
I'm currently struggling in finding an algorithm if a path is possible or not.
I have a field of points, the positions of these points are fully random. I have also a starting point, and a destination point. On my starting point I can jump to any point around the starting point in a limited radius, and continue the same from there, but only with a limited amount of jumps. Performance in this case is important! Existant algorithms like Dijkstra won't help me here.
Any idea?
You could construct an undirected graph with the points as vertices. Each of the edges connects two points which are no further apart than the jump distance limit. Once this graph is constructed, you can find the shortest path with traditional algorithms.
To construct the graph, you could assign the points to a grid of 2D matrix cells. The cell hight and width is the jump radius limit. Candidate points for an edge for a given point have to belong to its matrix cell or directly adjacent cells. This reduces the construction time.
A further speedup could be to restrict a first version of the graph to those grid cells which are located near the direct line-of-sight between start and end point. Only if the search is not sucessfull, you could broaden the search area and try again.
If start and end point are further apart than radius limit times jump limit, no feasible path exists.
Just in case someone want have a solution:
Since the amount of jumps are limited I've created a radial grid, where the maximum radius is the amount of circles multiplied by their own radius.
After that I simply use an A-star path finder. (I used one existant by http://www.rapidfirestudio.com)
I am looking for an algorithm to calculate the following:
I have:
A 3D triangle mesh. The triangles do not necessarily lie in one plane. The angle between the norm vectors of two neighbouring triangles is less then 90 degrees.
Two points. The two points lie either on an edge of the triangle mesh or inside a triangle of the mesh.
I need to calculate the polyline which represents the shortest path between the two points on the mesh.
What is the simplest and/or most effective strategy to do this?
As it stands, your problem is not well defined; there can be many solutions depending on the direction used to "project" the line segment onto the mesh.
Once you have chosen the direction of projection, flatten the mesh onto a plane perpendicular to the direction of projection. At this point, your mesh is a collection of 2d edges (line segments); just determine the intersection (if any) of each edge with your target line segment.
Edit:
The updated question is now well defined. Since my answer to the original question (above) has been marked as accepted, presumably that means the information given in the comments below are actually what was really being "accepted" for the update question. I'll summarize:
A google search of "shortest distance on 3d mesh" turns up some relevant information, like Shortest Path Approximation on Triangulated Meshes
Also, see: https://stackoverflow.com/a/10389377/294949 -- danh
Since your start/end points potentially lie anywhere on the mesh (not restricted to vertices) I guess you are searching for the geodesic shortest path (not Dikstra shortest path following edges). A nice algorithm is implemented in geometry-central: http://geometry-central.net/surface/algorithms/flip_geodesics/
The algorithm is described in the paper "You Can Find Geodesic Paths in Triangle Meshes by Just Flipping Edges".
A standard approach to this task of finding the shortest path polyline (or geodesic) on the surface of triangular mesh between two given points consists of two steps:
Find a path approximation between two points
Iteratively adjust it to make it locally shortest everywhere.
The first step (path approximation) can be computed, for example, using
Dijkstra algorithm, which considers only paths along mesh edges (no crossing of mesh triangles),
Or some variations of Dijkstra algorithm, better suited for near planar surfaces like A*-search,
Or it can be Fast marching method, which can find also the paths crossing the triangles, however not guaranteed to produce a geodesic line.
The next step is iterative adjustment of the approximate path till it becomes truly locally shortest. Some recent articles are really promising here, like Just Flipping Edges. But it requires to construct a path network from the original mesh before operating, so it can be really expensive if your task is just to find one shortest path on the mesh.
A more classical way is to consider every piece of current path approximation between it enters two consecutive vertices, and unfold the strip of crossed triangles on plane. Then find the shortest path in the planar strip, which is a task that can be solved exactly in a linear time, for example by Shortest Paths in Polygons method by Wolfgang Mulzer. The crossing of this line with the edges will give the shortest path on the mesh in between two vertices.
Then for every vertex on the path approximation, two walks around this vertex are evaluated using same unfolding in hope a path around will be shorter then the path exactly via the vertex. The last steps are repeated till convergence.
Below is an example of geodesic path on a mesh with 2 million triangles:
Left - from the application MeshInspector: the time of iterative adjustment is highlighted in green and it is only a small fraction of total time.
Right - the picture from Just Flipping Edges article, where total time is not given, but it is presumably even higher due to the necessity to construct path network from the mesh.
I'm implementing a robot to be able to solve any maze (where the robot only has front sensors, but I make it scan the surroundings), and I was able to get it to turn the maze into a map where 0 represents walls, and 1 represents roads, with possibly slanted roads. Now, the robot is not fast at turning, but fairly fast at moving down a straight line. Therefore, a normal shortest path algorithm through the somewhat slanted hallway would be slow, although the paths are wide enough for it.
For example, we find
0001111111000
0011111110000
0111111100000
1111111000000
1111110000000
As a possible map. I'd like the robot to recognize that it can walk diagonally, or even just go straight up then right then right again, instead of turning every time in a normal shortest path algorithm.
Any ideas? Also, a complete algorithm change is welcome too - I'm fairly new to this.
I've faced similar problem some time ago.
You can assign weights to surrounding cells and less weight to the front cell, thus making a weight graph that is made during the movement.
I used Dijkstra algorithm with weights of 2 for surrounding cells and weight 1 for the front cell, you must pass direction of robot to Dijkstra and when adding them to the priority queue, and when extracting cells from the queue add the neighbors with respect to the direction saved in the extracted cell.
Then make the move and then recompute the modified Dijkstra for finding the nearest unseen cell.
Imagine I am implementing Dijkstra's algorithm at a park. There are points and connections between those points; these specify valid paths the user can walk on (e.g. sidewalks).
Now imagine that the user is on the grass (i.e. not on a path) and wants to navigate to another location. The problem is not in Dijkstra's algorithm (which works fine), the problem is determining at which vertex to begin.
Here is a picture of the problem: (ignore the dotted lines for now)
Black lines show the edges in Dijkstra's algorithm; likewise, purple circles show the vertices. Sidewalks are in gray. The grass is, you guessed it, green. The user is located at the red star, and wants to get to the orange X.
If I naively look for the nearest vertex and use that as my starting point, the user is often directed to a suboptimal path, that involves walking further away from their destination at the start (i.e. the red solid path).
The blue solid path is the optimal path that my algorithm would ideally come up with.
Notes:
Assume no paths cross over other paths.
When navigating to a starting point, the user should never cross over a path (e.g. sidewalk).
In the image above, the first line segment coming out of the star is created dynamically, simply to assist the user. The star is not a vertex in the graph (since the user can be anywhere inside the grass region). The line segment from the star to a vertex is simply being displayed so that the user knows how to get to the first valid vertex in the graph.
How can I implement this efficiently and correctly?
Idea #1: Find the enclosing polygon
If I find the smallest polygon which surrounds my starting point, I can now create new paths for Dijkstra's algorithm from the starting point (which will be added as a new vertex temporarily) to each of the vertices that make up the polygon. In the example above, the polygon has 6 sides, so this would mean creating 6 new paths to each of its vertices (i.e. the blue dotted lines). I would then be able to run Dijkstra's algorithm and it would easily determine that the blue solid line is the optimal path.
The problem with this method is in determining which vertices comprise the smallest polygon that surrounds my point. I cannot create new paths to each vertex in the graph, otherwise I will end up with the red dotted lines as well, which completely defeats the purpose of using Dijkstra's algorithm (I should not be allowed to cross over a sidewalk). Therefore, I must take care to only create paths to the vertices of the enclosing polygon. Is there an algorithm for this?
There is another complication with this solution: imagine the user now starts at the purple lightning bolt. It has no enclosing polygon, yet the algorithm should still work by connecting it to the 3 points at the top right. Again, once it is connected to those, running Dijkstra's is easy.
Update: the reason we want to connect to one of these 3 points and not walk around everything to reach the orange X directly is because we want to minimize the walking done on unpaved paths. (Note: This is only a constraint if you start outside a polygon. We don't care how long you walk on the grass if it is within a polygon).
If this is the correct solution, then please post its algorithm as an answer.
Otherwise, please post a better solution.
You can start off by running Dijkstra from the target to find its distance to all vertices.
Now let's consider the case where you start "inside" the graph on the grass. We want to find all vertices that we can reach via a straight line without crossing any edge. For that we can throw together all the line segments representing the edges and the line segments connecting the start point to every vertex and use a sweep-line algorithm to find whether the start-vertex lines intersect any edge.
Alternatively you can use any offline algorithm for planar point location, those also work with a sweep line. I believe this is in the spirit of the more abstract algorithm proposed in the question in that it reports the polygon that surrounds the point.
Then we just need to find the vertex whose connection line to the start does not intersect any edge and the sum d(vertex, target) + d(vertex, start) is minimum.
The procedure when the vertex is outside the graph is somewhat underspecified, but I guess the exact same idea would work. Just keep in mind that there is the possibility to walk all around the graph to the target if it is on the border, like in your example.
This could probably be implemented in O((n+m) log m) per query. If you run an all-pairs shortest path algorithm as a preprocessing step and use an online point location algorithm, you can get logarithmic query time at the cost of the space necessary to store the information to speed up shortest path queries (quadratic if you just store all distance pairs).
I believe simple planar point location works just like the sweep line approaches, only with persistent BSTs to store all the sweepline states.
I'm not sure why you are a bothering with trying to find a starting vertex when you already have one. The point you (the user) are standing at is another vertex in of itself. So the real question now is to find the distance from your starting point to any other point in the enclosing polygon graph. And once you have that, you can simply run Dijkstra's or another shortest path algorithm method like A*, BFS, etc, to find the shortest path to your goal point.
On that note, I think you are better off implementing A* for this problem because a park involves things like trees, playgrounds, ponds (sometimes), etc. So you will need to use a shortest path algorithm that takes these into consideration, and A* is one algorithm that uses these factors to determine a path of shortest length.
Finding distance from start to graph:
The problem of finding the distance from your new vertex to other vertices can be done by only looking for points with the closest x or y coordinate to your start point. So this algorithm has to find points that form a sort of closure around the start point, i.e. a polygon of minimum area which contains the point. So as #Niklas B suggested, a planar point algorithm (with some modifications) might be able to accomplish this. I was looking at the sweep-line algorithm, but that only works for line segments so that will not work (still worth a shot, with modifications might be able to give the correct answer).
You can also decide to implement this algorithm in stages, so first, find the points with the closest y coordinate to the current point (Both negative and positive y, so have to use absolute value), then among those points, you find the ones with the closest x coordinate to the current point and that should give you the set of points that form the polygon. Then these are the points you use to find the distance from your start to the graph.