我有一个大小为 (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)
请提供任何帮助。
Answer:
trajectory = np.hstack((trajectory, np.ones((trajectory.shape[0], 1))))#(nx3)->(nx4)
trajectory_new = trajectory.dot(HTM.T)[:,:-1]