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 <Eigen/Dense>
#include <iostream>
#include <math.h>
int main(int argc, char** argv)
{
    std::cout << "Hello world"<<std::endl;

    Eigen::Matrix4f b1;
    b1 << 1,0,0,0,
            0,1,0,0,
            0,0,1,0,
            0,0,0,1;
    std::cout << "b1 : " << std::endl<<b1<<std::endl;

    Eigen::Matrix4f t1;
    float theta1 = M_PI/4.0;
    t1 <<   cos(theta1),-sin(theta1),0,0,
            sin(theta1),cos(theta1),0,0,
            0,0,1,0,
            0,0,0,1;
    Eigen::Matrix4f b2;
    b2 = t1 * b1;
    std::cout << "b2 : "<<std::endl<<b2<<std::endl;
    
    Eigen::Matrix4f t2;
    float theta2 = M_PI/2.0;
    t2 <<   1,0,0,0,
            0,cos(theta2),-sin(theta2),0,
            0,sin(theta2),cos(theta2),0,
            0,0,0,1;
    Eigen::Matrix4f b3;
    b3 = t1 * t2 * b1;
    std::cout << "b3 : "<<std::endl<<b3<<std::endl;

    Eigen::Matrix4f t3;
    t3 <<   1,0,0,2,
            0,1,0,1,
            0,0,1,0,
            0,0,0,1;
    Eigen::Matrix4f b4;
    b4 = t1 * t2 * t3 * b1;
    std::cout << "b4 : "<<std::endl<<b4<<std::endl;

    Eigen::Vector4f pb4,pb1;
    pb4 << 1,1,0,1;

    pb1 = b4 * pb4;
    std::cout <<"pb1 : "<<std::endl<<pb1<<std::endl;

/*
    Eigen::Matrix4f t4;
    t4 <<   1,0,0,1.41421,
            0,1,0,1.41421,
            0,0,1,1,
            0,0,0,1;
    b4 = t4 * t1 * t2 * b1;
    std::cout << "b4 : "<<std::endl<<b4<<std::endl;
*/
    
    Eigen::Matrix4f t4=t1,t5=t2,t6=t3;

    Eigen::Matrix4f b5,b6,b7;
    b5 = t4 * b1;
    std::cout << "b5 : "<<std::endl<<b5<<std::endl;

    b6 = t5 * t4 * b1;
    std::cout << "b6 : "<<std::endl<<b6<<std::endl;

    b7 = t6 * t5 * t4 * b1;
    std::cout << "b7 : "<<std::endl<<b7<<std::endl;



    return 0;
}