slam中变换矩阵的逆推导

最新推荐文章于 2024-06-17 21:21:54 发布

ergevv

最新推荐文章于 2024-06-17 21:21:54 发布

阅读量735

点赞数 19

分类专栏： slam 文章标签：矩阵 slam 变换矩阵

本文链接：https://blog.csdn.net/ergevv/article/details/137586025

版权

slam 专栏收录该内容

22 篇文章 0 订阅

订阅专栏

在SLAM（Simultaneous Localization And Mapping，同时定位与建图）中，变换矩阵通常用来描述一个坐标系相对于另一个坐标系的位置和姿态变化。这类矩阵通常包括旋转部分和平移部分，可以表示为齐次坐标下的4x4矩阵形式：

$\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}$

其中：

$\mathbf{R}$ 是一个3x3的正交旋转矩阵，表示坐标系之间的旋转关系；
$\mathbf{t}$ 是一个3x1的列向量，表示坐标系之间的平移向量；
$\mathbf{0}$ 是一个3x1的零向量，确保矩阵乘法规则下平移部分正确应用；
$1$ 是齐次坐标系中的标量分量，保证齐次坐标的性质。

变换矩阵的逆 $\mathbf{T}^{-1}$ 表示从目标坐标系回到原坐标系的变换，它用来对坐标进行逆向映射。对于上述形式的变换矩阵，其逆矩阵可以通过以下方式计算：

$\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}$

这里：

$\mathbf{R}^T$ 是旋转矩阵 $\mathbf{R}$ 的转置，也是其逆矩阵，因为正交旋转矩阵满足 $\mathbf{R}^T = \mathbf{R}^{-1}$ ；
$-\mathbf{R}^T \mathbf{t}$ 是将平移向量按照旋转矩阵的逆方向进行反向平移。

推导过程如下：

要求 $\mathbf{T}^{-1}$ 使得 $\mathbf{T} \cdot \mathbf{T}^{-1} = \mathbf{I}$ ，其中 $\mathbf{I}$ 是单位矩阵。展开得到：

$\begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \cdot \begin{bmatrix} \mathbf{A} & \mathbf{b} \\ \mathbf{c}^T & d \end{bmatrix} = \begin{bmatrix} \mathbf{I}_3 & \mathbf{0} \\ \mathbf{0}^T & 1 \end{bmatrix}$

分别对应元素相乘并利用矩阵乘法的规则，可以得出：

对于旋转和平移部分的乘积应为单位矩阵，即 $\mathbf{RA} = \mathbf{I}_3$ ，且 $\mathbf{Rb} + \mathbf{t} = \mathbf{0}$ 。这表明 $\mathbf{A} = \mathbf{R}^T$ （旋转矩阵的逆就是其转置），且 $\mathbf{b} = -\mathbf{R}^T \mathbf{t}$ （反向平移）。
对于底部的齐次坐标，有 $\mathbf{0}^T \mathbf{A} = \mathbf{0}^T$ 和 $\mathbf{0}^T \mathbf{b} + c = 0$ 。由于 $\mathbf{A} = \mathbf{R}^T$ 是一个3x3的旋转矩阵，其最后一列不会影响 $\mathbf{0}^T \mathbf{A}$ 的结果。而 $\mathbf{0}^T \mathbf{b}$ 无论如何都为零，因此 $c$ 可以是任意值。但在齐次坐标下，通常选择 $c = 0$ 以保持简洁。
最后，对于右下角的标量乘积，有 $\cdot 1 = 1$ ，这意味着 $d = 1$ 。