Find a point that is mutually closest to two or more 3d lines in a least-squares sense. Python code.

closestpoint3dlines.py

# Find a point that is mutually closest to two or more 3d lines in a least-squares sense.
# Based on: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#In_three_dimensions_2
import numpy as np

# Create two or more 3d lines in the form of a matrix [[x1,y1,z1], [x2,y2,z2]]
lines = []
lines.append(np.asmatrix([[ 4, 0,0], [1,1,4]]))
lines.append(np.asmatrix([[-3,-2,0], [4,2,3]]))
lines.append(np.asmatrix([[ 0, 0,0], [2,2,6]]))
lines.append(np.asmatrix([[ 1, 2,0], [2,0,6]]))

# Create the unit normal vectors and calculate the sums
sum1 = sum2 = 0
for line in lines :
    v = (line[1] - line[0]).transpose() / np.linalg.norm(line[1] - line[0])
    unit_normal = np.identity(3) - (v * v.transpose())
    sum1 += (unit_normal * unit_normal.transpose())
    sum2 += (unit_normal * unit_normal.transpose() * np.asmatrix(line[0]).transpose())

# Calculate the nearest point
x = np.linalg.inv(sum1) * sum2
print "Closest point to lines:"
print x


# Plot the lines and the mutually closest point
try:
    from mpl_toolkits.mplot3d.axes3d import Axes3D
    import matplotlib.pyplot as plt
    
    fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
    ax.set_xlabel("X")
    ax.set_ylabel("Y")
    ax.set_zlabel("Z")
    ax.scatter(x[0].A1, x[1].A1, x[2].A1, color="green")
    for line in lines :
        tline = line.transpose()
        ax.plot(tline[0].A1, tline[1].A1, tline[2].A1, color="red")
    plt.show()
except:
    print "Unable to plot"