从 GPS 位置和 KITTI 原始数据集的（滚动、俯仰、偏航）计算姿势/变换矩阵（旋转和平移）

Question

我正在尝试计算来自 KITTI 原始数据集的视频中两帧之间的相对姿势。该oxts数据提供了lat, lon, alt, roll, pitch, yaw对每个帧。如何将此数据转换为转换矩阵（旋转矩阵和平移向量）？

这个答案表明这是可能的，但没有给出解决方案。Python 是首选，但如果您有其他语言的解决方案，那也很好。我可以将代码翻译成python。

样本数据：
lat, lon, alt, roll, pitch, yaw = 49.015003823272, 8.4342971002335, 116.43032836914, 0.035752, 0.00903, -2.6087069803847

PS：我正在尝试使用投影几何将一帧姿势扭曲到另一帧。为此，我需要姿势、深度和相机矩阵。KITTI raw 提供相机矩阵。我打算从立体图像计算深度。所以，我只剩下计算它们之间的姿势/变换矩阵

Answer 1

相机位置由外部变换和内部变换定义。在 Kitti 的情况下，摄像机只能相对于世界旋转和平移（摄像机的本质应该始终相同，因为只是汽车在空间中移动）。简而言之：

• 相机内在矩阵将相机坐标给定的点投影到相机的图像平面上
• 相机外参矩阵是基础矩阵的变化，将点的坐标从世界坐标系转换到相机坐标系

现在考虑：

c=[R|t]*w

其中
c - 相机点的齐次坐标
[R|t] - 相机外在矩阵
w - 世界空间中点的坐标

从 kitti 提供的数据开始，使用 oxts 包来计算 6dof 尊重地球的变换。

def rotx(t):
    """Rotation about the x-axis."""
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[1,  0,  0],
                     [0,  c, -s],
                     [0,  s,  c]])


def roty(t):
    """Rotation about the y-axis."""
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[c,  0,  s],
                     [0,  1,  0],
                     [-s, 0,  c]])


def rotz(t):
    """Rotation about the z-axis."""
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[c, -s,  0],
                     [s,  c,  0],
                     [0,  0,  1]])

def transform_from_rot_trans(R, t):
    """Transforation matrix from rotation matrix and translation vector."""
    R = R.reshape(3, 3)
    t = t.reshape(3, 1)
    return np.vstack((np.hstack([R, t]), [0, 0, 0, 1]))

def pose_from_oxts_packet(metadata):

    """Helper method to compute a SE(3) pose matrix from an OXTS packet.
    Taken from https://github.com/utiasSTARS/pykitti
    """
    lat, lon, alt, roll, pitch, yaw = metadata
    scale = np.cos(lat * np.pi / 180.)

    er = 6378137.  # earth radius (approx.) in meters
    # Use a Mercator projection to get the translation vector
    ty = lat * np.pi * er / 180.

    tx = scale * lon * np.pi * er / 180.
    # ty = scale * er * \
    #     np.log(np.tan((90. + lat) * np.pi / 360.))
    tz = alt
    t = np.array([tx, ty, tz]).reshape(-1,1)

    # Use the Euler angles to get the rotation matrix
    Rx = rotx(roll)
    Ry = roty(pitch)
    Rz = rotz(yaw)
    R = Rz.dot(Ry.dot(Rx))
    return transform_from_rot_trans(R, t)

那么函数调用将使用从 oxts 文件读取的元数据：

metadata = fromtxt("drive/oxts/data/0000000000.txt")
pose_matrix = pose_from_oxts_packet(metadata[:6])

参考：Repo kitti 数据集https://github.com/utiasSTARS/pykitti