SLAM：三维空间刚体运动

高翔老师在《视觉SLAM高博十四讲》视频第3讲三维空间刚体运动中提了一个问题，觉得这个问题很有意思，故记录如下

世界坐标系到相机坐标系的变换矩阵是Twr

相机坐标系到世界坐标系的变换矩阵式Trw

则相机在世界坐标系下的轨迹坐标是Twr中的平移向量还是Trw中的平移向量

上述问题简化如下：

一个坐标系A的原点在另一个坐标系B中如何表示

解法如下：

将PA=[ 0 T ^T T 1 ] T ^T T代入

PB=TBAPA

得到 PB = [t T ^T T 1] T ^T T

即坐标系A的原点在坐标系B中的坐标为B到A的变换矩阵中的平移向量