用标量角张量创建变换矩阵
Posted
技术标签:
【中文标题】用标量角张量创建变换矩阵【英文标题】：Create a Transformation Matrix out of Scalar Angle Tensors 【发布时间】：2018-11-16 09:31:34 【问题描述】：原始问题
我想使用 keras 创建一个自定义 Lambda 函数，该函数执行关节臂的正向运动学。
这个函数有一组角度作为输入，应该输出一个包含末端执行器的位置和方向的向量。
我可以很容易地在 numpy 中创建这个函数；但是当我想把它移到 Keras 时，事情就变得困难了。
由于 lambda 函数的输入和输出都是张量，所以所有操作都应该使用张量和后端操作来完成。
问题是我必须从输入角度创建一个变换矩阵。
我可以使用K.cos 和K.sin（K 是后端张量流）来计算角度的余弦和正弦。但问题是如何创建一个 4X4 矩阵的张量，其中包含一些单元格，这些单元格只是数字（0 或1），而其他单元格是张量的一部分。
例如对于 Z 旋转：
T = tf.convert_to_tensor( [[c, -s, 0, dX],
[s, c, 0, dY],
[0, 0, 1, dZ],
[0, 0, 0, 1]])
这里c 和s 是使用K.cos(input[3]) 和K.sin(input[3]) 计算的。
这不起作用。我明白了：
ValueError: Shapes must be equal rank, but are 1 and 0 将形状 1 与其他形状合并。对于 'lambda_1/packed/0' (op: 'Pack')，输入形状为：[5]、[5]、[]、[]。
有什么建议吗？
进一步的问题
@Aldream 提供的代码运行良好。 问题是当我将它嵌入到 Lambda 层时，编译模型时出现错误。
...
self.model.add(Lambda(self.FK_Keras))
self.model.compile(optimizer="adam", loss='mse', metrics=['mse'])
如您所见，我使用了一个包含模型和各种功能的类。 首先，我有一个计算变换矩阵的辅助函数：
def trig_K( angle):
r = angle*np.pi/180.0
return K.cos(r), K.sin(r)
def T_matrix_K(rotation, axis="z", translation=K.constant([0,0,0])):
c, s = trig_K(rotation)
dX = translation[0]
dY = translation[1]
dZ = translation[2]
if(axis=="z"):
T = K.stack( [[c, -s, 0., dX],
[s, c, 0., dY],
[0., 0., 1., dZ],
[0., 0., 0., 1.]], axis=0)
if(axis=="y"):
T = K.stack( [ [c, 0.,-s, dX],
[0., 1., 0., dY],
[s, 0., c, dZ],
[0., 0., 0., .1]], axis=0)
if(axis=="x"):
T = K.stack( [ [1., 0., 0., dX],
[0., c, -s, dY],
[0., s, c, dZ],
[0., 0., 0., 1.]], axis=0)
return T
然后 FK_keras 计算末端执行器变换：
def FK_Keras(self, angs):
# Compute local transformations
base_T=T_matrix_K(angs[0],"z",self.base_pos_K)
shoulder_T=T_matrix_K(angs[1],"y",self.shoulder_pos_K)
elbow_T=T_matrix_K(angs[2],"y",self.elbow_pos_K)
wrist_1_T=T_matrix_K(angs[3],"y",self.wrist_1_pos_K)
wrist_2_T=T_matrix_K(angs[4],"x",self.wrist_2_pos_K)
# Compute end effector transformation
end_effector_T=K.dot(base_T,K.dot(shoulder_T,K.dot(elbow_T,K.dot(wrist_1_T,wrist_2_T))))
# Compute Yaw, Pitch, Roll of end effector
y=K.tf.atan2(end_effector_T[1,0],end_effector_T[1,1])
p=K.tf.atan2(-end_effector_T[2,0],K.tf.sqrt(end_effector_T[2,1]*end_effector_T[2,1]+end_effector_T[2,2]*end_effector_T[2,2]))
r=K.tf.atan2(end_effector_T[2,1],end_effector_T[2,2])
# Construct the output tensor [x,y,z,y,p,r]
output = K.stack([end_effector_T[0,3],end_effector_T[1,3],end_effector_T[2,3], y, p, r], axis=0)
return output
这里 self.base_pos_K 和其他翻译向量是常量：
self.base_pos_K = K.constant(np.array([x,y,z]))
Tle 代码卡在编译函数中并返回此错误：
ValueError: Shapes must be equal rank, but are 1 and 0 将形状 1 与其他形状合并。对于 'lambda_1/stack_1' (op: 'Pack')，输入形状为：[5]、[5]、[]、[]。
我试图创建一个这样的快速测试代码：
arm = Bot("")
# Articulation angles
input_data =np.array([90., 180., 45., 25., 25.])
sess = K.get_session()
inp = K.placeholder(shape=(5), name="inp")#)
res = sess.run(arm.FK_Keras(inp),inp: input_data)
此代码可以正常工作，没有错误。 可以将其集成到顺序模型的 Lambda 层中。
问题已解决
确实，问题与 Keras 处理数据的方式有关。它添加了一个批处理维度，在实现该功能时应考虑该维度。
我以不同的方式处理这个问题，包括重新实现 T_matrix_K 来处理这个额外的维度，但我认为@Aldream 提出的方式更优雅。
非常感谢@Aldream。他的回答很有帮助。
【问题讨论】：
【参考方案1】：使用K.stack():
import keras
import keras.backend as K
input = K.constant([3.14, 0., 0, 3.14])
dX, dY, dZ = K.constant(1.), K.constant(2.), K.constant(3.)
c, s = K.cos(input[3]), K.sin(input[3])
T = K.stack([[ c, -s, 0., dX],
[ s, c, 0., dY],
[0., 0., 1., dZ],
[0., 0., 0., 1.]], axis=0
)
sess = K.get_session()
res = sess.run(T)
print(res)
# [[ -9.99998748e-01 -1.59254798e-03 0.00000000e+00 1.00000000e+00]
# [ 1.59254798e-03 -9.99998748e-01 0.00000000e+00 2.00000000e+00]
# [ 0.00000000e+00 0.00000000e+00 1.00000000e+00 3.00000000e+00]
# [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
如何使用Lambda:
Keras 层期待/处理批处理数据。例如，Keras 会假设 Lambda(FK_Keras) 层的输入 (angs) 的形状为 (batch_size, 5)。因此，您的FK_Keras() 需要适应此类输入。
一种相当简单的方法是使用K.map_fn() 循环处理批处理中的每个角度列表，并对每个角度应用正确的T_matrix_K() 函数，只需对T_matrix_K() 进行少量编辑即可。
处理批次的其他小改动：
使用K.batch_dot() 代替K.dot()
相应地广播您的常量张量，例如self.base_pos_K
考虑到批量张量的附加第一维，例如将end_effector_T[1,0] 替换为end_effector_T[:, 1,0]
在下面找到一个缩短的工作代码（延伸到所有关节由您决定）：
import keras
import keras.backend as K
from keras.layers import Lambda, Dense
from keras.models import Model, Sequential
import numpy as np
def trig_K( angle):
r = angle*np.pi/180.0
return K.cos(r), K.sin(r)
def T_matrix_K_z(x):
rotation, translation = x[0], x[1]
c, s = trig_K(rotation)
T = K.stack( [[c, -s, 0., translation[0]],
[s, c, 0., translation[1]],
[0., 0., 1., translation[2]],
[0., 0., 0., 1.]], axis=0)
# We have 2 inputs, so have to return 2 outputs for `K.map_fn()`:
return T, 0.
def T_matrix_K_y(x):
rotation, translation = x[0], x[1]
c, s = trig_K(rotation)
T = K.stack( [ [c, 0.,-s, translation[0]],
[0., 1., 0., translation[1]],
[s, 0., c, translation[2]],
[0., 0., 0., .1]], axis=0)
# We have 2 inputs, so have to return 2 outputs for `K.map_fn()`:
return T, 0.
def FK_Keras(angs):
base_pos_K = K.constant(np.array([1, 2, 3])) # replace with your self.base_pos_K
shoulder_pos_K = K.constant(np.array([1, 2, 3])) # replace with your self.shoulder_pos_K
# Manually broadcast your constants to batches:
batch_size = K.shape(angs)[0]
base_pos_K = K.tile(K.expand_dims(base_pos_K, 0), (batch_size, 1))
shoulder_pos_K = K.tile(K.expand_dims(shoulder_pos_K, 0), (batch_size, 1))
# Compute local transformations, for each list of angles in the batch:
base_T, _ = K.map_fn(T_matrix_K_z, (angs[:, 0], base_pos_K))
shoulder_T, _ = K.map_fn(T_matrix_K_y, (angs[:, 1], shoulder_pos_K))
# ... (repeat with your other joints)
# Compute end effector transformation, over batch:
end_effector_T = K.batch_dot(base_T,shoulder_T) # add your other joints
# Compute Yaw, Pitch, Roll of end effector
y=K.tf.atan2(end_effector_T[:, 1,0],end_effector_T[:, 1,1])
p=K.tf.atan2(-end_effector_T[:, 2,0],K.tf.sqrt(end_effector_T[:, 2,1]*end_effector_T[:, 2,1]+end_effector_T[:, 2,2]*end_effector_T[:, 2,2]))
r=K.tf.atan2(end_effector_T[:, 2,1],end_effector_T[:, 2,2])
# Construct the output tensor [x,y,z,y,p,r]
output = K.stack([end_effector_T[:, 0,3],end_effector_T[:, 1,3],end_effector_T[:, 2,3], y, p, r], axis=1)
return output
# Demonstration:
input_data =np.array([[90., 180., 45., 25., 25.],[90., 180., 45., 25., 25.]])
sess = K.get_session()
inp = K.placeholder(shape=(None, 5), name="inp")#)
res = sess.run(FK_Keras(inp),inp: input_data)
model = Sequential()
model.add(Dense(5, input_dim=5))
model.add(Lambda(FK_Keras))
model.compile(optimizer="adam", loss='mse', metrics=['mse'])
【讨论】：
谢谢你的回答，我昨天在调试我的代码时实际上意识到了这一点。我没有时间发表评论。我的问题解决了。非常感谢以上是关于用标量角张量创建变换矩阵的主要内容，如果未能解决你的问题，请参考以下文章