python - 如何求解未知矩阵的矩阵方程？

Question

我正在尝试解决 equation , s m' = A[R|t]M'

即

m = K。吨。 M 其中 m, K, M 和 T 的最后一列 [ R | t ] 是已知的。

我想获取 3*3 旋转矩阵的每个元素的值。我有。

这个问题也有人回答了here

但是当我们为m 和M 取新值时，我无法理解如何在每次创建新的方程组后获取 3*3 旋转矩阵的值>.

m 包含以像素为单位的投影点坐标，我在图像上有 16 个不同的点用于相机捕获的图案，并且每个 u 和 v 都有 16 组值。

m=np.array([u,v,1])

K 是我的内在矩阵/相机矩阵/相机内在参数矩阵，我将 fx、fy(焦距)和 cx、cy(主点)的值作为相机内在矩阵

K=np.matrix([ [fx, 0, cx, 0], 
              [ 0, fy, cy, 0], 
              [ 0, 0, 1, 0]])

T 是传递给“世界”坐标系到相机坐标系的变换(外部矩阵，[R | t])，我也有Tx、Ty 和 Tz 的值。

T= np.matrix([[x00, x01, x02, Tx], 
              [x10, x11, x12, Ty], 
              [x20, x21, x22, Tz], 
              [0 , 0 , 0 , 1 ]])

M 是笛卡尔坐标系“世界”中点的齐次坐标，即世界坐标空间中 3D 点的坐标。我有来自模式的 16 个点，因此每个 X、Y、Z 我有 16 个不同的值。

M=np.array([X,Y,Z,1])

我的目标是获取矩阵 T 的元素 x00、x01、x02、x10、x11、x12、x20、x21、x22 的值。有人可以帮忙吗？？

更多说明:

假设对于m矩阵(以像素为单位的投影点坐标)，u和v的值为:

u = [ 337, 337, 316, 317, 302, 302, 291, 292, 338, ...]

和

v =[ 487, 572, 477, 547, 470, 528, 465, 516, 598, ...]

即以像素为单位的第一个投影点的坐标是337(行号)和487(列号)

因此，

对于第一组方程，矩阵，m 会有值，

import sympy as sy            
import numpy as np


# m = sy.Matrix([u, v, 1]
m = sy.Matrix([337, 487, 1])

,

对于第二组方程，矩阵，m 会有值，

# m = sy.Matrix([u, v, 1]
m = sy.Matrix([337, 572, 1])

很快...

对于K矩阵(内参矩阵)的值:

K = sy.Matrix([[711.629,  0, 496.220, 0],
               [0,  712.682, 350.535, 0],
               [0,   0,  0, 1]])

对于 M 矩阵(世界坐标空间中 3D 点的坐标)，X、Y 和 Z 的值是:

X = [4.25, 4.25, 5.32, 5.32, 6.27, 6.27, 7.28, 7.28, 4.20, ...] 
Y = 0
Z =  [0.63, 1.63, 0.63, 1.63, 0.59, 1.59, 0.60, 1.92, 2.92, ...]  

对于第一组方程，矩阵M将是

# M=np.array([X,Y,Z,1])
M = sy.Matrix([0.63, 0, 4.25, 1])

,

对于第二组方程，矩阵，M 会有值，

# M=np.array([X,Y,Z,1])
M = sy.Matrix([1.63, 0, 4.25, 1])

很快...

对于 T 矩阵(外部矩阵，[ R | t ])，我们将 Tx, Ty, Tz 的值设置为 0, -1.35, 0 。因此，T矩阵将是:

T = sy.Matrix([[x11, x12, x13, 0],
               [x21, x22, x23, -1.32],
               [x31, x32, x33, 0],
               [0,     0,   0,  1]])

我需要制作九组这样的矩阵方程:m = K * T * M 使用不同的 m 和 M 值，所以我可以从这些方程组中计算出 T 矩阵中 9 个未知数的值。

Answer 1

基本上，您有矩阵方程(使用 OpenCV 文档的符号):

A @ (R @ w + t) == m

其中 A.shape == (3, 3), R.shape == (3, 3), w.shape == (3 , n), t.shape == (3, 1), m.shape == (3, n), 代表n 指向世界坐标 w 和图像坐标 m。

这个等式可以重新排列为

w.T @ R.T == (inv(A) @ m - t).T

其中 inv(A) 是 A 的逆。左侧和右侧的形状是 (n, 3)。这具有矩阵方程的格式，具有 9 个未知数(在 R.T 中)和 n 个方程。在这种形式下，您可以输入 np.linalg.lstsq 以获得最小二乘解决方案 - 假设您的 n >= 3 具有足够的独立点。

这是一个随机数的演示:

import numpy as np

# Setup test case
np.random.seed(1)
R = np.random.randint(-9, 9, size=(3, 3)).astype(np.float64)
t = np.array([1, 1.5, 2]).reshape(3, 1) # column vector
Rt = np.hstack([R, t]) # shape (3, 4)
A = np.diag([0.5, 0.5, 1.0]) # shape (3, 3)

n = 20 # number of points
# M: shape (4, n)
M = np.vstack([np.random.uniform(size=(3, n)), np.ones((1, n))])
m = A @ Rt @ M # m.shape == (3, n)

# Now try to reconstruct R, given A, M, t, and m.

w = M[:3, :] # world XYZ coordinates, shape (3, n)

# Matrix equation: A @ (R @ w + t) == m
# Equivalent to w.T @ R.T == (inv(A) @ m - t).T
RTfit, _, _, _ = np.linalg.lstsq(w.T, (np.linalg.inv(A) @ m - t).T, rcond=None)

Rfit = np.around(RTfit.T, 6)
print(f'Original R:\n{R}\nReconstructed R:\n{Rfit}')

输出:

Original R:
[[-4.  2.  3.]
 [-1.  0.  2.]
 [-4.  6. -9.]]
Reconstructed R:
[[-4.  2.  3.]
 [-1. -0.  2.]
 [-4.  6. -9.]]

请注意，您还可以使用三个数据点 (n=3) 进行精确求解:

Rsolve = np.linalg.solve(w.T[:3], (np.linalg.inv(A) @ m[:, :3] - t).T).T

但在这种情况下，您需要仔细选择您的三个点，否则它不会起作用。

这是对您的数据的尝试:

t = np.array([[0, -1.32, 0]]).T
w = np.array([
    [4.25, 4.25, 5.32, 5.32, 6.27, 6.27, 7.28, 7.28, 4.20],
    np.zeros(9),
    [0.63, 1.63, 0.63, 1.63, 0.59, 1.59, 0.60, 1.92, 2.92]
    ])
m = np.array([
    [337, 337, 316, 317, 302, 302, 291, 292, 338],
    [487, 572, 477, 547, 470, 528, 465, 516, 598],
    np.ones(9)
    ])
A = np.array([
    [711.629,  0, 496.220],
    [712.682, 350.535, 0],
    [0, 0, 1]
    ])
RTfit, _, _, _ = np.linalg.lstsq(w.T, (np.linalg.inv(A) @ m - t).T, rcond=None)
Rfit = np.around(RTfit.T, 6)
print(Rfit)

输出:

array([[-0.040938,  0.      , -0.016044],
       [ 0.448038,  0.      ,  0.52933 ],
       [ 0.14251 ,  0.      ,  0.127464]])

它无法有意义地求解 R 矩阵的中间列，因为输入的所有 Y 值均为零。 (如果你用 np.linalg.solve 试试这个，你会得到一个奇异矩阵错误。)

拟合不是特别好，绘制 m 和 A @ (R @ w + t) 可以证明:

不匹配意味着不存在与数据一致的 R 矩阵。在您的评论中，您询问 R 矩阵是否是最佳解决方案。它是匹配方程的 LHS 和 RHS 的最佳解决方案(w.T @ Rfit.T 与 (inv(A) @ m - t).T)。

鉴于上图中存在较大的不匹配，推测生成的 R 矩阵的准确性没有多大意义。很可能是输入数据有问题；点 (m, w)、t 向量或 A 矩阵。