それ高速化できるかも！MATLABの回転行列の関数でループ処理を回避する方法【学習#2】

pall@資格を取りたい機械系修士2年

2024年12月8日 14:25

rotm2eul()は、MATLABで3次元空間の剛体の回転の計算時に使用する関数で、回転行列を入力すると姿勢の回転角度を出力します。

過去の記事では、

【変換①】姿勢の回転角度 ⇒ 回転行列
【変換②】回転行列 ⇒ 姿勢の回転角度

という変換をMATLABの関数を用いて行った際に「入力時の姿勢の回転角度と出力時の姿勢の回転角度が一致しない」ことについてまとめました。

その記事の最後の部分で

rotm2eul()のメリットとしては、N個の回転行列を一度に引数とすると、まとめて計算してくれる優れものです。

と触れました。今回は、複数個の回転行列を引数とした場合にどれくらい計算時間が高速化されるのかを検証してみました。

1. 複数個の回転行列を変換したくなった動機

研究活動の中で3次元空間の剛体の軌道を最適化しようとした際に、最適化関数の中で回転行列から固定角の変換を行う必要がありました。

例として、最適化計算における1回の反復計算の中で回転行列→固定角の変換を100回する必要がある場合、最適化計算の反復回数が100回であれば100×100=10000回の変換が必要になるため、この変換処理をできるだけ高速化したくなりますよね。

最初、最適化計算の関数では下のコードのように、回転行列の個数だけループでrotm2eul()を何度も呼び出す処理をしていました。

% 100 個の 3x3 回転行列を作成
N = 100; % 回転行列の数
rotmArray = zeros(3, 3, N); % 初期化

% 回転行列をランダム生成（例: 固定軸の回転行列）
for i = 1:N
    theta = rand() * 2 * pi; % ランダムな回転角 (0 ~ 2π)
    rotmArray(:,:,i) = [cos(theta) -sin(theta) 0;
                        sin(theta)  cos(theta) 0;
                             0          0      1]; % Z 軸回りの回転
end

% ループで逐次オイラー角への変換
eulAnglesLoop = zeros(N, 3); % 結果格納用
for i = 1:N
    eulAnglesLoop(i,:) = rotm2eul(rotmArray(:,:,i), 'ZYX');
end

% 結果の表示
disp(eulAnglesLoop);

しかし、よく調べるとrotm2eul()は一度に複数個の回転行列を変換できることを知り、実際に試したところ計算速度が向上しました。

2. 関数rotm2eul()

rotm2eul()は、rotm2eul は、3×3 の回転行列（Rotation Matrix）をオイラー角（Euler Angles）へ変換するための MATLAB 関数です。この関数は主に以下の特徴や用途を持っています。

機能

与えられた 3×3 回転行列に対して、指定した回転順序（例えば 'ZYX', 'ZXY', 'XYZ' など）に基づいて、対応するオイラー角（3つの角度）を返します。出力は [phi, theta, psi] といった形式で、角度はラジアン単位となります。

使用法

eulAngles = rotm2eul(rotm, sequence)

rotm: 3×3 の回転行列、あるいは 3×3×N の回転行列が格納された配列。
sequence: オイラー角の回転順序を示す文字列（例: 'ZYX', 'ZXY', 'XYZ', 'XZY' など）。

eulAngles には N×3 の行列が返され、各行は1組のオイラー角を表します。公式ドキュメントに3×3×Nの配列を設定できると記載がありますが、初めて使用したときに私はこの部分に気づけませんでした、、、

Toolbox依存性

rotm2eul ()は MATLAB 本体のみでは提供されていない関数で、Robotics System Toolbox や Aerospace Toolbox といった特定のツールボックスをインストールし、ライセンスを所持している場合に使用できます。そのため、使用したい場合は追加で料金を支払う必要がある点は少しデメリットです。

3. 比較

次のようなコードで「ループで変換」「一度に変換」の計算時間を比較しました。

% N を 50から1000まで、50刻みで生成
N_list = 50:50:1000;
numTrials = 30; % 各Nに対する計測回数

% 計算結果を格納
timeLoopAll = zeros(numTrials, length(N_list));
timeBulkAll = zeros(numTrials, length(N_list));

% 計算時間の計測
for idx = 1:length(N_list)
    N = N_list(idx);
    
    for trial = 1:numTrials
        % 回転行列をランダム生成
        rotmArray = zeros(3,3,N);
        for i = 1:N
            theta = rand() * 2 * pi;
            rotmArray(:,:,i) = [cos(theta), -sin(theta), 0;
                                sin(theta),  cos(theta), 0;
                                      0,          0,    1];
        end
        
        % (1) ループで逐次変換
        tic;
        eulAnglesLoop = zeros(N,3);
        for i = 1:N
            eulAnglesLoop(i,:) = rotm2eul(rotmArray(:,:,i), 'ZYX');
        end
        timeLoop = toc;
        
        % (2) 一度に変換
        tic;
        eulAnglesBulk = rotm2eul(rotmArray, 'ZYX');
        timeBulk = toc;
        
        % 計測結果を保存
        timeLoopAll(trial, idx) = timeLoop;
        timeBulkAll(trial, idx) = timeBulk;
    end
    
    % 中間報告
    fprintf('N = %d 計測完了\n', N);
end

% 各Nに対する計測時間の平均・標準偏差を計算
timeLoopMean = mean(timeLoopAll, 1);
timeLoopStd = std(timeLoopAll, 0, 1);
timeBulkMean = mean(timeBulkAll, 1);
timeBulkStd = std(timeBulkAll, 0, 1);

% プロット
figure; 
errorbar(N_list, timeLoopMean, timeLoopStd, '-o', 'DisplayName','ループ');
hold on;
errorbar(N_list, timeBulkMean, timeBulkStd, '-x', 'DisplayName','一度に');
xlabel('回転行列の個数 N');
ylabel('計算時間 [s]');
legend('Location','best');
grid on;
hold off;

計算時間を比較するために、回転行列の個数を50から1000まで50個ずつ変えた条件で計算時間を計測します。その際に、各個数の条件で30回ずつ計算を行い、平均と標準偏差を算出し、グラフにばらつきも可視化します。

以下が計測結果です。結果は明らかに一度に計算する方が高速であることが確認できます。ループ処理は、回転行列の個数に比例して計算時間が増加するのに対し、一度に処理する場合はほとんど一定です。

2節の例では、最適化計算の反復1回で100回の回転行列の変換が行われる場合に、反復計算が100回である場合を考えました。この計測結果から計算時間を比較します。

【ループ処理】0.0050×100×100 = 50 s
【一度に処理】0.0001×100×100 = 1 s

この変換の処理だけで最適化計算で49 s も短くすることができます。

4. まとめ

回転行列から姿勢の回転角度を算出する関数rotm2eul()について、「ループ処理」と「一度に処理」する場合の計算時間の比較を行いました。実際に計測すると「一度に処理」する場合の方が明らかに高速であることがわかりました。

公式ドキュメントで記載を見逃してしまい、複数の回転行列を一度に変換できることを知らずに使用していたので、これだけ高速化できることを知ったときは驚きました。

以上！！！

【前回の記事】

【次の記事】