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

Solution 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}$).

Solution 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