Robotics System Toolbox で得た同次変換行列を​、rigid3dオブ​ジェクトに変換するに​はどうすればよいです​か?

Robotics System Toolboxの getTransform 関数から取得した同次変換行列を使用して、pctransform 関数を実行し、ロボットアームの手先の座標系で表現された点群座標をベースフレームでの座標に変換しようとしました。(R2022aを使用。)

以下のように同次変換行列をrigid3dオブジェクトに変換し、 pctransform 関数に入力しましたが、変換後の座標が期待した通りになりません。

transform = getTransform( robot, confg, 'sourcebody', 'targetbody');
rot=tform2rotm(transform);
trvec=tform2trvec(transform); 
tform=rigid3d(rot,trvec); 
pcOut=pctransform(pcIn,tform);

Answer 1

[MathWorks Support Team]

コードの４行目で、回転行列 'rot' を次のように転置することで期待した結果が得られます。

>>tform=rigid3d(rot', trvec);

転置が必要になる理由は、getTransform 関数と rigid3d オブジェクトが同次変換行列を異なる方法で実装しているためです。

getTransform (および多くの工学分野)では、同次座標は列ベクトル形式（[x; y; z; 1]）で表されており、同次変換行列 G は G = [Rot Tran(3x1); 0 0 0 1] と定義され、q = G*p (左から乗算) として使用されます。

https://www.mathworks.com/help/robotics/ref/rigidbodytree.gettransform.html

一方、 rigid3dでは

行ベクトル

形式の座標（[x y z 1]）および変換行列T = [Rot [0;0;0]; Tran(1x3) 1] が用いられ、使用時は s = r*T (

右

から乗算)で変換を計算します。

https://jp.mathworks.com/help/images/ref/rigid3d.html

直観的には、後者の方程式全体を転置すると前者と同じ形式（s' = T' * r'）であることから、T'が前者の形式で得られた変換行列Gと一致していればよいことが分かります。

Gから抽出した回転成分を転置して「rigid3d」オブジェクトを作成することで、T' = [Rot' [0;0;0];Tran(1x3) 1]' = [Rot Tran(3x1); 0 0 0 1]となり、Gに一致します。