这是使用 numpy 和 scipy 的解决方案。scipy 主要用于生成随机旋转，除了易于自己编码的 scipy.linalg.norm。从 numpy 中使用的主要东西是叉积和矩阵乘法，这也很容易自己编写代码。
基本思想是这样的：给定三个不共线的点 x1,x2,x3，可以找到向量（轴）v1,v2,v3 的正交三元组，其中 v1 在平面中的 x2-x1,v2 方向上由 (x2-x1) 和 (x3-x1) 跨越，并且 v3 完成了三重奏。
点 y1,y2,y3 相对于 x1,x2,x3 旋转和平移。从 y1,y2,y3 生成的轴 w1,w2,w3 从 v1,v2,v3 旋转（即不平移）。这两组三元组都是正交的，因此很容易从中找到旋转：R = W * transpose(V)
一旦我们有了旋转，找到平移很简单：y1 = R*x + t，所以t = y1 - R*x。使用最小二乘求解器并结合所有三个点来估计 t 可能会更好。
import numpy
import scipy.linalg
def rand_rot():
"""Return a random rotation
Return a random orthogonal matrix with determinant 1"""
q, _ = scipy.linalg.qr(numpy.random.randn(3, 3))
if scipy.linalg.det(q) < 0:
# does this ever happen?
print "got a negative det"
q[:, 0] = -q[:, 0]
return q
def rand_noncollinear():
"""Return 3 random non-collinear vectors"""
while True:
b = numpy.random.randn(3, 3)
sigma = scipy.linalg.svdvals(b)
if sigma[2]/sigma[0] > 0.1:
# "very" non-collinear
break
# "nearly" collinear; try again
return b[:, 0], b[:, 1], b[:, 2]
def normalize(a):
"""Return argument normalized"""
return a/scipy.linalg.norm(a)
def colstack(a1, a2, a3):
"""Stack three vectors as columns"""
return numpy.hstack((a1[:, numpy.newaxis],
a2[:, numpy.newaxis],
a3[:, numpy.newaxis]))
def get_axes(a1, a2, a3):
"""Generate orthogonal axes from three non-collinear points"""
# I tried to do this with QR, but something didn't work
b1 = normalize(a2-a1)
b2 = normalize(a3-a1)
b3 = normalize(numpy.cross(b1, b2))
b4 = normalize(numpy.cross(b3, b1))
return b1, b4, b3
# random rotation and translation
r = rand_rot()
t = numpy.random.randn(3)
# three non-collinear points
x1, x2, x3 = rand_noncollinear()
# some other point
x4 = numpy.random.randn(3)
# the images of the above in the transformation.
# y4 is for checking only -- won't be used to estimate r or t
y1, y2, y3, y4 = [numpy.dot(r, x) + t
for x in x1, x2, x3, x4]
v1, v2, v3 = get_axes(x1, x2, x3)
w1, w2, w3 = get_axes(y1, y2, y3)
V = colstack(v1, v2, v3)
W = colstack(w1, w2, w3)
# W = R V, so R = W * inverse(V); but V orthogonal, so inverse(V) is
# transpose(V):
rfound = numpy.dot(W, V.T)
# y1 = R x1 + t, so...
tfound = y1-numpy.dot(r, x1)
# get error on images of x2 and x3, just in case
y2err = scipy.linalg.norm(numpy.dot(rfound, x2) + tfound - y2)
y3err = scipy.linalg.norm(numpy.dot(rfound, x3) + tfound - y3)
# and check error image of x4 -- getting an estimate of y4 is the
# point of all of this
y4err = scipy.linalg.norm(numpy.dot(rfound, x4) + tfound - y4)
print "y2 error: ", y2err
print "y3 error: ", y3err
print "y4 error: ", y4err
