位姿矩阵求逆

为了更好的说明问题，我们先来看一下位姿矩阵的定义。

位姿矩阵分析

如下图所示：
如果在B坐标系下有一点PB，我们需要知道该点在坐标系A下的坐标PA，怎么求呢？

通过位姿变换矩阵，位姿变换矩阵如下：

其中的含义是：式子中的
R分量为旋转分量3×3的矩阵
t分量为平移分量3×1的向量。
其分别对应关系是什么呢?
R 表示的 A 坐标系旋转成 B 坐标系需要进行怎么样的旋转。
其有一种计算方法为：B 坐标系的XYZ向量在 A 坐标系下的表示，并且将其归一化，以列向量的形式进行书写，构成3×3的矩阵。即X分量写在第一列，Y在第二列，Z在第三列
t 表示由 A 的原点指向 B 的原点矢量，在 A 坐标系下的分量（表示）。
绿色的箭头为平移向量的表示。

位姿矩阵求逆矩阵

如果我们想知道 PA 如何转换成 PB ，即将以下公式进行求逆的操作：

我们可以直接对该位姿变换矩阵进行求逆的操作。
但是，我们知道，位姿矩阵中的旋转矩阵是一个标准的
正交矩阵。
证明链接: 旋转矩阵是正交矩阵的证明

可以通过这个性质来更加快速的求出位姿矩阵的逆矩阵。

正交矩阵的性质：

• 1)AT是正交矩阵
• 2)A*AT=E（E为单位矩阵）
• 3)AT的各行是单位向量且两两正交
• 4)AT的各列是单位向量且两两正交
• 5)(Ax,Ay)=(x,y)x,y∈R
• 6)|A|=1或-1
• 7)正交矩阵通常用字母Q表示。
• 8）A的转置等于A的逆

通过性质8我们可以知道，旋转矩阵的逆就等于它的转置。
那么，一个位姿矩阵的逆怎么表示：
假如说，一个位姿矩阵为：

那么，它的逆可以快速求出：

即旋转分量的逆为转置，平移分量的逆为 （-R * t） （其中R为3×3矩阵，t 为3×1的矩阵）；
注意：平移有负号

证明过程为：

参考博客：https://blog.csdn.net/weixin_33674976/article/details/89666910