如何向量化齐次变换矩阵/张量的计算？

Posted 2023-02-21

【中文标题】如何向量化齐次变换矩阵/张量的计算？

【英文标题】：How to vectorize calculation of homogenous transformation matrix/tensor?

【发布时间】：2019-03-29 09:39:19

【问题描述】：

对于我的模拟，我需要计算许多转换矩阵，因此我想对我现在使用的 for 循环进行矢量化。

有没有办法对现有的 for 循环进行矢量化，或者我之前可能需要另一种方法来计算矢量和矩阵？

我准备了一个小例子：

n_dim = 1e5;
p1_3 = zeros(3,n_dim);                          % translationvector (no trans.) [3x100000]
tx = ones(1,n_dim)*15./180*pi;                  % turn angle around x-axis (fixed) [1x100000]
ty = zeros(1,n_dim);                            % turn angle around y-axis (no turn) [1x100000]
tz = randi([-180 180], 1, n_dim)./180*pi;       % turn angle around z-axis (different turn) [1x100000]
hom = [0 0 0 1].*ones(n_dim,4);                 % vector needed for homogenous transformation [100000x4]

% calculate sin/cosin values for rotation [100000x1 each]
cx = cos(tx)';
sx = sin(tx)';

cy = cos(ty)';
sy = sin(ty)';

cz = cos(tz)';
sz = sin(tz)';

% calculate rotation matrix [300000x3]
R_full = [        cy.*cz,         -cy.*sz,     sy; ...
          cx.*sz+sx.*sy.*cz, cx.*cz-sx.*sy.*sz, -sx.*cy; ...
          sx.*sz-cx.*sy.*cz, cz.*sx+cx.*sy.*sz,  cx.*cy];

% prealocate transformation tensor
T = zeros(4,4,n_dim);

% create transformation tensor here
% T = [R11 R12 R13 p1;
%      R21 R22 R23 p2;
%      R31 R32 R33 p3;
%       0   0   0   1]
tic
for i = 1:n_dim           
  T(:,:,i) = [[R_full(i,1), R_full(i,2), R_full(i,3); ...
               R_full(n_dim+i,1), R_full(n_dim+i,2), R_full(n_dim+i,3); ...
               R_full(2*n_dim+i,1), R_full(2*n_dim+i,2), R_full(2*n_dim+i,3)], p1_3(:,i);
               hom(i,:)];
end
toc

【问题讨论】：

您使用的是 MATLAB 还是 Octave？它们不一样...

我正在使用八度

【参考方案1】：

试试这个：

T = permute(reshape(R_full,n_dim,3,3),[2,3,1]);
T(4,4,:) = 1;

你的方法：

Elapsed time is 0.839315 seconds.

这个方法：

Elapsed time is 0.015389 seconds.

【讨论】：

谢谢！ permute 命令对我来说仍然有点神秘 :D 我只需要调整 reshape() 的索引，在我的示例中它是 n_dim 与 100000 而不是 1000 :)

糟糕，已修复。谢谢！这个想法很简单：您将数据排列为n_dim*3 x 3，将几个长度-n_dim 向量并排放置。相反，您可以将其排列为 3-D 矩阵中的 n_dim x 3 x 3。置换除了切换这些维度之外什么都不做，而是改为3 x 3 x n_dim，因为你需要它来处理你的T。有点像矩阵转置的概括，它翻转矩阵中的两个维度。

【参考方案2】：

编辑

我加了Florian's answer，当然他赢了。

---

你准备好疯狂索引 foo 了吗？我们开始：

clear all;
close all;
clc;

n_dim_max = 200;

t_loop = zeros(n_dim_max, 1);
t_indexing = t_loop;
t_permute = t_loop;

fprintf("---------------------------------------------------------------\n");

for n_dim = 1:n_dim_max

  p1_3 = zeros(3,n_dim);                          % translationvector (no trans.) [3x100000]
  tx = ones(1,n_dim)*15./180*pi;                  % turn angle around x-axis (fixed) [1x100000]
  ty = zeros(1,n_dim);                            % turn angle around y-axis (no turn) [1x100000]
  tz = randi([-180 180], 1, n_dim)./180*pi;       % turn angle around z-axis (different turn) [1x100000]
  hom = [0 0 0 1].*ones(n_dim,4);                 % vector needed for homogenous transformation [100000x4]

  % calculate sin/cosin values for rotation [100000x1 each]
  cx = cos(tx)';
  sx = sin(tx)';

  cy = cos(ty)';
  sy = sin(ty)';

  cz = cos(tz)';
  sz = sin(tz)';

  % calculate rotation matrix [300000x3]
  R_full = [        cy.*cz,         -cy.*sz,     sy; ...
            cx.*sz+sx.*sy.*cz, cx.*cz-sx.*sy.*sz, -sx.*cy; ...
            sx.*sz-cx.*sy.*cz, cz.*sx+cx.*sy.*sz,  cx.*cy];

  % prealocate transformation tensor
  T = zeros(4,4,n_dim);

  % create transformation tensor here
  % T = [R11 R12 R13 p1;
  %      R21 R22 R23 p2;
  %      R31 R32 R33 p3;
  %       0   0   0   1]
  tic
  for i = 1:n_dim           
    T(:,:,i) = [[R_full(i,1), R_full(i,2), R_full(i,3); ...
                 R_full(n_dim+i,1), R_full(n_dim+i,2), R_full(n_dim+i,3); ...
                 R_full(2*n_dim+i,1), R_full(2*n_dim+i,2), R_full(2*n_dim+i,3)], p1_3(:,i);
                 hom(i,:)];
  end
  t_loop(n_dim) = toc;

  tic
  % prealocate transformation tensor
  TT = zeros(4, 4);
  TT(end) = 1;
  TT = repmat(TT, 1, 1, n_dim);

  % Crazy index finding.
  temp = repmat(1:(3*n_dim):(3*3*n_dim), 3, 1) + n_dim .* ((0:2).' * ones(1, 3));
  temp = repmat(temp, 1, 1, n_dim);
  t = zeros(1, 1, n_dim);
  t(:) = 0:(n_dim-1);
  temp = temp + ones(3, 3, n_dim) .* t;

  % Direct assignment using crazily found indices.
  TT(1:3, 1:3, :) = R_full(temp);
  t_indexing(n_dim) = toc;

  tic
  % prealocate transformation tensor
  TTT = zeros(4, 4);
  TTT(end) = 1;
  TTT = repmat(TTT, 1, 1, n_dim);

  TTT(1:3, 1:3, :) = permute(reshape(R_full, n_dim, 3, 3), [2, 3, 1]);
  t_permute(n_dim) = toc;

  % Check
  fprintf("n_dim: %d\n", n_dim);
  fprintf("T equals TT: %d\n", (sum(T(:) == TT(:))) == (4 * 4 * n_dim));
  fprintf("T equals TTT: %d\n", (sum(T(:) == TTT(:))) == (4 * 4 * n_dim));
  fprintf("---------------------------------------------------------------\n");
end

figure(1);
plot(1:n_dim_max, t_loop, 1:n_dim_max, t_indexing, 1:n_dim_max, t_permute);
legend('Loop', 'Indexing', 'Permute');
xlabel('Dimension');
ylabel('Elapsed time [s]');

抱歉，脚本很长，因为它是您的初始解决方案、我的解决方案（和Florian's solution）和测试脚本一体化。懒惰的星期五是我没有正确拆分事情的原因......

我是怎么到那里的？简单的“逆向工程”。我将您的解决方案用于n_dim = [2, 3, 4] 并确定[~, ii] = ismember(T(1:3, 1:3, :), R_full)，即R_full 到T(1:3, 1:3, :) 的映射。然后，我分析了索引方案，并找到了模拟任意n_dim 的映射的适当解决方案。完毕！ ;-) 是的，我喜欢疯狂的索引。