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

closestpoint2dlines.py

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

# Create two or more lines in the form of a matrix [[x1,y1], [x2,y2]]
lines = []
lines.append(np.asmatrix([[4, 4], [4, -3]]))
lines.append(np.asmatrix([[3, 2], [10, 2]]))
lines.append(np.asmatrix([[3, 3], [6, 4]]))
lines.append(np.asmatrix([[8, -3], [5, 4]]))

# Create the unit normal vectors
unit_normals = []
t = np.matrix([[0, -1], [1, 0]])
for line in lines :
    x1 = np.asmatrix(line[0])
    x2 = np.asmatrix(line[1])
    unit_normals.append(t * (x2 - x1).transpose() / np.linalg.norm(x2 - x1))

# Create the sums
sum1 = sum2 = 0
for index, unit_normal in enumerate(unit_normals) :
    sum1 = sum1 + (unit_normal * unit_normal.transpose())
    sum2 = sum2 + (unit_normal * unit_normal.transpose() * np.asmatrix(lines[index][0]).transpose())

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

# Plot the lines and the nearest point
plt.plot(x[0], x[1], 'ro', color="green")
for line in lines :
    plt.plot([line[0,0], line[1,0]], [line[0,1], line[1,1]], 'go-', color="red")
plt.show()