Finding the intersection point of many lines in 3D (point closest to all lines)

Question

I have many lines (let's say 4) which are supposed to be intersected. (Please consider lines are represented as a pair of points). I want to find the point in space which minimizes the sum of the square distances to all of the lines or in other words, the point which is closest to all the lines.

I want to formulate this as a Least Squares Problem, but I'm not quite sure how it would be. I found the way to compute the distance between line and point. So, I need help to go further.

(As I can't access for optimization packages, I want to implement this with least squares.)

Answer 1

In some degenerate cases there may be no such a one point (for instance, if all the lines are parallel). However there's a single solution in the general case.

I assume you're trying to solve a more general problem where all the lines are not required to intersect exactly (otherwise there's a much simpler solution than the least squares).

Derivation:

You say the every line is represented by two points. Let's rather work in the convention where a line is represented by one point and a direction vector, which is just a vector subtraction of those two points. That is, instead of describing a line by points $\mathbf{a}$ and $\mathbf{b}$ we'll describe it by a point $\mathbf{a}$ and a vector $\mathbf{d}$ whereas $\mathbf{d}=\mathbf{b}-\mathbf{a}$.

Our point (which we're trying to find) is $\mathbf{c}$.

The distance of this point to the line is:

$H=\frac{\|(\mathbf{c}-\mathbf{a})\times\mathbf{d}\|}{\|\mathbf{d}\|}$

Using identity $(\mathbf{a}\times\mathbf{b})\cdot(\mathbf{a}\times\mathbf{b})=\|\mathbf{a}\|^2\|\mathbf{b}\|^2-(\mathbf{a}\cdot\mathbf{b})^2$

we have:

$H^2=\frac{\|\mathbf{c}-\mathbf{a}\|^2\|\mathbf{d}\|^2-\|(\mathbf{c}-\mathbf{a})\cdot\mathbf{d}\|^2 }{\|\mathbf{d}\|^2}$

$H^2 = \|\mathbf{c}-\mathbf{a}\|^2-\frac{\|(\mathbf{c}-\mathbf{a})\cdot\mathbf{d}\|^2 }{\|\mathbf{d}\|^2}$

The square sum of the distances of the point $\mathbf{c}$ to all the lines is just the sum of the above expressions for all the lines. The problem is to minimize this sum. This sum depends on a variable $\mathbf{c}$ (which is actually 3 variables, the components of $\mathbf{c}$). This is a standard least squares problem, which generally has a single solution (unless there's a degeneracy).

Solving the least squares for this specific case.

Since we want find such a $\mathbf{c}$ that minimizes this sum, its derivative with regard to $\mathbf{c}$ should be zero. In other words:

$\frac{d(H^2)}{d\mathbf{c}}=2(\mathbf{c}-\mathbf{a})-2\mathbf{d}\frac{(\mathbf{c}-\mathbf{a})\cdot\mathbf{d}}{\|\mathbf{d}\|^2}$

$0=\sum_{i=0}^m{\mathbf{c}-\mathbf{a}^{(i)}-\mathbf{d}^{(i)}\frac{(\mathbf{c}-\mathbf{a}^{(i)})\cdot\mathbf{d}^{(i)}}{\|\mathbf{d}^{(i)}\|^2}}$

This gives 3 equations (since it's a vector equation) with 3 unknowns (components of $\mathbf{c}$).

Answer 2

In order to find the intersection point of a set of lines, we calculate the point with minimum distance to them. Each line is defined by an origin ${a}_{i}$ and a unit direction vector, ${n}_{i}$. The square of the distance from a point $p$ to one of the lines is given from Pythagoras:

$$ d_{i}^{2}={{\left[ \left| \left| p-{{a}_{i}} \right| \right| \right]}^{2}}-{{\left[ {{\left( p-{{a}_{i}} \right)}^{T}}*{{n}_{i}} \right]}^{2}}={{\left( p-{{a}_{i}} \right)}^{T}}*\left( p-{{a}_{i}} \right)-{{\left[ {{\left( p-{{a}_{i}} \right)}^{T}}*{{n}_{i}} \right]}^{2}} $$

Where ${\left(p-a_i\right)}^T*n_i$ is the projection of ($p-a_i)$ on the line i. The sum of distances to the square to all lines is:

$$ \underset{i}{\mathop \sum }\,d_{i}^{2}=\underset{i}{\mathop \sum }\,\left[ {{\left( p-{{a}_{i}} \right)}^{T}}*\left( p-{{a}_{i}} \right)-{{\left[ {{\left( p-{{a}_{i}} \right)}^{T}}*{{n}_{i}} \right]}^{2}} \right] $$

To minimize this expression, we differentiate it with respect to $p$.

$$ \underset{i}{\mathop \sum }\,\left[ 2*\left( p-{{a}_{i}} \right)-2* \right[{{\left( p-{{a}_{i}} \right)}^{T}}*{{n}_{i}}]*{{n}_{i}}]=0 $$ $$ \underset{i}{\mathop \sum }\,\left( p-{{a}_{i}} \right)=\underset{i}{\mathop \sum }\,\left[ {{n}_{i}}*{{n}_{i}}^{T} \right]*\left( p-{{a}_{i}} \right) $$

It results:

$$ [\underset{i}{\mathop \sum }\,\left[ {{n}_{i}}*{{n}_{i}}^{T}-I \right]]*p=\underset{i}{\mathop \sum }\,\left[ {{n}_{i}}*{{n}_{i}}^{T}-I \right]*{{a}_{i}} $$

Where $I$ is the identity matrix. This is a matrix $~S*p=C$, with solution $p={{S}^{+}}*C$ , ${{S}^{+}}~$, being the pseudo-inverse of $S$.

Straightforward implementation can be found on: http://www.mathworks.com/matlabcentral/fileexchange/37192-intersection-point-of-lines-in-3d-space

Answer 3

For each line i, let pⁱ be a point on the line and let uⁱ be a unit vector parallel to the line. Extend uⁱ to an orthonormal basis uⁱ, vⁱ, wⁱ (e.g., rotate the standard basis, use Gram–Schmidt, etc.). The overdetermined system of equations (in variable x) is, for all i,

(x - pⁱ) . vⁱ = 0
(x - pⁱ) . wⁱ = 0.

Solve using the usual methods.