将向量转换为另一个参考系
Transform a vector to another frame of reference
我有一辆绿色车辆，它很快就会与一个蓝色物体（距离立方体 200 远）相撞
它在 [-100,0,200] 处有一个 Kinect 深度相机 D，它可以看到立方体的角（灰色球体）
测量深度为 464，X 平面为 6.34°，Y 平面为 12.53°。
我想计算角的位置，如果在 [150,0,0] 处有一个相机 F，它会看到这个：
换句话说，将红色向量转换为黄色向量。我知道这是通过 transformation matrix 实现的，但我无法找到如何从 D-F 向量 [250,0,-200] 计算矩阵或如何使用它；我的 high-school 数学可以追溯到 40 年前。
math.se 有一个 similar question 但它没有解决我的问题，我在 robotices.se 上也找不到任何东西。
我意识到我应该展示一些我试过的代码，但我不知道从哪里开始。如果有人能帮我解决这个问题，我将不胜感激。
ROS 提供了 tf 库，允许您在帧之间进行转换。您可以简单地在相机的姿势和所需位置的姿势之间设置静态变换。然后，您可以在机器人上所需点的参考系中获取相机检测到的任何点的位姿。 ROS tf 将完成您需要的一切以及我在下面解释的一切。
较长的答案是您需要构建一个转换树。首先，计算两个姿势之间的静态变换。姿势是一个 7 维变换，包括平移和方向。这最好表示为四元数和 3D 向量。
现在，对于您的 kinect 参考系中的所有姿势，您必须将它们转换为您想要的参考系。让我们将此框架称为 base_link，将您的相机框架称为 camera_link。
我将继续并确定 base_link 是 camera_link 的父级。从技术上讲，这些转换是双向的，但因为您可能需要转换树，并且因为 ROS 关心这一点，所以您需要决定谁是父对象。
要将旋转从 camera_link 转换为 base_link，您需要计算旋转差异。这可以通过将 base_link 方向的四元数乘以 camera_link 方向的共轭来完成。这是一个超级快速的 Python 示例：
def rotDiff(self,q1: Quaternion,q2: Quaternion) -> Quaternion:
"""Finds the quaternion that, when applied to q1, will rotate an element to q2"""
conjugate = Quaternion(q2.qx*-1,q2.qy*-1,q2.qz*-1,q2.qw)
return self.rotAdd(q1,conjugate)
def rotAdd(self, q1: Quaternion, q2: Quaternion) -> Quaternion:
"""Finds the quaternion that is the equivalent to the rotation caused by both input quaternions applied sequentially."""
w1 = q1.qw
w2 = q2.qw
x1 = q1.qx
x2 = q2.qx
y1 = q1.qy
y2 = q2.qy
z1 = q1.qz
z2 = q2.qz
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return Quaternion(x,y,z,w)
接下来，您需要添加向量。天真的方法是简单地添加矢量，但在计算这些时需要考虑旋转。您真正需要的是坐标变换。 camera_link 相对于 base_link 的位置是一些 3D 向量。根据您的绘图，这是 [-250, 0, 200]。接下来，我们需要将向量重新投影到您感兴趣的点到 base_link 的旋转坐标系中。即，您的相机在 z = 0 平面上出现的所有相机在 12.53 度处看到的点实际上都在相对于 base_link 的 12.53 度平面上，您需要找出它们的坐标相对于你的相机就好像你的相机与 base_link.
的方向相同
有关后续数学的详细信息，请阅读此 PDF（特别是从第 9 页开始）。
为此，我们需要在 base_link 的参考系中找到您的矢量组件。我发现将四元数转换为旋转矩阵最容易阅读，但也有等效的直接方法。
要将四元数转换为旋转矩阵：
def Quat2Mat(self, q: Quaternion) -> rotMat:
m00 = 1 - 2 * q.qy**2 - 2 * q.qz**2
m01 = 2 * q.qx * q.qy - 2 * q.qz * q.qw
m02 = 2 * q.qx * q.qz + 2 * q.qy * q.qw
m10 = 2 * q.qx * q.qy + 2 * q.qz * q.qw
m11 = 1 - 2 * q.qx**2 - 2 * q.qz**2
m12 = 2 * q.qy * q.qz - 2 * q.qx * q.qw
m20 = 2 * q.qx * q.qz - 2 * q.qy * q.qw
m21 = 2 * q.qy * q.qz + 2 * q.qx * q.qw
m22 = 1 - 2 * q.qx**2 - 2 * q.qy**2
result = [[m00,m01,m02],[m10,m11,m12],[m20,m21,m22]]
return result
现在您的旋转已表示为旋转矩阵，是时候进行最后的计算了。
根据我上面 link 的麻省理工学院讲义，我将任意命名矢量到您来自相机的兴趣点 A。
找到表示base_link和camera_link之间旋转的四元数对应的旋转矩阵，简单地进行一次矩阵乘法。如果您在 Python 中，则可以使用 numpy 来执行此操作，但为了明确起见，这里是乘法的长形式：
def coordTransform(self, M: RotMat, A: Vector) -> Vector:
"""
M is my rotation matrix that represents the rotation between my frames
A is the vector of interest in the frame I'm rotating from
APrime is A, but in the frame I'm rotating to.
"""
APrime = []
i = 0
for component in A:
APrime.append(component * M[i][0] + component * M[i][1] + component * m[i][2])
i += 1
return APrime
现在，来自 camera_link 的向量被表示为好像 camera_link 和 base_link 共享一个方向。
现在您可以简单地添加 camera_link 和 base_link 之间的静态翻译（或减去 base_link -> camera_link），得到的向量将是您的观点的新翻译。
综上所述，您现在可以收集相机检测到的每个点相对于任意参考系的平移和方向，以收集与您的应用程序相关的姿势数据。
您可以将所有这些放在一个简单地称为 tf() 的函数中，并将这些转换向上和向下堆叠成一个复杂的转换树。只需将所有转换添加到一个共同的祖先并减去所有转换到您的目标节点，以便找到任意两个任意相关帧之间的数据转换。
编辑：Hendy 指出不清楚 Quaternion() class 我在这里指的是什么。
为了这个答案的目的，这就是所有必要的：
class Quaternion():
def __init__(self, qx: float, qy: float, qz: float, qw: float):
self.qx = qx
self.qy = qy
self.xz = qz
self.qw = qw
但是如果你想让这个 class 超级方便，你可以定义 __mul__(self, other: Quaternion 和 __rmul__(self, other: Quaternion) 来执行四元数乘法（顺序很重要，所以一定要两者都做！） . conjugate(self)、toEuler(self)、toRotMat(self)、normalize(self) 也可能是方便的补充。
请注意，由于 Python 的打字怪癖，上面的 other: Quaternion 只是为了清楚起见。你需要一个 longer-form if type(other) != Quaternion: raise TypeError('You can only multiply quaternions with other quaternions) 错误处理块来使它成为有效的 python :)
以下定义对于此答案不是必需的，但它们可能对 reader.
import numpy as np
def __mul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = self.qw
r2 = other.qw
v1 = [self.qx,self.qy,self.qz]
v2 = [other.qx,other.qy,other.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def __rmul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = other.qw
r2 = self.qw
v1 = [other.qx,other.qy,other.qz]
v2 = [self.qx,self.qy,self.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def conjugate(self):
return Quaternion(self.qx*-1,self.qy*-1,self.qz*-1,self.qw)
我有一辆绿色车辆，它很快就会与一个蓝色物体（距离立方体 200 远）相撞
它在 [-100,0,200] 处有一个 Kinect 深度相机 D，它可以看到立方体的角（灰色球体）
测量深度为 464，X 平面为 6.34°，Y 平面为 12.53°。
我想计算角的位置，如果在 [150,0,0] 处有一个相机 F，它会看到这个：
换句话说，将红色向量转换为黄色向量。我知道这是通过 transformation matrix 实现的，但我无法找到如何从 D-F 向量 [250,0,-200] 计算矩阵或如何使用它；我的 high-school 数学可以追溯到 40 年前。
math.se 有一个 similar question 但它没有解决我的问题，我在 robotices.se 上也找不到任何东西。
我意识到我应该展示一些我试过的代码，但我不知道从哪里开始。如果有人能帮我解决这个问题，我将不胜感激。
ROS 提供了 tf 库，允许您在帧之间进行转换。您可以简单地在相机的姿势和所需位置的姿势之间设置静态变换。然后，您可以在机器人上所需点的参考系中获取相机检测到的任何点的位姿。 ROS tf 将完成您需要的一切以及我在下面解释的一切。
较长的答案是您需要构建一个转换树。首先，计算两个姿势之间的静态变换。姿势是一个 7 维变换，包括平移和方向。这最好表示为四元数和 3D 向量。
现在，对于您的 kinect 参考系中的所有姿势，您必须将它们转换为您想要的参考系。让我们将此框架称为 base_link，将您的相机框架称为 camera_link。
我将继续并确定 base_link 是 camera_link 的父级。从技术上讲，这些转换是双向的，但因为您可能需要转换树，并且因为 ROS 关心这一点，所以您需要决定谁是父对象。
要将旋转从 camera_link 转换为 base_link，您需要计算旋转差异。这可以通过将 base_link 方向的四元数乘以 camera_link 方向的共轭来完成。这是一个超级快速的 Python 示例：
def rotDiff(self,q1: Quaternion,q2: Quaternion) -> Quaternion:
"""Finds the quaternion that, when applied to q1, will rotate an element to q2"""
conjugate = Quaternion(q2.qx*-1,q2.qy*-1,q2.qz*-1,q2.qw)
return self.rotAdd(q1,conjugate)
def rotAdd(self, q1: Quaternion, q2: Quaternion) -> Quaternion:
"""Finds the quaternion that is the equivalent to the rotation caused by both input quaternions applied sequentially."""
w1 = q1.qw
w2 = q2.qw
x1 = q1.qx
x2 = q2.qx
y1 = q1.qy
y2 = q2.qy
z1 = q1.qz
z2 = q2.qz
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return Quaternion(x,y,z,w)
接下来，您需要添加向量。天真的方法是简单地添加矢量，但在计算这些时需要考虑旋转。您真正需要的是坐标变换。 camera_link 相对于 base_link 的位置是一些 3D 向量。根据您的绘图，这是 [-250, 0, 200]。接下来，我们需要将向量重新投影到您感兴趣的点到 base_link 的旋转坐标系中。即，您的相机在 z = 0 平面上出现的所有相机在 12.53 度处看到的点实际上都在相对于 base_link 的 12.53 度平面上，您需要找出它们的坐标相对于你的相机就好像你的相机与 base_link.
有关后续数学的详细信息，请阅读此 PDF（特别是从第 9 页开始）。
为此，我们需要在 base_link 的参考系中找到您的矢量组件。我发现将四元数转换为旋转矩阵最容易阅读，但也有等效的直接方法。
要将四元数转换为旋转矩阵：
def Quat2Mat(self, q: Quaternion) -> rotMat:
m00 = 1 - 2 * q.qy**2 - 2 * q.qz**2
m01 = 2 * q.qx * q.qy - 2 * q.qz * q.qw
m02 = 2 * q.qx * q.qz + 2 * q.qy * q.qw
m10 = 2 * q.qx * q.qy + 2 * q.qz * q.qw
m11 = 1 - 2 * q.qx**2 - 2 * q.qz**2
m12 = 2 * q.qy * q.qz - 2 * q.qx * q.qw
m20 = 2 * q.qx * q.qz - 2 * q.qy * q.qw
m21 = 2 * q.qy * q.qz + 2 * q.qx * q.qw
m22 = 1 - 2 * q.qx**2 - 2 * q.qy**2
result = [[m00,m01,m02],[m10,m11,m12],[m20,m21,m22]]
return result
现在您的旋转已表示为旋转矩阵，是时候进行最后的计算了。
根据我上面 link 的麻省理工学院讲义，我将任意命名矢量到您来自相机的兴趣点 A。
找到表示base_link和camera_link之间旋转的四元数对应的旋转矩阵，简单地进行一次矩阵乘法。如果您在 Python 中，则可以使用 numpy 来执行此操作，但为了明确起见，这里是乘法的长形式：
def coordTransform(self, M: RotMat, A: Vector) -> Vector:
"""
M is my rotation matrix that represents the rotation between my frames
A is the vector of interest in the frame I'm rotating from
APrime is A, but in the frame I'm rotating to.
"""
APrime = []
i = 0
for component in A:
APrime.append(component * M[i][0] + component * M[i][1] + component * m[i][2])
i += 1
return APrime
现在，来自 camera_link 的向量被表示为好像 camera_link 和 base_link 共享一个方向。
现在您可以简单地添加 camera_link 和 base_link 之间的静态翻译（或减去 base_link -> camera_link），得到的向量将是您的观点的新翻译。
综上所述，您现在可以收集相机检测到的每个点相对于任意参考系的平移和方向，以收集与您的应用程序相关的姿势数据。
您可以将所有这些放在一个简单地称为 tf() 的函数中，并将这些转换向上和向下堆叠成一个复杂的转换树。只需将所有转换添加到一个共同的祖先并减去所有转换到您的目标节点，以便找到任意两个任意相关帧之间的数据转换。
编辑：Hendy 指出不清楚 Quaternion() class 我在这里指的是什么。
为了这个答案的目的，这就是所有必要的：
class Quaternion():
def __init__(self, qx: float, qy: float, qz: float, qw: float):
self.qx = qx
self.qy = qy
self.xz = qz
self.qw = qw
但是如果你想让这个 class 超级方便，你可以定义 __mul__(self, other: Quaternion 和 __rmul__(self, other: Quaternion) 来执行四元数乘法（顺序很重要，所以一定要两者都做！） . conjugate(self)、toEuler(self)、toRotMat(self)、normalize(self) 也可能是方便的补充。
请注意，由于 Python 的打字怪癖，上面的 other: Quaternion 只是为了清楚起见。你需要一个 longer-form if type(other) != Quaternion: raise TypeError('You can only multiply quaternions with other quaternions) 错误处理块来使它成为有效的 python :)
以下定义对于此答案不是必需的，但它们可能对 reader.
import numpy as np
def __mul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = self.qw
r2 = other.qw
v1 = [self.qx,self.qy,self.qz]
v2 = [other.qx,other.qy,other.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def __rmul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = other.qw
r2 = self.qw
v1 = [other.qx,other.qy,other.qz]
v2 = [self.qx,self.qy,self.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def conjugate(self):
return Quaternion(self.qx*-1,self.qy*-1,self.qz*-1,self.qw)