Triangulation works by checking your angle to three KNOWN targets.
"I know the that's the Lighthouse of Alexandria, it's located here (X,Y) on a map, and it's to my right at 90 degrees." Repeat 2 more times for different targets and angles.
Trilateration works by checking your distance from three KNOWN targets.
"I know the that's the Lighthouse of Alexandria, it's located here (X,Y) on a map, and I'm 100 meters away from that." Repeat 2 more times for different targets and ranges.
But both of those methods rely on knowing WHAT you're looking at.
Say you're in a forest and you can't differentiate between trees, but you know where key trees are. These trees have been hand picked as "landmarks."
You have a robot moving through that forest slowly.
Do you know of any ways to determine location based solely off of angle and range, exploiting geometry between landmarks? Note, you will see other trees as well, so you won't know which trees are key trees. Ignore the fact that a target may be occluded. Our pre-algorithm takes care of that.
1) If this exists, what's it called? I can't find anything.
2) What do you think the odds are of having two identical location 'hits?' I imagine it's fairly rare.
3) If there are two identical location 'hits,' how can I determine my exact location after I move the robot next. (I assume the chances of having 2 occurrences of EXACT angles in a row, after I reposition the robot, would be statistically impossible, barring a forest growing in rows like corn). Would I just calculate the position again and hope for the best? Or would I somehow incorporate my previous position estimate into my next guess?
If this exists, I'd like to read about it, and if not, develop it as a side project. I just don't have time to reinvent the wheel right now, nor have the time to implement this from scratch. So if it doesn't exist, I'll have to figure out another way to localize the robot since that's not the aim of this research, if it does, lets hope it's semi-easy.
Great question.
The name of the problem you're investigating is localization, and it, together with mapping, are two of the most important and challenging problems in robotics at the moment. Put simply, localization is the problem of "given some sensor observations how do I know where I am?"
Landmark identification is one of the hidden 'tricks' that underpin so much of the practice of robotics. If it isn't possible to uniquely identify a landmark, you can end up with a high proportion of misinformation, particularly given that real sensors are stochastic (ie/ there will be some uncertainty associate with the result). Your choice of an appropriate localisation method, will almost certainly depend on how well you can uniquely identify a landmark, or associate patterns of landmarks with a map.
The simplest method of self-localization in many cases is Monte Carlo localization. One common way to implement this is by using particle filters. The advantage of this is that they cope well when you don't have great models of motion, sensor capability and need something robust that can deal with unexpected effects (like moving obstacles or landmark obscuration). A particle represents one possible state of the vehicle. Initially particles are uniformly distributed, as the vehicle moves and add more sensor observations are incorporated. Particle states are updated to move away from unlikely states - in the example given, particles would move away from areas where the range / bearings don't match what should be visible from the current position estimate. Given sufficient time and observations particles tend to clump together into areas where there is a high probability of the vehicle being located. Look up the work of Sebastian Thrun, particularly the book "probabilistic robotics".
What you're looking for is Monte Carlo localization (also known as a particle filter). Here's a good resource on the subject.
Or nearly anything from the probabilistic robotics crowd, Dellaert, Thrun, Burgard or Fox. If you're feeling ambitious, you could try to go for a full SLAM solution - a bunch of libraries are posted here.
Or if you're really really ambitious, you could implement from first principles using Factor Graphs.
I assume you want to start by turning on the robot inside the forest. I further assume that the robot can calculate the position of every tree using angle and distance.
Then you can identify the landmarks by iterating through the trees and calculating the distance to all its neighbours. In Matlab you can use pdist to get a list of all (unique) pairwise distances.
Then you can iterate through the trees to identify landmarks. For every tree, compare the distances to all its neighbours to the known distances between landmarks. Whenever you find a candidate landmark, you check its possible landmark neighbours for the correct distance signature. Since you say that you always should be able to see five landmarks at any given time, you will be trying to match 20 distances, so I'd say that the chance of false positives is not too high. If the candidate landmark and its candidate fellow landmarks do not match the complete relative distance pattern, you go check the next tree.
Once you have found all the landmarks, you simply triangulate.
Note that depending on how accurately you can measure angles and distances, you need to be able to see more landmark trees at any given time. My guess is that you need to space landmarks with sufficiently density that you can see at least three at a time if you have high measurement accuracy.
I guess you need only distance to two landmarks and the order of seeing them (i.e. from left to right you see point A and B)
(1) "Robotic mapping" and "perceptual aliasing".
(2) Two identical hits are inevitable. Since the robot can only distinguish between a finite number X of distinguishable tree configurations, even if the configurations are completely random, there is almost certainly at least one location that looks "the same" as some other location even if you encounter far fewer than X/2 different trees. Those are called "birthday paradox collisions". You may be lucky that the particular location you are at is in fact actually unique, but I wouldn't bet my robot on it.
So you:
(a) have a map of a large area with
some, but not all trees on it.
(b) a
robot somewhere in the actual forest
that, without looking at the map, has
looked at the nearby trees and
generated an internal map of a all
the trees in a tiny area and its
relative position to them
(c) To the
robot, every tree looks the same as
every other tree.
You want to find: Where is the robot on the large map?
If only each actual tree had a unique name written on it that the robot could read, and then (some of) those trees and their names were on the map, this would be trivial.
One approach is to attach a (not necessarily unique) "signature" to each tree that describes its position relative to nearby trees.
Then, as you travel along, the robot drives up to a tree and finds a "signature" for that tree, and you find all the trees on the map that "match" that signature.
If only one unique tree on the map matches, then the tree the robot is looking might be that tree on the map (yay, you know where the robot is) -- put down a weighty but tentative dot on the map at the robot's relative position to the matching tree -- the tree the robot is next to is certainly not any of the other trees on the map.
If several of the trees on the map match -- they all have the same non-unique signature -- then you could put some less-weighty tentative dots on the map at the robots position relative to each one of them.
Alas, even if find one or more matches, it is still possible that the tree the robot is looking at is not on the map at all, and the signature of that tree is coincidentally the same as one or more trees on the map, and so the robot could be anywhere on the map.
If none of the trees on the map matches, then the tree the robot is looking at is definitely not on the map. (Perhaps later on, once the robot knows exactly where it is, it should start adding these trees to the map?)
As you drive down the path, you push the dots in your estimated direction and speed of travel.
Then as you inspect other trees, possibly after driving down the path a little further, you eventually have lots of dots on the map, and hopefully one heavy, highly overlapping cluster at the actual position, and hopefully each other dot is an easily-ignored isolated coincidences.
The simplest signature is a list of distances from a particular tree to nearby trees.
A particular tree on the map is "matched" to a particular tree in the forest when, for each and every nearby tree on the map, there is a corresponding nearby tree in the forest at "the same" distance, as far as you can tell with your known distance and angular errors.
(By "nearby", I mean "close enough that the robot should be able to definitely confirm that the tree is actually there", although it's probably simpler to approximate this with something like "My robot can see all trees out to a range of R, so I'm only going to bother even trying to match trees that are within a circle of R*1/3 from my robot, and my list of distances only include trees that are within a circle of R*2/3 from the particular tree I'm trying to match").
If you know your north-south orientation even very roughly, you can create signatures that are "more unique", i.e., have fewer spurious matches on the map and (hopefully) in the real forest.
A "match" for the tree the robot is next to occurs when, for each nearby tree on the map, there is a corresponding tree in the forest at "the same" distance and direction, as far as you can tell with your known distance and angular errors.
Say you see that tree "Fred" on the map has another tree 10 meters in the N to W quadrant from it, but the robot is next to a tree that definitely doesn't have any trees at that distance in the N to W quadrant, but it has a tree 10 meters away to the South.
In that case, then (using a more complex signature) you can definitely tell the robot is not next to Fred, even though the simple signature would give a (false) match.
Another approach:
The "digital paper" solves a similar problem ... Can you plant a few trees in a pattern that is specifically designed to be easily recognized?
Related
I'm looking for an efficient way (or existing library/tool) to group 3D coordinates that are within a given distance from another "group member" coordinate. So if 5 coordinates are within 2m of at least 1 member of that group, they'll be lumped into a single group.
Comparing the distance from every coordinate to every other coordinate would be awful performance. Some possible solutions require knowing how many groups you'll have ahead of time, which I do not. Some solutions in python rely on large math libraries I won't have in java and would prefer not to rewrite.
The easiest efficient approach here would be a sweep and prune algorithm to identify the close-enough pairs, and a union-find structure to use those edges to produce the groups. You should be able to find existing code for both halves pretty easily.
I have access to a list of lat/long coordinates, and I want to know (roughly) the US State these coordinates are located in. I can do with loss of precision, but I can't rely on external libraries or API. I can also add a database of locations in my code.
What is a reasonable way to do this?
I thought about 3 possibilities:
Represent each state by a single point at its center, then do a nearest-neighbour search
Represent each state by points located at cities in the state, then do a nearest-neighbour search (with much more points)
Represent each state by a simple bounding box, then use some algorithm to query which bounding box my point belongs to
What do you think is best? I would tend to think about solution 3, but I can't find a list of coarse "bounding boxes" for US states
I made a little search and find out a proper solution for what you are looking for with a dataset of bounding box.
Answer on StackOverflow: LINK
Dataset: LINK
Algorithm to use(implement): LINK
So yes, the proper way to implement it's using the solution 3 with the given dataset.
Hope it helps :)
Will not work, consider
Has a high likelihood to not work for at least some states. Consider states with towns/cities more clustered to the middle, against states with towns/cities clustered to the edge.
Will not work (these were supposed to be 90 degree angles, perfect squares, but drawing with a mouse is hard :) )
If you want to do this even vaguely accurately you will need some shape data which defines the boundaries between states. You will then need an algorithm which can determine whether a point is within an irregular polygon
See List of the United States (US) state boundaries / borders as latitude/longitude pairs for geofence?
So I've been looking for an explanation of a 2-opt improvement for the traveling salesman problem, and I get the jist of it, but I don't understand one thing.
I understand that IF two edges of a generated path cross each other, I can just switch two points and they will no longer cross. HOWEVER - I do not understand how I can determine whether or not two edges cross.
To make my question clear, this is what I've done so far: (I did it in java)
I have an object called Point that represents a city, with an x and y coordinate.
I have a PointSet which has a set of Points contained in a List.
I have a method for PointSet called computeByNN() which arranges the PointSet in a fairly short manner through a Nearest Neighbor algorithm.
So now I have a sorted PointSet (not optimal, but still short) and I want to do 2-opt on it. However, I don't know where to start. Should I check each and every line segment to see if they cross, and if they do, switch two points? I feel like that defeats the purpose of heuristics and it becomes a sort of brute force solution. Is there an efficient way to find if two segments of the tour cross?
I applogize if my question is not clear. I'll try to edit it to make it clearer if anyone needs me to.
If you like you can create a look-up table to detect crossed edges. For n = 1000, order 10^12 entries is obviously too extravagant. However you probably are most worried about the shorter edges? Suppose you aimed to include edges to about √n of the nearest neighbors for each node. Then you are only in the realm of megabytes of space and in any case O(n^2) preprocessing. From there it's a heuristic, so good luck!
Also will mention this can be done on the fly.
I have been working on a 2D top down shooter game for a while, I've implemented most of the game and wrote the engine from scratch in JOGL but i ran into a small problem and would like to get other peoples view on how to best approach the problem. So I have creeps spawning at random locations in the map, and each of these creeps use A* path finding, it has been optimized to minimize unnecessary checks, but the maps are massive can be anything from 10x10 to 200x200 tiles and the only thing slowing down the game significantly is the AI, I've also tried to implement a distance based solution where the creeps Idle until i am in a certain range but that still slows down the game a lot because a lot of creeps are spawned. Any advice would be appreciated.
There are numbers of ways of speeding up your code.
First - there are many modifications of the A* algorithm, which may be used, like:
Hierarchical A*, which is often used method in games, where the map is analyzed on many resolution levels, from "general planning" to the "local path search" http://aigamedev.com/open/review/near-optimal-hierarchical-pathfinding/
Jump Point Search A*, which dramaticaly speeds up A* for maps with lots of open spaces (like RPG games) http://gamedev.tutsplus.com/tutorials/implementation/speed-up-a-star-pathfinding-with-the-jump-point-search-algorithm/
Other modifications can be more application specific, if your creeps are searching a path to the player (there is one goal for all creeps), then you can change your search to one of following algorithms:
calculate distance from player to each point in the map using Dijkstra algorithm, for 200x200 it will be very quick (40,000 vertices with O(nlgn) algorithm), and simply move your creep to any adjacent point with less distance to player then current one
run A* search from the player to any creep (with lowest id for example), once the path is found - change aim to the next creep but do not reset the algorithm itself, let if use already computed paths and distances (as they are already optimal paths from player), obviously - if during execution you encounter another creep then your goal - you simply record it (found path is optimal).
Another possible modification, which can be applied if your map is somehow specific (contains doors/entrances to some parts of it) is to place triggers, which "enable" creeps AI. This is O(1) solution, but requires a specific type of map.
And one final idea would be to implement some suboptimal solutions, by for example:
First, calculate A* for each creep
If the distance to the player is smaller then some threshold value T, then in next iteration - recalculate your path, so there is no lag
otherwise - follow your path for at least 10-50 iterations before another path search
There are countless more optimizations, but we would need more details regarding your game as well as time you wish to spend on those optimizations.
I wrote an object tracker that will try to detect and follow a moving object in a recorded video. In order to maximize the detection rate, my algorithm is using a bunch of detection & tracking algorithms (cascade, foreground & particle tracker). Each tracking algorithm will return me some point of interest that might be part of the object that I'm trying to track. Let's assume (for the simplicity of this example) that my object is a rectangle and that the three tracking algorithms returned the points 1, 2 and 3:
Based on the relation / distance of these three points it is possible to calculate the center of gravity (blue X in above image) of the tracked object. So for each frame I might be able to come up with some good estimate of the center of gravity. However, the object might move from one frame to the next:
In this example I merely rotated the original object. My algorithm will give me three new points of interest: 1',2' and 3'. I could again calculate the center of gravity based on these three new points, but I would throw away important information that I've acquired from the previous frame: based on points 1, 2 and 3 I already do know something about the relationship of these points and thus by combining the information from 1, 2 and 3 and 1',2' and 3' I should be able to come up with a better estimate of the center of gravity.
Furthermore, the next frame might yield a forth data point:
This is what I would like to do (but I don't know how):
based on the individual points (and their relationship to each other) that are returned from the different tracking algorithms, I want to build up a localization map of the tracked object. Intuitively I feel like I need to come up with A) an identification function that will identify individual points across frames and B) some cost function that will determine how similar tracked points (and the relationship / distance between them) are from frame to frame, but I can't get my head around on how to implement this. Alternatively, maybe some kind of map buildup based on the points will work. But again, I don't know how to approach this.
Any advice (and example code) is highly appreciated!
EDIT1
a simple particle filter might probably work too, but I again don't know how to define the cost function. A particle filter for tracking a certain color is easy to program: for each pixel you calculate the difference between target color and pixel color. But how would I do the same for estimating the relationship between tracked points?
EDIT2 intuitively I feel like Kalman filters could also help with the prediction step. See slides 24 - 32 of this pdf. Or am I misled?
What I think you're trying to do is essentially build up a state space of features, which can be applied to a filtering process, such as an Extended Kalman Filter. This is a useful framework when you have multiple observations in every frame, and you're trying to estimate or measure something indicated by these observations.
To determine the similarity of the tracked points, you can perform simple template matching from frame to frame for small regions around the points. One way of doing this is to extract an NxN (say, 7x7) region around point a in frame n and point a' in frame n+1, followed by normalised cross correlation between the extracted regions. This will give you a reasonable measure of how similar the patches are. If the patches are not similar, then you've probably lost track of that point.
There is an enormous literature on this and related problems starting in the 80's. Try searching for "optical flow" algorithms". The input for such algorithms is two successive frames of the same scene. The output is a vector field, one vector per pixel in the second image, which shows what the direction and speed of movement of the feature in that field. This presentation is a pretty nice summary.
A nice thing about optical flow is that many algorithms for it parallelize nicely and map onto your favorite video card GPU, so they can run in real time. Think ESPN overlays.
According to me, in order to identify who is who in each frame, you will have to use a greater dimension. For example if you want to know which point is where between two frame (considering your extracted point are same), you will have to build vectors or simplex and then deduce an organisation between your points (like angles values).
The main problem is that combinations increase with point number. If your camera is a fixed point then, you could use background as a reference in order to deduce object rotations and translations, i mean build vectors between background interest points and object points in order to clearly identify them.
hope that help go forward.
I would recommend looking in to the divided difference filter (DDF), which is similar to the extended Kalman filter (EKF), but does not require an approximate model of the dynamics of your system (which you may not have). Basically the DDF approximates the derivatives used in the EKF using a difference equation. There are plenty of papers online about this, but I do not know whether you have access to them so I have not linked them here. If you are working from a university or a company that has access to online journals (like IEEE Explore), then just Google "divided difference filter" and check out some of the papers.