[SLAM] 动态SLAM中的矩阵计算

1. 基本要素

O表示物体坐标系，C表示相机坐标系，W表示世界坐标系；
下标1表示时刻1，下标2表示时刻2；

2. 同一时刻内的关系

对于object上一个点，有下图关系；
①其在物体坐标系O到相机坐标系C的坐标变换表示物体$O$相对于相机$C$的位姿，记为$L_{co}$，用绿色线表示；
该位姿往往是多目标追踪方法中给出的物体位姿形式；
②其在相机坐标系C到世界坐标系W的坐标变换表示相机的位姿，记为$T_{wc}$，用绿色线表示；
该位姿往往是SLAM算法中给出的传感器位姿形式；

③此时有在物体坐标系O到世界坐标系W的坐标变换表示物体$O$相对于世界坐标系$W$的位姿，记为$L_{wo}$，如下图所示，用蓝色线表示
有$L_{wo}=T_{wc}\ast L_{co}$

3. 相邻时刻间的关系

此时定义两个变换

3.1 $H_{motion}$

点在时刻2物体坐标系下到时刻1物体坐标系的坐标变换，表示$O_1$相对于$O_2$的位姿变换，记为$H_{motion}$，用玫红色线表示
有$H_{motion}={L_{w{o}_1}}^{-1}\ast L_{w{o}_2}$，其反应了目标坐标系$O$在两个时刻之间的变化；

3.2 $H_{point}$

点在世界坐标系下，在时刻1的坐标到时刻2的坐标的坐标变换，表示一个点在两个时刻的之间的运动，记为$H_{point}$，用黄色线表示
有$H_{point}=L_{wo2}\ast {L_{w{o}_1}}^{-1}$，其中
①${L_{w{o}_1}}^{-1}$表示黄色点在时刻1下世界坐标系$W$转换到物体坐标系$O_1$；
②由于刚体假设其在物体坐标系$O_1$+时刻1的坐标 = 其在物体坐标系$O_2$+时刻2的坐标；
③此时通过$L_{wo2}$就再次将时刻2下物体坐标系$O_2$转换到世界坐标系$W$；

3.3 两个H间关系

根据两个H的表达式：
$H_{point}=L_{wo2}\ast {L_{w{o}_1}}^{-1}=(T_{w{c}_2}\ast L_{c{o}_2})\ast (T_{w{c}_1}\ast L_{c{o}_1}{)}^{-1}$
$H_{motion}={L_{w{o}_1}}^{-1}\ast L_{w{o}_2}=(T_{w{c}_1}\ast L_{c{o}_1}{)}^{-1}\ast (T_{w{c}_2}\ast L_{c{o}_2})$
可以很容易地看出他们的关系为，两个H变换可以通过两个时刻的物体位姿进行转换：
$H_{motion}=L_{wo1}^{-1}\ast H_{point}\ast L_{wo1}=L_{wo2}^{-1}\ast H_{point}\ast L_{wo2}$
$H_{point}=L_{wo1}\ast H_{motion}\ast L_{wo1}^{-1}=L_{wo2}\ast H_{motion}\ast L_{wo2}^{-1}$
注意第一个式子，对于两个时刻间坐标系变换$H_{motion}$，由于时刻1的物体位姿$L_{wo1}$已经求取，可由点在世界坐标系下的坐标变换$H_{point}$表出；

4. H矩阵求取

若不考虑目标的运动，直接利用目标上的匹配点求解位姿，可得到一个虚拟的相机位姿$C_2{}^{\prime }$，不难看出$C_2$与$C_2{}^{\prime }$的相对位姿和$O_2$与$O_1$的相对位姿是一样的；
根据前文已经知道$H_{motion}$表示的是2时刻坐标系相对于1时刻坐标系的位姿；那么就可以通过两个相机位姿得到$H_{motion}=T_{w{c}_2^{\prime }}^{-1}\ast T_{wc2}$
$H_{motion}$矩阵的求解如下图所示：

同理$H_{point}$表示世界坐标系下点坐标随着时间的变换，有$H_{point}=T_{wc2}\ast T_{w{c}_2^{\prime }}^{-1}$