We have 3d system with the source is sending signal and four receivers are receiving it. We know the coordinate location of these four receivers. We have the Time difference of arrival. Could you tell us how to calculate the location of the source.
Your problem is actually almost the same as the GPS navigation system. There you have four transmitters at known locations and one receiver in an unknown location, but the principle is exactly the same.
You get a system of four equations:
|p_1 - p_x| = c * (t + t_1)
|p_2 - p_x| = c * (t + t_2)
|p_3 - p_x| = c * (t + t_3)
|p_4 - p_x| = c * (t + t_4)
where p_x is the position vector of the transmitter and p_n the position vectors of the receivers. t is an unknown (even though it can be solved), t_n are the time differences to each receiver. |v| is the norm of vactor v (length, i.e. distance between stations). c is the speed of light.
There are different ways of solving this group of equations. You might want to have a look at any textbook or web resource of GPS mathematics.
If you know the locations of all four receivers, then you can draw circles around them, with each circle having a radius proportional to the time it took the signal to reach that receiver. Where those four circles intersect is the position of the transmitter.
It's a little bit more complicated than that because two circles will intersect in two positions. So you'll need the ability to discard one of the two points from the first circle intersection.
If the circles don't exactly intersect at one common point, you'll need some fuzzy matching to get the intersection point.
See https://math.stackexchange.com/questions/39561/finding-the-intersecting-points-on-two-circles for information on how to compute circle intersection.
Related
The program uses vertices in R^2 as nodes and there are edges between nodes, and ultimately more is built from there. There are a high number of circuitous ways that a point (x,y) in R^2 might be reached that may rely on layer after layer of trig functions. So it makes sense to define one vertex as the canonical vertex for all points in a square with side length 2*epsilon centered at the point. So various calculations happen, out comes a point (x,y) where I wish to put a vertex, a vertex factory checks to see if there is already vertex deemed as canonical that should be used for this point, if so it returns that vertex, if not a new vertex is created with the coordinates of the point and that vertex is now deemed canonical. I know this can lead to ambiguities given the possibility for overlap of the squares but that is immaterial for the moment, epsilon can be set to make the probability of such a case vanishingly small.
Clearly a list of canonical vertices must be kept.
I have working implementations using List<Vertex> and HashSet<Vertex>, however the vertex creation process seems to scale poorly when the number of vertices grows to over over 100,000 and incredibly poorly if the number gets anywhere near 1,000,000.
I have no idea how to efficiently implement the VertexFactory. Right now Vertex has method IsEquivalentTo(Vertex v) and returns true if v is contained in the square around the instance vertex calling it, false otherwise.
So the the vertex creation process looks like this:
Some point (x,y) get calculated and requests a new vertex from the vertex manager.
VertexManager creates a temporary vertex temp then uses a foreach to iterate over every vertex in the container using IsEquivalentTo(temp) to find a match and return it, if no match is found then add temp to the container and return it. I should state that if a match is found obviously it breaks out of the foreach loop.
I may be way off but my first guess would be put an order on the vertices such as
v1 < v2 iff ( (v1.X < v2.X) || ( (v1.X == v2.X) && (v1.Y < v2.Y) ) )
and then store the vertices in a sorted container. But to be honest I do not know enough about about the various containers to know which is the most appropriate for the purpose, standard C# best practices, etc
Edit:
I cannot mark the comment as an answer so thanks to GSerg whose comment guided me to kd-trees. This is exactly what I was looking for.
Thanks
I'm working on a personal project involving Bezier curves. I have seen many implementations that handle quadratic and cubic curves individually, but I'm looking to create a more generalized algorithm where I can add and remove control points by increasing and decreasing the degree(order) of the curve.
This is not a part of my main question, but if anyone knows an example of a generalized algorithm that I can look at, I will be grateful if they can point me that direction.
First of all, I understand that any conversion from a low order to a high order curve is an approximation, and not equivalent. I am satisfied with computationally "close enough".
Secondly, I also understand that there is a loss of information when stepping down from a higher order to a lower order curve. This is unavoidable as the higher order curve has more "bends" in the curve that the lower order curve simply cannot approximate.
I'm fine with these limitations as it is necessary to be able to add and subtract control points within the curve for desired functionality.
My first question is related to this one asked approximately 5 years ago:
Convert quadratic curve to cubic curve
A quadratic curve has three (3) control points:
Vector3 Start;
Vector3 Control1;
Vector3 End;
The conversion to a cubic we introduce a fourth control point...
Vector3 Control2;
..."Between" Control1 and End. We then set Control1 and Control2 accordingly:
Control2 = new Vector3(
End.x + ((2.0f / 3.0f) * (Control1.x - End.x)),
End.y + ((2.0f / 3.0f) * (Control1.y - End.y)),
End.z + ((2.0f / 3.0f) * (Control1.z - End.z))
);
Control1 = new Vector3(
Start.x + ((2.0f / 3.0f) * (Control1.x - Start.x)),
Start.y + ((2.0f / 3.0f) * (Control1.y - Start.y)),
Start.z + ((2.0f / 3.0f) * (Control1.z - Start.z))
);
I am not certain that this is correct. In the example, only the 'x' component was set. I merely extrapolated 'y' and 'z' from it. If this is not correct, then I'd appreciate knowing what is correct.
This example only covers converting from quadratic to cubic curves. The controlling value appears to be the (2.0f/3.0f) in setting the coordinates. So, does that mean that converting from cubic to quartic would be (2.0f/4.0f) or (3.0f/4.0f) or something else entirely? What is the properly generalized function for this conversion for an arbitrary order of curve?
My project is also going to be working with splines. I am using an edge to edge method for constructing my splines where an edge is defined as an arbitrarily ordered curve from 1 (a line) to n, where the first and last Vector3 in the list of control points are the start and end points of the edge, and connecting edges share the end point of the previous edge with the start point of the next.
Unlike Bezier Curves, I'm not adding and subtracting control points. Instead, I'm dividing and combining edges. What would be a good method for subdividing a curve into two lower order curves with an order no lower than 2 approximating the original curve? Likewise, what would be a good method for combining two curves into a single high order curve?
I realize this is a lot to ask in a single post, but I felt it better to keep it together in one topic rather than dividing it up.
Thanks for any help!
You'll want to read over http://pomax.github.io/bezierinfo/#reordering for how to raising a curve to a higher order, which is almost trivial:
That may look scary, but if you actually look at what each of the three terms is, all three are stupidly simple bits of grade school maths. You typically just want the new weights, so generating those as a new list of coordinates is absolutely trivial and written in about a minute. In Javascript:
function getOneHigher(x, y):
k = x.length;
s = k - 1;
let nx = [], ny =[];
nx[0] = x[0]; nx[k] = x[s];
ny[0] = y[0]; ny[k] = y[s];
for (let i=1; i<k; i++) {
nx[i] = ((k-i)*x[i] + i*x[i-1]) / k;
ny[i] = ((k-i)*y[i] + i*y[i-1]) / k;
}
return {nx, ny};
}
And we're done. Calling getOneHigher(x, y) will yield two arrays of new coordinates for the same curve, represented as a Bezier curve of a degree one higher than we put in. Note that what this means is that we've kept the set of curve coordinates the same (literally: the functions are identical ) while reducing the tangents at each coordinate (because the derivatives are not identical), effectively "slowing down" anything that travels the path that this curve traces.
You'll then also want to follow the link to Sirver's Castle in that section which explains how to do the lossy conversion down in a way that minimizes loss of information. And this should have been multiple questions: this is a Q&A site, future visitors can't find answers to specific questions if you don't ask specific questions but instead combine several technically unrelated ones in a single post.
I'm trying to find the fastest and easiest way in a C# program to calculate the intersections of two circles. From what I can tell there are two possible methods, and you'll have to forgive me for not knowing the official names for them.
We're assuming you know the center points for both circles and their exact radii, from which you can calculate the distance between them, so all that is missing are the point(s) of intersection. This is taking place on a standard x-y plot.
The first is a kind of substitution method like the one described here where you combine the two circle formulas and isolate either x or y, then sub it back in to an original formula to end up with a quadratic equation that can be solved for two (or possibly one or none) coordinates for an axis, which then lets you find the corresponding coordinates on the other axis.
The second I have seen a reference to is using a Law of Cosines method to determine the angles, which would then let you plot a line for each side on the grid, and put in your radius to find the actual intersection point.
I have written out the steps for the first method, and it seems rather lengthy. The second one is going to take some research/learning to write out but sounds simpler. What I've never done is translate processes like this into code, so I don't know ultimately which one will be the easiest for that application. Does anyone have advice on that? Or am I perhaps going about the the complete wrong way? Is there a library already out there that I can use for it instead of reinventing the wheel?
Some context: I'm worried mainly about the cost to the CPU to do these calculations. I plan on the application doing a heck of a lot of them at once, repeatedly, hence why I want the simplest way to accomplish it.
Computational geometry is almost always a pain to implement. It's also almost always quite CPU-intensive. That said, this problem is just algebra if you set it up right.
Compute d = hypot(x2-x1, y2-y1), the distance between the two centres. If r1 + r2 < d, there is no intersection. If r1+r2 == d, the intersection is at (x1, y1) + r1/(r1+r2) * (x2-x1,y2-y1). If d < abs(r1-r2), one circle is contained in the other and there is no intersection. You can work out the case where the two circles are tangent and one is contained in the other. I will only deal with the remaining case.
You want to find distances h orthogonal to (x2-x1,y2-y1) and p parallel to (x2-x1,y2-y1) so that p^2 + h^2 = r1^2 and (d-p)^2 + h^2 = r2^2. Subtract the two equations to get a linear equation in p: d^2-2dp = r2^2-r1^2. Solve this linear equation for p. Then h = sqrt(r1^2 - p^2).
The coordinates of the two points are (x1,y1) + p (x2-x1,y2-y1) / d +/- h (y2-y1, x1-x2) / d. If you work through the derivation above and solve for p/d and h/d instead, you may get something that does fewer operations.
I am trying to minimize the difference between sets of square markers in 3d space with a set of unknown parameters.
I have a model set of these square markers (represented by 3d position and rotation) which should at the end of optimization match up with a set of observed square markers.
I am using Levenberg–Marquardt to optimize the set of unknown parameters, these parameters will alter the position and rotation of the model 3d markers until they match (more or less) with the observed 3d marker positions.
The observed 3d markers come from a computer vision marker detection algorithm. It gives the id of the markers seen in each frame and the transformation from the camera of each marker (using Coplanar posit). Each 'frame' would only be able to see a small number of markers in the total set of markers, there will also be inaccuracies in the transformation.
I have thought of how to construct my minimization function and I thought to try to compare the relative rotations and minimize the difference between the rotations in each iteration of the LM optimisation.
Essentially:
foreach (Marker m1 in markers)
{
foreach (Marker m2 in markers)
{
Vector3 eulerRotation = getRotation(m1, m2);
ObservedMarker observed1 = getMatchingObserved(m1);
ObservedMarker observed2 = getMatchingObserved(m2);
Vector3 eulerRotationObserved = getRotation(observed1, observed2);
double diffX = Math.Abs(eulerRotation.X - eulerRotationObserved.X);
double diffY = Math.Abs(eulerRotation.Y - eulerRotationObserved.Y);
double diffZ = Math.Abs(eulerRotation.Z - eulerRotationObserved.Z);
}
}
Where diffX, diffY and diffZ are the values to be minimized.
I am using the following to calculate the angles:
Vector3 axis = Vector3.Cross(getNormal(m1), getNormal(m2));
axis.Normalize();
double angle = Math.Acos(Vector3.Dot(getNormal(m1), getNormal(m2)));
Vector3 modelRotation = calculateEulerAngle(axis, angle);
getNormal(Marker m) calculates the normal to the plane that the square marker lies on.
I am sure I am doing something wrong here though. Throwing this all into the LM optimiser (I am using ALGLib) doesn't seem to do anything, it goes through 1 iteration and finishes without changing any of the unknown parameters (initially all 0).
I am thinking that something is wrong with the function I am trying to minimize over. It seems sometimes the angle calculated (3rd line) returns NaN (I am currently setting this case to return diffX, diffY, diffZ as 0). Is it even valid to compare the euler angles as above?
Any help would be greatly appreciated.
Further information:
Program is written in C#, I am using XNA as well.
The model markers are represented by its four corners in 3D coords
All the model markers are in the same coordinate space.
Observed markers are the four corners as translations from the camera position in camera coordinate space
If m1 and m2 markers are the same marker id or if either m1 or m2 is not observed, I set all the diffs to 0 (no difference).
At first I thought this might be a typo, but then I realized that this could be a bug, having been a victim of similar cases myself in the past.
Shouldn't diffY and diffZ be:
double diffY = Math.Abs(eulerRotation.Y - eulerRotationObserved.Y);
double diffZ = Math.Abs(eulerRotation.Z - eulerRotationObserved.Z);
I don't have enough reputation to post this as a comment, hence posting it as an answer!
Any luck with this? Is it correct to assume that you want to minimize the "sum" of all diffs over all marker combinations? I think if you want to use LM you should not use Math.Abs.
One alternative would be to formulate your objective function manually and use another optimizer. I have recently ported two non-linear optimizers to C# which do not even require you to compute derivatives:
COBYLA2, supports non-linear constraints but require more iterations.
BOBYQA, limited to variable bounds constraints, but provides a considerable more efficient iteration scheme.
In one of mine applications I am dealing with graphics objects. I am using open source GPC library to clip/merge two shapes. To improve accuracy I am sampling (adding multiple points between two edges) existing shapes. But before displaying back the merged shape I need to remove all the points between two edges.
But I am not able to find an efficient algorithm that will remove all points between two edges which has same slope with minimum CPU utilization. Currently all points are of type
PointF
I am calculating slope using following function
private float Slope(PointF point1, PointF point2)
{
return (point2.Y - point1.Y) / (point2.X - point1.X);
}
Any pointer on this will be a great help.
What algorithm are you currently using? I can think only of going through all point and for each 3 to check wherher middle point is on vector (or close to) defined by 2 other points.
Do you need math for that operation?
Just to be clear, you have three points A = (a,b), C = (c,d), and E = (e,f), and are wondering if the segment AE goes through C and thus you can replace the pair of segments AC and CE with the single segment AE?
slope AC = (d-b)/(c-a) = slope CE = (f-d)/(e-c)
multiply through by the denominators, you get
(d-b)(e-c) = (f-d)(c-a)
that's just four subtracts, two multiplies, and a compare. You'll need to do the comparison with some error tolerance due to the use of floating point.
Well.. I found the solution for my question. Instead of using Sampling method provided by SDK, I created my own sampling method which insert a point between two points at a fixed distance. This reduces the number of point that I need to process and in turn reducing processor usage.