前言
在使用Matlab机器人工具箱时,需要做一个多维向量乘法,但是又不想写循环语句。具体来讲:
姿态变换矩阵 T 为 4x4x1 相对位移矩阵 F 为 4x4xN 想要求 T*F(页)
代码
xdata=rpy2tr([pi/2 ,0,0;0,0,pi/2])
x=[1 2 3 0 0 pi]
T=transl(x(1:3))*rpy2tr(x(4:6))
F=xdata
t=size(T);
f=size(F);
b = reshape(sum(bsxfun(@times,reshape(T,[t(1),1,t(2)]),...
reshape(permute(F,[2 1 3]),1,f(2),f(1),[])),3),f(1),t(2),[])
关键点
a. 将前面的姿态变换矩阵T按照列重置维数为 T(行)x1xT(列) b. 将后面的相对位移矩阵F按照行重置维数为 1xF(列)xF(行)xF(页) c. 考虑到reshape的方式,需要先将F进行“转置”,使用permute 将第一、二维交换 e. 使用bsxfun将reshape后的多维矩阵按元素相乘,获得矩阵 T(行)xF(列)xF(行)xF(页) f. 将所得矩阵按照F行的深度进行相加sum g. 重置矩阵为T(行)xF(列)xF(页)
|