numpy ：在 3D 欧几里得坐标中找到 2 组 4 个点的变换矩阵

Question

我正在尝试求解变换矩阵 M（以其数值形式）。源点是已知的 (s1,s2,s3,s4) 目标点也是已知的 (d1,d2,d3,d4)。来自金字塔的点。 转换是任意的，并将每个点 sn 映射到其目标 dn。

显然 s1 到 s4 和 d1 到 d4 是已知的并且使用数值，为了清楚起见只是这样呈现它们

s1  = np.matrix([[s1x],[s1y],[s1z],[0]])
s2 = np.matrix([[s2x],[s2y],[s2z],[0]])
s3 = np.matrix([[s3x],[s3y],[s3z],[0]])
s4 = np.matrix([[s4x],[s4y],[s4z],[0]])

d1  = np.matrix([[d1x],[d1y],[d1z],[0]])
d2 = np.matrix([[d2x],[d2y],[d2z],[0]])
d3 = np.matrix([[d3x],[d3y],[d3z],[0]])
d4 = np.matrix([[d4x],[d4y],[d4z],[0]])

对于每个点，dn = M x sn 与：

M = np.matrix([4,4])

一般公式似乎在这里：https://en.wikipedia.org/wiki/Transformation_matrix#Finding_the_matrix_of_a_transformation

我基本上是尝试将其以代码形式呈现，如果可能的话降低复杂性以便使用 numpy linalg 求解器。

我检查了其他线程，我的问题实际上是关于一般情况的。 （最少需要 4 个点）

Answer 1

也许我遗漏了一些东西，但这可以通过两行来完成：

# set up

# create example affine trafo in homogeneous coordinates
M = np.r_[np.random.normal(size=(3,4)),[[0,0,0,1]]]
n = 4 # or more
# create n points as the columns of s
# note that homogeneous coordinates have a "dummy" 1, not 0, as last element
s = np.r_[np.random.normal(size=(3,n)),np.ones((1,n))]
# apply trafo, transformed points are the columns of d
d = M@s

# solve

# solving is as simple as
M_rec,resid,rank,sing = np.linalg.lstsq(s.T,d.T)
M_rec = M_rec.T

# you may want to inspect resid (should be small),
# rank (should be 4) and sing (shouldn't spread too wide)

# check

np.allclose(M,M_rec)
# True

Answer 2

基本上，你可以通过建立一个线性方程组并求解它来解决这个问题。

import numpy as np

# Setup problem

np.random.seed(0)
# Make random affine transformation
m = np.concatenate([np.random.rand(3, 4), [[0.0, 0.0, 0.0, 1.0]]], axis=0)
# Make random points
s1 = np.random.rand(3)
s2 = np.random.rand(3)
s3 = np.random.rand(3)
s4 = np.random.rand(3)
# Compute transformed points
d1 = m[:-1, :-1] @ s1 + m[:-1, -1]
d2 = m[:-1, :-1] @ s2 + m[:-1, -1]
d3 = m[:-1, :-1] @ s3 + m[:-1, -1]
d4 = m[:-1, :-1] @ s4 + m[:-1, -1]

# Solve problem

# Arrange points as matrices of homogeneous coordinates
s = np.stack([s1, s2, s3, s4], axis=1)
s = np.concatenate([s, np.ones_like(s[:1])], axis=0)
d = np.stack([d1, d2, d3, d4], axis=1)
d = np.concatenate([d, np.ones_like(d[:1])], axis=0)
# Make equations (a @ x = b)
n = s.shape[1]
a = np.zeros((3 * n, 12), dtype=s.dtype)
b = np.zeros(3 * n, dtype=s.dtype)
a[:n, 0] = s[0]
a[:n, 1] = s[1]
a[:n, 2] = s[2]
a[:n, 3] = s[3]
b[:n] = d[0]
a[n:2 * n, 4] = s[0]
a[n:2 * n, 5] = s[1]
a[n:2 * n, 6] = s[2]
a[n:2 * n, 7] = s[3]
b[n:2 * n] = d[1]
a[2 * n:, 8] = s[0]
a[2 * n:, 9] = s[1]
a[2 * n:, 10] = s[2]
a[2 * n:, 11] = s[3]
b[2 * n:] = d[2]
# Least-squares solution to equations
x = np.linalg.pinv(a) @ b
# Reshape into affine transformation matrix
m2 = np.concatenate([x.reshape(3, 4), [[0.0, 0.0, 0.0, 1.0]]], axis=0)
# Check result
print(np.allclose(m, m2))
# True

使用np.linalg.pinv（或np.linalg.lstsq）可以得到问题的最小二乘解，以防点不完全匹配。此解决方案适用于任意数量的点。如果你有很多，也许你可以使用稀疏求解器。但在这种情况下，最好使用RANSAC 来找到一个对异常值稳定的好解决方案。