[Math] Point closest to a set four of lines in 3D

computational geometrygeometry

Given four lines in $3D$ (represented as a couple of points), I want to find the point in space which minimizes the sum of distances between this point and every line.

I'm trying to find a way to formulate this as a problem of Least Square, but I'm not quite sure as to how I should. I'm currently trying to use the definition of distance provided at: http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html

Any ideas?

Best Answer

As pointed out by the others, minimising the sum of distances is different from minimising the sum of squared distances. The former requires the use of an optimisation package. The latter is a least square problem that is fairly well studied with free solvers in abundance. Specifically:

  1. Let each straight line, say $L_i$, be specfied by two points $v_i$ and $w_i$ on it.
  2. Compute the unit vector $u_i = (v_i-w_i)/\Vert v_i-w_i\Vert$. This is the direction vector of $L_i$. The projection matrix $I-u_iu_i^T$ will project every vector $x\in\mathbb{R}^3$ to the plane $P_i$ that passes through the origin and orthogonal to $L_i$.
  3. In particular, the intersection of $L_i$ and $P_i$ is given by $p_i = (I-u_iu_i^T)w_i$ and the distance from a point $x\in\mathbb{R}^3$ to the line $L_i$ is $\Vert (I-u_iu_i^T)x - p_i\Vert$.
  4. Hence the sum of squared distances is $\sum_{i=1}^4 \Vert (I-u_iu_i^T)x - p_i\Vert^2$, which can be expanded into $x^T Ax - 2b^T x + c$ with $A = \sum_i(I-u_iu_i^T)$ (a 3x3 matrix), $b = \sum_i p_i$ (a 3-vector) and $c=\sum_i p_i^Tp_i$ (a scalar).
  5. Now come the pretty standard stuffs. The minimiser of $x^T Ax - 2b^T x + c$ is the least square solution to $Ax=b$, which in theory is $x = A^+ b$, where $A^+$ denotes the Moore-Penrose pseudoinverse of $A$.
  6. In practice, $A$ is usually nonsingular; in this case, $A^+$ is just $A^{-1}$. If you are sure that $A$ is nonsingular, you may simply invert the matrix and calculate $x$ as $A^{-1}b$ or (for better numerical stability) solve the equation by QR factorisation.
  7. However, $A$ can at times be singular or nearly singular. If you really want to play safe, you may solve the least square problem by applying singular value decomposition (SVD) or by using the conjugate gradient method. For C++ users, the Eigen3 library already contains a SVD solver. See the section "Least squares solving" in the official tutorial.