Eigen 中旋转矩阵、旋转向量、欧拉角、四元数的使用

用到 Eigen/Core 和 Eigen/Geometry 模块

定义一个$3\times 3$ 矩阵并使用单位阵对其初始化：矩阵名称 = Eigen::Matrix3d::Identity()

定义一个旋转向量：Eigen::AngleAxised 矩阵名称(旋转角*(弧度)，旋转轴($3\times 1$)*)

输出精度设定：cout.precision(val) (小数点后保留 val 位)

旋转向量转换成旋转矩阵：

• 矩阵名称 = 向量名称.toRotationMatrix()

• 矩阵名称 = 向量名称.matrix()

旋转变换 ：旋转后向量名称 = 旋转矩阵 $\ast$ 旋转前向量名称

旋转矩阵转换为欧拉角：向量名称_(存储欧拉角)_ = 旋转矩阵名称.eulerAngles(2, 1, 0) (数字2，1，0 代表ZYX顺序)

定义一个欧式变换矩阵并初始化：Eigen::Isometry3d 变换矩阵名称 = Eigen::Isometry3d::Identity()

设定欧式变换矩阵中的旋转矩阵：变换矩阵名称.rotate(旋转向量)

设定欧式变换矩阵中的平移向量：变换矩阵名称.pretranslate(平移向量)

提取变换矩阵：变换矩阵名称.matrix()

利用变换矩阵进行坐标变换：变换后矩阵名称 = 变换矩阵名称 $\ast$ 变换前矩阵名称

定义一个四元数：Eigen::Quaterniond 四元数名称

四元数归一化：

• 四元数名称.normalize()
• 四元数名称 = 四元数名称.normalized()

将旋转向量转换为四元数：Eigen::Quaterniond (旋转向量名称)

将旋转矩阵转换为四元数：Eigen::Quaterniond (旋转矩阵名称)

四元数系数向量：四元数名称.coeffs() (系数顺序 x, y, z, w 。其中 w 为实部，其余为虚部)

四元数虚部向量：四元数名称.vec()

四元数反旋转：四元数名称.conjugate()

四元数转旋转向量：旋转向量名称 = 四元数名称

四元数转旋转矩阵：旋转矩阵名称 = 四元数名称.matrix()

四元数旋转变换：旋转后的向量 = 四元数 $\ast$ 旋转前的向量

扩展学习：Eigen矩阵库使用说明

#include 
#include 
#include 
#include 
          
using namespace std;

int main(int argc, char **argv)
 {        
        Eigen::Quaterniond q_1(0.2, 0.3, 0.1, 0.35);
        Eigen::Quaterniond q_2(0.4, -0.1, 0.2, -0.5);
        q_1.normalize();
        q_2.normalize();
        Eigen::Vector3d t_1(0.3, 0.1, 0.1);
        Eigen::Vector3d t_2(-0.1, 0.5, 0.3);
        Eigen::Vector3d p_1(0.5, 0, 0.2);                                                                                                                            
        Eigen::Vector3d p_2;
        Eigen::Matrix3d R_1 = q_1.matrix();
        Eigen::Matrix3d R_2 = q_2.matrix();
        Eigen::Isometry3d T_1 = Eigen::Isometry3d::Identity();
        Eigen::Isometry3d T_2 = Eigen::Isometry3d::Identity();
        T_1.rotate(R_1);
        T_2.rotate(R_2);
        T_1.pretranslate(t_1);
        T_2.pretranslate(t_2);
         
        p_2 = T_2 * T_1.inverse() * p_1;
        cout.precision(2);
        cout << p_2.transpose() << endl;
          
        Eigen::Isometry3d T_1w(q_1), T_2w(q_2);
        cout << "befor : \n" << T_1w.matrix() << endl;
        T_1w.pretranslate(t_1);
        cout << "after : \n" << T_1w.matrix() << endl;
        T_2w.pretranslate(t_2);
        p_2 = T_2w * T_1w.inverse() * p_1;
        cout << p_2.transpose() << endl;
         
        return 0;
 }