I have multiple Lines in 3D space. Each Line is described by 2 points, a start point, and a stop point. I'm trying to find the Line that best 'fits' these Lines (minimize the distance between the new line, and the other Lines). Ideally, I'd also like to know the least-square distance of the new line to the other lines (or some other statistic for deciding whether it is a good fit or not).
Below is Code I've written so far that does what I want, but I feel there is a faster, analytical way to calculate it, and speed is extremely important for my application.
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize._minimize import minimize
import time
def min_line(FINAL_LINE,LINES):
sqr=0
x0,x1,y0,y1,z0,z1=FINAL_LINE
#for each original line, calculate the distance from it to the new line
for l in LINES:
p1,p2,dist=closestDistanceBetweenLines(l[0],l[1],np.array([x0,y0,z0]),np.array([x1,y1,z1]))
#add that distance to the total
sqr+=np.square(dist)
return sqr
def closestDistanceBetweenLines(a0,a1,b0,b1):
'''
https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
Return the closest points on each segment and their distance
'''
# Calculate denomitator
A = a1 - a0
B = b1 - b0
magA = np.linalg.norm(A)
magB = np.linalg.norm(B)
_A = A / magA
_B = B / magB
cross = np.cross(_A, _B);
denom = np.linalg.norm(cross)**2
# If lines are parallel (denom=0) test if lines overlap.
# If they don't overlap then there is a closest point solution.
# If they do overlap, there are infinite closest positions, but there is a closest distance
if not denom:
d0 = np.dot(_A,(b0-a0))
# Segments overlap, return distance between parallel segments
return [],[],np.linalg.norm(((d0*_A)+a0)-b0)
# Lines criss-cross: Calculate the projected closest points
t = (b0 - a0);
detA = np.linalg.det([t, _B, cross])
detB = np.linalg.det([t, _A, cross])
t0 = detA/denom;
t1 = detB/denom;
pA = a0 + (_A * t0) # Projected closest point on segment A
pB = b0 + (_B * t1) # Projected closest point on segment B
return pA,pB,np.linalg.norm(pA-pB)
if __name__ == '__main__':
#setup the 3d figure for plotting
fig = plt.figure()
ax = plt.axes(projection='3d')
#4 sets of start/stop points forming 4 lines
a_start=np.array([0, 0, 0])
a_end=np.array([0.1, 1, 1])
b_start=np.array([0, 2, 0])
b_end=np.array([-0.1, 1, 1])
c_start=np.array([1, 0, 0])
c_end=np.array([.9, 1, 1])
d_start=np.array([1, 2, 0])
d_end=np.array([1.1, 1, 1])
e_start=np.array([1, 0, 0])
e_end=np.array([.9, 1, 2])
f_start=np.array([1, 2, 0])
f_end=np.array([1.1, 1, 2])
#turn them into a single Numpy array
a=[a_start,a_end]
b=[b_start,b_end]
c=[c_start,c_end]
d=[d_start,d_end]
e=[e_start,e_end]
f=[f_start,f_end]
all_points=np.array([a,b,c,d])
if(len(all_points)>2):
rets=minimize(min_line,[0,0,0,1,1,1], all_points)
print(rets)
#plot the original lines Green
for p in all_points:
ax.plot3D([p[0][0],p[1][0]],[p[0][1],p[1][1]],[p[0][2],p[1][2]],'g')
#plot the new, fit line Black
ax.plot3D([rets.x[0],rets.x[1]],[rets.x[2],rets.x[3]],[rets.x[4],rets.x[5]],'k')
plt.show()
Each Lines Starting point will always remain the same. The Starting points will never be the same as each other (they are all coplanar though). There will always be at least 3 Starting Lines.
