将向量变换到另一个参考系

Question

我有一辆绿色车辆，它将很快与一个蓝色物体（距离立方体 200 米）相撞\n

\n\n

它在 [-100,0,200] 处有一个 Kinect 深度相机 D，可以看到立方体的角（灰色球体）\n

\n\n

测量的深度为 464，位于 X 平面中的 6.34\xc2\xb0 处和 Y 平面中的 12.53\xc2\xb0 处。\n\n

\n\n

我想计算角点的位置，如果在 [150,0,0] 处有一个摄像机 F，它会出现这样的位置：\n

\n\n

换句话说，将红色向量变换为黄色向量。我知道这是通过变换矩阵实现的，但我不知道如何从 DF 向量 [250,0,-200] 计算矩阵或如何使用它；我的高中数学可以追溯到 40 年前。

\n\n

math.se 有一个类似的问题，但它没有涵盖我的问题，而且我在 robots.se 上也找不到任何内容。

\n\n

我意识到我应该展示一些我尝试过的代码，但我不知道从哪里开始。如果有人能帮助我解决这个问题，我将非常感激。

\n

Answer 1

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。也就是说，您的相机在 12.53 度的平面上看到的所有点z = 0实际上都在相对于您相机的 12.53 度平面上base_link，您需要找出它们相对于您的相机的坐标，就好像您的相机位于同一平面上一样方向为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

现在您的旋转已表示为旋转矩阵，是时候进行最终计算了。

按照上面链接中的麻省理工学院讲义，我将任意命名矢量到您从相机中感兴趣的点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()我在这里指的是哪一类。

出于本答案的目的，这就是必要的：

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)

但如果你想让这个类超级方便，你可以定义__mul__(self, other: Quaternion并__rmul__(self, other: Quaternion)执行四元数乘法（顺序很重要，所以请确保两者都做！）。conjugate(self), toEuler(self), toRotMat(self),normalize(self)也可能是方便的补充。

请注意，由于 Python 打字的怪癖，以上内容other: Quaternion只是为了清楚起见。您需要一个较长形式的if type(other) != Quaternion: raise TypeError('You can only multiply quaternions with other quaternions)错误处理块才能使其成为有效的 python :)

以下定义对于这个答案来说不是必需的，但它们可能对读者有用。

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