使用 MATLAB 计算相机矩阵

Question

我目前正在尝试在给定一组世界点 (X) 及其对应的图像点 (x) 的情况下计算相机矩阵 P。但是，在测试结果时，P（3 x 4 相机矩阵）乘以世界点并没有给我正确的对应图像点。但是，只有第一列 PX = x。另一列不会返回近似图像点。

代码：

X = [1 2 3; 4 5 6; 7 8 9; 1 1 1];
x = [3 2 1; 6 5 4; 1 1 1];

[mX, nX] = size(X);
[mx, nx] = size(x);

for i = 0:(nX-1)
  XX{i+1} = transpose(X(1+i: 4+i));
end

for i = 0:(nx-1)
  xx{i+1} = transpose(x(i+1:3+i));
end

%TODO - normalization

A = [];
%construct matrix
for i = 1:nX
  A = [A; zeros(1,4) -1*(xx{i}(3)*transpose(XX{i})) xx{i}(2)*transpose(XX{i})];
  A = [A; xx{i}(3)*transpose(XX{i}) zeros(1,4) -1*xx{i}(1)*transpose(XX{i})];
end

%using svd to solve for non zero solution
[u s v] = svd(A);

p = v(:, size(v,2));
p = reshape(p, 4,3)';

第一列的输出，效果很好：

>> p*XX{1}

ans =

    0.0461
    0.0922
    0.0154

>> ans/0.0154

ans =

    2.9921
    5.9841
    0.9974

>> xx{1}

ans =

     3
     6
     1

第二列的输出，不起作用：

>> p*XX{2}

ans =

    0.5202
    0.0867
    0.1734

>> ans/0.1734

ans =

    2.9999
    0.5000
    1.0000

>> xx{2}

ans =

     6
     1
     2

顺便说一句，有人告诉我，在计算相机矩阵之前，我需要对世界点和图像点进行归一化。我没有完成这一步，也不知道怎么做。如果这是导致问题的原因，请说明可以采取的措施。先感谢您。

Answer 1

这是因为您没有正确索引到矩阵。您正在使用线性索引来访问矩阵的每一列。在这种情况下，您的 for 循环需要独立访问每一列。因此，for 循环的每次迭代都必须访问 3D 点的 4 个元素组和 2D 点的 3 个元素组。

因此，您只需为您的 for 循环执行此操作：

for i = 0:(nX-1)
    XX{i+1} = transpose(X(4*i + 1 : 4*(i + 1)));
end

for i = 0:(nx-1)
    xx{i+1} = transpose(x(3*i + 1 : 3*(i + 1)));
end

在此之后，代码应该没有问题。为了验证，我们可以循环遍历每个 3D 点，并在您使用单元格时确定其 2D 等效项：

out = zeros(size(xx)); % Declare output matrix
for ii = 1 : numel(XX) % For each 3D point...
    out(:,ii) = p * XX{ii}; % Transform the point
    out(:,ii) = out(:,ii) / out(end,ii); % Normalize
end

因此我们得到：

>> out

out =

    3.0000    2.0000    1.0000
    6.0000    5.0000    4.0000
    1.0000    1.0000    1.0000

与您的x比较：

>> x

x =

     3     2     1
     6     5     4
     1     1     1

建议 - 使用矢量化

如果我可以提出建议，请不要在这里使用元胞数组。您可以使用矢量化创建用于求解的方程矩阵。具体来说，您可以直接创建矩阵A，而无需任何for 循环：

A = [zeros(N, 4) -X.' bsxfun(@times, x(2,:).', X.'); 
     X.' zeros(N, 4) bsxfun(@times, -x(1,:).', X.')];

如果您拥有 MATLAB R2016b 及更高版本，则可以通过内部广播来做到这一点：

A = [zeros(N, 4) -X.' x(2,:).' .* X.'; 
     X.' zeros(N, 4) -x(1,:).' .* X.']

请注意，由于矢量化，与原始矩阵 A 相比，您会看到行被打乱了。因为我们正在求解矩阵A 的零空间，所以对行进行洗牌没有效果。因此，您的代码可以简化为：

X = [1 2 3; 4 5 6; 7 8 9; 1 1 1];
x = [3 2 1; 6 5 4; 1 1 1];

A = [zeros(N, 4) -X.' bsxfun(@times, x(2,:).', X.'); 
     X.' zeros(N, 4) bsxfun(@times, -x(1,:).', X.')];

% Use this for MATLAB R2016b and up
% A = [zeros(N, 4) -X.' x(2,:).' .* X.'; 
%      X.' zeros(N, 4) -x(1,:).' .* X.']

[u, s, v] = svd(A);
p = v(:, end);
p = reshape(p, 4, 3).';

要最终计算输出矩阵，您可以使用简单的矩阵乘法。您使用单元的事实要求您必须使用 for 循环，并且使用矩阵乘法执行此操作要快得多：

out = p * X;

然后，您可以取出结果的最后一行，并将其他每一行除以该行。

out = bsxfun(@rdivide, out, out(end,:));

再次使用 MATLAB R2016b 及更高版本，您可以这样做：

out = out ./ out(end,:);