Если это просто перевод и вращение, то это преобразование, известное как аффинное преобразование .
Это в основном принимает форму:
secondary_system = A * primary_system + b
, где A - это матрица 3x3 (поскольку вы находитесь в 3D), а b - это перевод 3x1.
Это может быть эквивалентно написано
secondary_system_coords2 = A2 * primary_system2,
, где
secondary_system_coords2 - это вектор [secondary_system,1], primary_system2 - это вектор [primary_system,1], а A2 - это матрица 4x4:
[ A b ]
[ 0,0,0,1 ]
(см. Вики-страницу для получения дополнительной информации).
Итак, по сути, вы хотите решить уравнение:
y = A2 x
для A2, где y состоит из точек от secondary_system с 1, застрявшим на конце, и x - от primary_system с 1, застрявшим на конце, а A2 - 4x4 матрица.
Теперь, если x - квадратная матрица, мы можем решить ее следующим образом:
A2 = y*x^(-1)
Но x - это 4x1. Однако вам повезло, и у вас есть 4 наборов x с 4 соответствующими наборами y, так что вы можете создать x, равный 4x4, например:
x = [ primary_system1 | primary_system2 | primary_system3 | primary_system4 ]
, где каждый из primary_systemi является вектором столбца 4x1. То же самое с y.
Если у вас есть A2, чтобы преобразовать точку из системы 1 в систему 2, вы просто делаете:
transformed = A2 * point_to_transform
Вы можете настроить это (например, в numpy) следующим образом:
import numpy as np
def solve_affine( p1, p2, p3, p4, s1, s2, s3, s4 ):
x = np.transpose(np.matrix([p1,p2,p3,p4]))
y = np.transpose(np.matrix([s1,s2,s3,s4]))
# add ones on the bottom of x and y
x = np.vstack((x,[1,1,1,1]))
y = np.vstack((y,[1,1,1,1]))
# solve for A2
A2 = y * x.I
# return function that takes input x and transforms it
# don't need to return the 4th row as it is
return lambda x: (A2*np.vstack((np.matrix(x).reshape(3,1),1)))[0:3,:]
Тогда используйте это так:
transformFn = solve_affine( primary_system1, primary_system2,
primary_system3, primary_system4,
secondary_system1, secondary_system2,
secondary_system3, secondary_system4 )
# test: transform primary_system1 and we should get secondary_system1
np.matrix(secondary_system1).T - transformFn( primary_system1 )
# np.linalg.norm of above is 0.02555
# transform another point (x,y,z).
transformed = transformFn((x,y,z))
Примечание: Конечно, здесь есть числовая ошибка, и это, возможно, не лучший способ решения для преобразования (вы можете сделать что-то вроде наименьших квадратов).
Кроме того, ошибка преобразования primary_systemx в secondary_systemx имеет (для этого примера) порядка 10 ^ (- 2).
Вы должны будете рассмотреть, является ли это приемлемым или нет (оно кажется большим, но оно может быть приемлемым по сравнению с вашими входными точками, которые имеют порядок 10 ^ 6).