三维空间中刚体的变换旋转和平移

这里旋转主要可以采用旋转向量， 旋转矩阵， 欧拉角，四元数。

我们也能反向从坐标轴表现形式得到旋转矩阵：

欧拉角是采用偏航， 俯仰， 滚转（yaw, pitch, roll来表示）

这里是先绕Z， 再绕Y，最后绕X旋转得到的

四元数：q = [cos(a/2) , Nxsin(a/2), Nxsin(a/2), Nzsin(a/2)]

两个点之间的变换关系可以表示为：

                                p1 = q *p * (q)-1

#include <iostream>
#include<cmath>

using namespace std;

#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
int main(int argc, char **argv) {
    Eigen::Matrix3d  rotation_matrix = Eigen::Matrix3d::Identity();  
    
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0., 0., 1));
    
    cout << rotation_vector.matrix()<<endl;
    std::cout << "Hello, world!" << std::endl;
    
    
    rotation_matrix = rotation_vector.toRotationMatrix();
    Eigen::Vector3d  v(1, 0 , 0);
    
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << v_rotated<<endl;
    
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    
    cout <<" 欧拉角："<< euler_angles.transpose()<<endl;
    
    Eigen::Isometry3d T =  Eigen::Isometry3d::Identity();
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    
    cout << T.matrix()<<endl;
    
    
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout <<"四元数:" << q.coeffs()<<endl;
    
    v_rotated = q * v; /*数学上为q*V * v(-1)*/
    
    cout<<v_rotated<<endl;
    
    
    //cout << T.rotate(rotation_vector)<<endl;
    return 0;
}