Question

我有一辆绿色车辆很快就会碰到一个蓝色物体（离立方体有200个）

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

测量深度为464，在X平面为6.34°，在Y平面为12.53°。

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

换句话说，将红色矢量转换为黄色矢量。我知道这是通过transformation matrix实现的，但是我无法找到如何从D-F矢量[250,0，-200]计算矩阵或如何使用它;我的高中数学可以追溯到40年前。

math.se有一个similar question，但它没有解决我的问题，我在robotices.se上也找不到任何东西。

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

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的旋转框架。即，您的相机在相机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

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

根据我上面链接的麻省理工学院讲义，我将从摄像机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()的函数中，并在复杂的转换树中上下堆叠这些转换。只需将所有转换添加到共同的祖先，然后将所有转换减去目标节点，以便在任意两个任意相关帧之间找到数据转换。

将矢量转换为另一个参照系

1 个答案: