I'm working on a problem at my job, where I am trying to estimate the locations of N points in 3D space, given the distances between pairs of points. Ideally all $N(N+1)/2$ distances would be known, although it could be less, though I don't expect it would be by much. Each point is guaranteed to know the distance to at least 4 other points.
Here's what I have so far:
In order to remove the ambiguity of global orientation (which can be solved for later using a method external to this problem), the first point, P1, is fixed at the origin, and the next 3 are placed in the first octant. For simplicity, P2 is placed on the positive x-axis, P3 in the positive x-y plane, and P4 in the positive x-y-z octant. This fixes 6 variables (x1, y1, z1, y2, z2, z3 all equal zero), and constrains 6 more (x2, x3, y3, x4, y4, z4 all greater than zero). The points P1-P4 could be solved exactly for using multilateration, but I don't know if that is better or worse than including them in the system of equations.
If I know $M \le \frac{N(N+1)}{2}$ distances, then I can set up a system of M x 3N equations of the form $f_{mn} = (x_m - x_n)^2 + (y_m - y_n)^2 + (z_m - z_n)^2 - d_{mn}^2$ . If $M = 3N$, then I am familiar with solving this through the Newton-Raphson method using the Jacobian. However, M is expected to almost always be greater than 3N. Using the constraints mentioned in the previous point, I can reduce this down to something like $M - 6 \times 3N - 12$, or somewhere in between, but again I'm not certain whether or not that is helpful.
Here's what I am trying to figure out:
Is this the sort of problem I could solve with an iterative method? I am aware of the Newton-Raphson method for an exactly constrained system, but I do not know what to do about an overconstrained system or one with variable constraints. I am an engineer more than a mathematician, so it is slightly hard to find the right sorts of resources on these things. If an iterative method isn't appropriate, there is a search method I could use, where I would perform multilateration on successive points, but I would prefer not to resort to this, as it can be tricky to programmatically determine points that are least likely to be coplanar, which would propagate large errors.
Is there information on what sort of errors I can expect? Ideally, is there information on positional error as a function of uncertainty in the distance between points? This is something I could determine after the fact, but if there is literature already, then that would be great.
Thanks for any help! I really appreciate it
The typical approach to overconstraint systems of equations is least squares approach. Instead of solving $f_{nm} = 0$ the least squares method minimizes $$ J = \sum_{(n, m) \in \mathcal M} f_{nm}^2 \to \min_{x_i, y_i, z_i}. (*) $$ Here I've denoted by $\mathcal M$ all the point pairs with known distance between them. You might take a look at Levenberg–Marquardt algorithm, it was designed for generic nonlinear least-squares problem.
Sadly, this might not work for big N, or may require very close initial guess. The method might find a local optimum (just like in case of $M = 3N$ the Newton's method might hit a singular jacobian and terminate without a solution).
If you know most of the pairwise distances, then you may try to solve the problem in a flood fill manner. Let $K$ be the set of points with known coordinates. On the start $K = \{1, 2, 3, 4\}$. Then you repeatedly choose a point $q$ that has most known distances to points from $K$. If there are many of such points, take the closest (e.g. by sum of the distances) to $K$. Solve the problem for $(x_q, y_q, z_q)$ (in a least-squares fashion) and add $q$ to $K$. Repeat until no more points left.
If this algorithms succeeds to find all points' coordinates you may refine them further by taking them as initial guess for global optimization problem (*).
I made a proof-of-concept Jupyter notebook with this algorithm. I'm not sure if it works all the time, but in various random cases it recovers original points pretty well (up to rigid motion and distance perturbation, of course). Sometimes, locating point by known points gave incorrect result (local optimum problems, as I said before) but choosing initial guess as average of known points seemed to fix that.
Here's the link to the code https://gist.github.com/uranix/c5e5e6db687eda0d12ffa4fa570bc3b4