Last active
May 18, 2021 14:20
-
-
Save rabssm/56bab95f250ad403a62b610cd5bfc73b to your computer and use it in GitHub Desktop.
Find a point that is mutually closest to two or more 3d lines in a least-squares sense. Python code.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# 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" | |
Author
rabssm
commented
Sep 1, 2018
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment