slam小单元——位姿矩阵

这个系列是对slam中的一些小概念做理解和简单的测试

位姿矩阵

这个反应的是坐标系和坐标系之间的关系，
作用：

1. 移动向量
2. 将一个坐标系下的向量（坐标）表达在另一个坐标系下

如下图所示，在最开始的时候，坐标系xyz和x’y’z’重合。
t1：绕z’轴旋转45°
t2：绕x‘轴旋转90°
t3：沿x’轴运动2，再沿y‘运动1
与之相对应的是：
t4：绕z轴旋转45°
t5：绕x轴旋转90°
t6：沿x轴运动2，再沿y轴运动1
那么：
（1）b2 = t1 * b1
得到的b2矩阵是，坐标系2在坐标系1下的表示，也就是${}_2^{1}T$

（2）b3 = t1 * t2 * b1
得到的b3矩阵是，坐标系3在坐标系1下的表示，也就是${}_3^{1}T$
注意沿着自身轴的运动，是右乘

（3）b4 = t1 * t2 * t3 * b1
得到的b4矩阵是，坐标系4在坐标系1下的表示，也就是${}_4^{1}T$
注意沿着自身轴的运动，是右乘

（4）b5 = t4 * b1
得到的b5矩阵是，坐标系5在坐标系1下的表示，也就是${}_5^{1}T$

（5）b6 = t5 * t4 * b1 = t2 * t1 * b1
得到的b6矩阵是，坐标系6在坐标系1下的表示，也就是${}_6^{1}T$
注意沿着基坐标系轴的运动，是左乘

（6）b7 = t6 * t5 * t4 * b1 = t3 * t2 * t1 * b1
得到的b7矩阵是，坐标系7在坐标系1下的表示，也就是${}_7^{1}T$
注意沿着基坐标系轴的运动，是左乘

总结：
为了寻找两个坐标系之间的关系，需要从基坐标系——1系——出发，沿着自身，或者沿着固定的1系进行运动，得到一个新的2系。此时，可以通过右乘或者左乘的方法，得到${}_5^{1}T$.

测试代码

#include 
#include 
#include 
int main(int argc, char** argv)
{
    std::cout << "Hello world"<