【发布时间】:2017-06-14 07:33:26
【问题描述】:
我有一个大小为 (4x4) 的齐次变换矩阵和一个大小为 (nx3) 的轨迹。该轨迹的每一行都是一个向量。
我想将齐次变换矩阵乘以每一行轨迹。下面是代码:
#append zero column at last
trajectory = np.hstack((trajectory, np.zeros((trajectory.shape[0], 1)))) #(nx3)->(nx4)
trajectory_new = np.zeros((1, 3)) #(1x3)
for row in trajectory:
vect = row.reshape((-1,1)) #convert (1x4) to (4x1)
vect = np.dot(HTM, vect) #(4x4) x (4x1) = (4x1)
vect = vect.T #(1x4)
vect = np.delete(vect, -1, axis=1) #remove last element from vector
trajectory_new = np.vstack((trajectory_new, vect)) #(nx3)
trajectory_new = np.delete(trajectory_new, 0, axis=0)#remove first row
上面的代码有效。但是,我正在寻找更简单的解决方案,例如:
trajectory_new = np.apply_along_axis(np.multiply, 0, trajectory, HTM)
请帮忙。
答案:
trajectory = np.hstack((trajectory, np.ones((trajectory.shape[0], 1))))#(nx3)->(nx4)
trajectory_new = trajectory.dot(HTM.T)[:,:-1]
【问题讨论】:
-
不用堆叠也可以,如
this post所示。 -
我在谈论我的帖子。我在之前的评论链接中发布的链接。
-
@Divakar:抱歉,但是您已从 HTM 中删除了最后一列,这是不正确的。您可以删除 HTM 的最后一行,但不能删除最后一列。更准确地说,HTM 是 (4x4),其中第一个 (3x3) 表示旋转矩阵,最后一列表示 3D 空间中的平移。 HTM 的最后一行总是 [0, 0, 0, 1],这使得 HTM (4x4) 矩阵。
-
但是您没有使用 HTM 的最后一列来计算输出
trajectory_new。这就是为什么您需要在开头添加零并使用原始方法删除第一行的原因。