Tbw Tb->w 不是世界系到机体系的变换矩阵,是机体系到世界系的变换矩阵,把机体系变到世界系!!!求机体系在世界系下的位姿,不是把世界系变到机体系,而是机体系变到世界系,比如机体系原点,在机体系下坐标是(0,0,0),但是在世界系下就是一个非零值,这个坐标是世界系下的坐标不是机体系下的。
所以变换矩阵相乘按照左乘(坐标是列向量)是写成下面这样
eigen里也默认是左乘,左乘是从最右边向左边乘,依次乘以左边的。
我发现他们写右下角标的顺序和我相反,T12就是从坐标系2到坐标系1的变换,也是坐标系2在坐标系1下的位姿,右边就是写坐标系1的坐标,这样就不会乱,而你之前的写法是T12,导致出现右边写的坐标系2的坐标!!!