-1

This problem has had me stumped for days.

I have a group of lines formed from some data that produces 3D lines of the form:

P = a + dt

Where a is a position vector and d is the unit direction vector.

So basically I want to find the nearest point to all of these lines, using a least squares fit. I've been unable to find an algorithm online or how to implement it in Java. Im using the apache commons math library using Vector3D or RealVectors to calculate the line equations. So any help on an algorithm or example code to solve this problem would be helpful.

jameslfc19
  • 1,084
  • 1
  • 10
  • 14
  • look up "least squares" - in essence an extended phythagoras. https://en.wikipedia.org/wiki/Least_squares – kai Mar 10 '19 at 00:46
  • @kai I understand the concept of least squares im not sure how to implement it in this case with 3 dimensional vectors and matrices – jameslfc19 Mar 10 '19 at 00:48
  • Thats actually how the least squares differs from pythagoras - distance of to points in 2D : d= sqrt(x²+y²) and least squares is distance in N-D d = sqrt(x1²+x2²+x3² +... + xn²) so for 3D d= sqrt(x²+y²+z²). For a minimum the sqrt makes no difference. Now you put the distance of your lines into the formula.. – kai Mar 10 '19 at 01:13
  • There is another way to explain least squares, In 3D: You look for the sphere with the smalles radius that hits all your lines. The center of this sphere is your sought point. - You end up with the same equation as with sqrt(x²+y²+z²) you calculate a radius of a sphere. And again for minimization the unit makes no difference, so you can use x²+y²+z² or sqrt( x²+y²+z²) with or without the sqrt: same result for the point. An yes Matrices and Vectors make live easier if you are used to them. Otherwise don't - no need to use them. They are just a different way of representation. – kai Mar 10 '19 at 13:49
  • A last hint: what happens if you have two parallel lines? How many points fullfill your criteria? Take that into account! Maybe it helps for imagination that lines can be parallel, scew and crossing. – kai Mar 10 '19 at 15:19
  • @kai: you also confuse least squares and maxmin, but never mind. –  Mar 10 '19 at 19:51

2 Answers2

1

Assuming that you want to minimize the sum of squared distances to the lines, and assuming WLOG the vectors d to be unit, the total squared distance is

Σ ((ap)² - (ap.d)²)

where the sum is take over all (a, d) lines.

The gradient of this expression is

Σ (2 ap - 2 (ap.d) d)

and by canceling it we get a linear system in p.

  • Okay, that makes sense but how do you compute the vector (ap)? The cross product between the position vector and the line vector? Because p has the arbitrary constant t, how does that cancel out? – jameslfc19 Mar 10 '19 at 22:19
  • @jameslfc19: do you see any t in those equations ? This is a simple 3x3 linear system. –  Mar 11 '19 at 00:18
  • the p vector is of the form a + dt? Isn’t it? – jameslfc19 Mar 11 '19 at 00:19
  • @jameslfc19 `ap = (p-a)`, the vector from one line point to the solution point – coproc Mar 11 '19 at 07:51
  • @jameslfc19: there is no t in my solution. –  Mar 11 '19 at 08:22
  • Okay so how are you supposed to solve for p in this scenario, after setting the gradient to 0? – jameslfc19 Mar 11 '19 at 15:24
  • @jameslfc19: like any 3x3 linear system, write Ap=b and use Cramer for instance. –  Mar 11 '19 at 15:31
  • Feel like I’m being really stupid but how do I factor out the b vector in this case then? https://imgur.com/a/ja9dWdS – jameslfc19 Mar 11 '19 at 23:22
1

Extended version of @YvesDaoust answer:

Given the parametric line:

L(t) = A + t D

Where D is a unit vector i.e. D • D = 1

The distance of a point P to a line L is:

t = D • (P - A)

The closest point P to a set of lines, in the least squares sense, is minimizing the Sum of errors E:

E = (P - L(t))^2

E = (P - (A + tD))^2

E = (P - A - tD)^2

E = (P - A - (D • (P - A))D)^2

E = (P - A - (D•P - D•A)D)^2

E = (P - A - (D•P)D + (D•A)D)^2

E = (P - (D•P)D - A + (D•A)D)^2

E = (P - (D D^T)P - A + (D•A)D)^2

E = ((I - (D D^T)) P - A + (D•A)D)^2

E = (C x - b)^2

Where 

C = I - (D D^T)
x = P
b = A - (D•A)D

Where "I" is the 3x3 identity matrix. The least squares solution is:

C^T C x = C^T b

Other way to derive the same system is taking the derivative of equation:

E = (P - A - tD)^2

E = P•P - 2 P•A - 2t P•D + A•A + 2t A•D + t^2

Taking the derivative with respect to P and simplifying we get (remember to apply the chain rule when taking the derivative of "t" since it is a function of P):

dE/dP = 2(P - A  - ((P - A) • D) D)

Equating this to zero:

dE/dP = 0

2(P - A  - ((P - A)•D) D) = 0

P - ((P - A)•D) D = A

P - (P•D) D + (A•D) D = A

P - (P•D) D = A - (A•D) D

P - (D D^T) P = A - (A•D) D

(I - D D^T) P = A - (A•D) D

Where "I" is the 3x3 identity matrix. That is again a linear system of the form C x = b.

Remember that you still need to apply the summation to both sides of the equation, so it really is:

Sum(C^T C) x = Sum(C^T b)

So finally:

x = Sum(C^T C)^-1 Sum(C^T b)
Mauricio Cele Lopez Belon
  • 1,422
  • 1
  • 7
  • 9