Eigen学习日志4

#include "iostream"
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <cmath>
using namespace Eigen;
//初始化各种坐标变换矩阵
void eigen_geometry();
//坐标变换矩阵的转换
void eigen_transform_geometry();
//坐标变换例子
void eigen_example_geometry();

void eigen_geometry(){
    //常用的坐标变换矩阵

    //1.旋转矩阵R(3X3): Eigen::Matrix3d
    //初始化旋转矩阵R
    Matrix3d R = Matrix<double,3,3>::Identity();
    std::cout<<"R:"<<std::endl<<R<<std::endl;



    //2.旋转向量w(1x1,3x1): Eigen::AngleAxisd
    //初始化旋转向量w,使用旋转角度和旋转轴向量（必须为单位向量）来初始化角轴!!!!!
    /*
     * 务必记住旋转轴向量必须为单位向量，即该向量v(x,y,z)的模为1.即||v||=1!!!!!，sqrt(x^2+y^2+z^2)=1
     * 只有旋转轴向量为单位向量时才代表旋转
     */
    //以（0，0，1）为旋转轴，旋转45度
    AngleAxisd w(M_PI/4,Vector3d(0,0,1));
    //旋转向量使用AngleAxisd，它的底层不直接是Matrix,但运算可以当作矩阵（因为重载了运算符）
    //AngleAxisd.matrix()将旋转向量转换为Matrix类型
    std::cout<<"w:"<<std::endl<<w.matrix()<<std::endl;
    //AngleAxisd.toRotationMatrix()将旋转向量转换为Matrix类型
    std::cout<<"w:"<<std::endl<<w.toRotationMatrix()<<std::endl;



    //3.四元数（4X1）Q(q0,q1*i,q2*j,q3*k),q0为实部，q1,q2,q3为虚部

    //注：只有单位四元数才代表旋转！！！！

    //Eigen::Quaterniond
    //Eigen::Quaternion< _Scalar, _Options >::Quaternion( const Scalar & w,const Scalar & x,const Scalar &y,const Scalar & z )
    //要注意Eigen中四元数赋值的顺序，实数w在首；但是实际上它的内部存储顺序是[x y z w]。实际上后面输出系数的时候也是按内部存储顺序输出

    //初始化四元数，使用旋转的角度和旋转轴向量（必须为单位向量）来初始化四元数
    //四元数与旋转轴向量 n(nx,ny,nz) 和旋转角度 theta 的对应关系如下
    //Q = [cos(theta/2),sin(theta/2)*nx,sin(theta/2)*ny,sin(theta/2)*nz]^T
    //theta = 2*arccos(q0)， [nx,ny,nz]^T = [q1,q2,q3]^T/sin(theta/2)

    //以（0，0，1）为旋转轴，旋转45度
    Quaterniond Q(cos((M_PI/4)/2),0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));
    //输出系数[x y z w]:Quaterniond.coeffs(),输出虚部[x y z]:Quaterniond.vec()
    std::cout<<"Q:"<<std::endl<<Q.coeffs()<<std::endl;
    std::cout<<"Q的虚部:"<<std::endl<<Q.vec()<<std::endl;



    //4.平移向量t(3X1) Eigen::Vector3d
    Vector3d t(0,1,0);



    //5.变换矩阵T(R,t)(4X4) Eigen::Isometry3d
    //初始化变换矩阵,使用Isometry3d ::Identity()初始化
    Isometry3d T = Isometry3d ::Identity();  //虽然称为3d，实质上是4＊4的矩阵　　齐次坐标
    //Isometry3d.matrix()将旋转向量转换为Matrix类型
    std::cout<<"T:"<<std::endl<<T.matrix()<<std::endl;

    //6.欧拉角eulerAngle(3X1)(Z,Y,Z)--(偏航角yaw，俯仰角pitch，滚转角roll): Eigen::Vector3d
    Vector3d eulerAngle;


}

void eigen_transform_geometry(){

    //创建旋转矩阵
    Matrix3d R;
    R<<1,3,1,
       2,3,4,
       3,1,2;
    std::cout<<"R:"<<std::endl<<R<<std::endl;

    //把旋转向量w转换为旋转矩阵R
    //以（0，0，1）为旋转轴，旋转45度
    AngleAxisd w_to_R(M_PI/4,Vector3d(0,0,1));
    std::cout<<"旋转向量w_to_R:"<<std::endl<<w_to_R.matrix()<<std::endl;
    //方法一，直接赋值
    Matrix3d w_to_r1;
    w_to_r1 = w_to_R;
    std::cout<<"把旋转向量w_to_R转换为旋转矩阵w_to_r1:"<<std::endl<<w_to_r1<<std::endl;
    //方法二、通过AngleAxisd.matrix()
    Matrix3d w_to_r2;
    w_to_r2 = w_to_R.matrix();
    std::cout<<"把旋转向量w_to_R转换为旋转矩阵w_to_r2:"<<std::endl<<w_to_r2<<std::endl;

    //把欧拉角eulerAngle转换为旋转矩阵eulerA_to_R
    //欧拉角eulerAngle(3X1)(Z,Y,Z)--(偏航角yaw，俯仰角pitch，滚转角roll): Eigen::Vector3d
    Vector3d eulerAngle(2,1,0);
    std::cout<<"欧拉角eulerAngle(偏航角yaw-2,俯仰角pitch-1,滚转角roll-0):"<<std::endl<<eulerAngle<<std::endl;
    Matrix3d eulerA_to_R;
    //先把欧拉角转换为各个轴（Z,Y,Z）的旋转向量，再把它相乘得总的旋转向量
    AngleAxisd yawAngle( AngleAxisd(eulerAngle(0),Vector3d::UnitZ()) );
    AngleAxisd pitchAngle( AngleAxisd(eulerAngle(1),Vector3d::UnitY()) );
    AngleAxisd rollAngle( AngleAxisd(eulerAngle(2),Vector3d::UnitX()) );
    eulerA_to_R = yawAngle*pitchAngle*rollAngle;
    std::cout<<"把欧拉角eulerAngle转换为旋转矩阵eulerA_to_R:"<<std::endl<<eulerA_to_R<<std::endl;

    //把四元数Q转换为旋转矩阵Q_to_R
    //以（0，0，1）为旋转轴，旋转45度
    Quaterniond Q(cos((M_PI/4)/2),0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));
    std::cout<<"Q:"<<std::endl<<Q.coeffs()<<std::endl;
    Matrix3d Q_to_R;
    //通过Quaterniond.matrix()方法把四元数转换为旋转矩阵
    Q_to_R = Q.matrix();
    std::cout<<"把四元数Q转换为旋转矩阵Q_to_R:"<<std::endl<<Q_to_R<<std::endl;



    //创建一个旋转向量
    AngleAxisd w(M_PI,Vector3d(0,0,1));

    //把旋转矩阵R1转换为旋转向量w1
    Matrix3d R1;
    R1<<1,3,1,
            2,3,4,
            3,1,2;
    std::cout<<"R1:"<<std::endl<<R1<<std::endl;
    //方法一，直接赋值
    AngleAxisd w1;
    w1 = R1;
    std::cout<<"旋转矩阵R1转换为旋转向量w1:"<<std::endl<<w1.toRotationMatrix()<<std::endl;
    //方法二、通过AngleAxisd.fromRotationMatrix(Matrix ..)方法把旋转矩阵转换为旋转向量
    AngleAxisd w2;
    w2.fromRotationMatrix(R1);
    std::cout<<"旋转矩阵R1转换为旋转向量w2:"<<std::endl<<w2.toRotationMatrix()<<std::endl;

    //把欧拉角eulerAngle2转换成旋转向量euler_to_w
    //先把欧拉角转换为各个轴（Z,Y,Z）的旋转向量，再把它相乘得总的旋转向量
    Vector3d eulerAngle2(2,1,0);
    std::cout<<"欧拉角eulerAngle2(偏航角yaw-2,俯仰角pitch-1,滚转角roll-0):"<<std::endl<<eulerAngle2<<std::endl;
    AngleAxisd euler_to_w;
    AngleAxisd yawAngle1( AngleAxisd(eulerAngle2(0),Vector3d::UnitZ()) );
    AngleAxisd pitchAngle1( AngleAxisd(eulerAngle2(1),Vector3d::UnitY()) );
    AngleAxisd rollAngle1( AngleAxisd(eulerAngle2(2),Vector3d::UnitX()) );
    euler_to_w = yawAngle1*pitchAngle1*rollAngle1;
    std::cout<<"把欧拉角eulerAngle2转换成旋转向量euler_to_w:"<<std::endl<<euler_to_w.toRotationMatrix()<<std::endl;

    //把四元数Q1转换为旋转向量Q1_to_w
    //以（0，0，1）为旋转轴，旋转45度
    Quaterniond Q1(cos((M_PI/4)/2),0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));
    std::cout<<"Q1:"<<std::endl<<Q1.coeffs()<<std::endl;
    AngleAxisd Q1_to_w;
    //直接赋值
    Q1_to_w = Q1;
    std::cout<<"把四元数Q1转换为旋转向量Q1_to_w:"<<std::endl<<Q1_to_w.toRotationMatrix()<<std::endl;



    //创建一个欧拉角
    Vector3d EulerAngle(2,1,0);

    //把旋转矩阵R2转换为欧拉角R2_to_euler;
    Matrix3d R2;
    R2<<1,3,1,
        2,3,4,
        3,1,2;
    std::cout<<"R2:"<<std::endl<<R2<<std::endl;
    Vector3d R2_to_euler;
    //通过Matrix.eulerAngles(2,1,0)把旋转矩阵转换为欧拉角
    R2_to_euler = R2.eulerAngles(2,1,0);
    std::cout<<"把旋转矩阵R2转换为欧拉角R2_to_euler:"<<std::endl<<R2_to_euler<<std::endl;

    //把旋转向量W2转换为欧拉角w2_to_euler;
    //以（0，0，1）为旋转轴，旋转45度
    AngleAxisd W2(M_PI/4,Vector3d(0,0,1));
    std::cout<<"旋转向量W2:"<<std::endl<<W2.matrix()<<std::endl;
    Vector3d w2_to_euler;
    //通过AngleAxisd.matrix().eulerAngles(2,1,0)把旋转向量转换为旋转矩阵再转换为欧拉角
    w2_to_euler = W2.matrix().eulerAngles(2,1,0);
    std::cout<<"把旋转向量W2转换为欧拉角w2_to_euler:"<<std::endl<<w2_to_euler<<std::endl;

    //把四元数Q3转换为欧拉角Q3_to_euler;
    //以（0，0，1）为旋转轴，旋转45度
    Quaterniond Q3(cos((M_PI/4)/2),0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));
    std::cout<<"Q3:"<<std::endl<<Q3.coeffs()<<std::endl;
    Vector3d Q3_to_euler;
    //通过Quaterniond.matrix().eulerAngles(2,1,0)把旋转向量转换为旋转矩阵再转换为欧拉角
    Q3_to_euler = Q3.matrix().eulerAngles(2,1,0);
    std::cout<<"把四元数Q3转换为欧拉角Q3_to_euler:"<<std::endl<<Q3_to_euler<<std::endl;



    //创建一个四元数
    Quaterniond q(cos((M_PI/4)/2),0*sin((M_PI/4)/2),0*sin((M_PI/4)/2),1*sin((M_PI/4)/2));

    //把旋转矩阵R3转换为四元数R3_to_Q
    Matrix3d R3;
    R3<<1,3,1,
        2,3,4,
        3,1,2;
    std::cout<<"R3:"<<std::endl<<R3<<std::endl;
    Quaterniond R3_to_Q;
    //直接赋值
    R3_to_Q = R3;
    std::cout<<"把旋转矩阵R3转换为四元数R3_to_Q:"<<std::endl<<R3_to_Q.coeffs()<<std::endl;

    //把旋转向量w3转换为四元数w3_to_q
    AngleAxisd w3(M_PI/4,Vector3d(0,0,1));
    std::cout<<"旋转向量w3:"<<std::endl<<w3.matrix()<<std::endl;
    Quaterniond w3_to_q;
    //直接赋值
    w3_to_q = w3;
    std::cout<<"把旋转向量w3转换为四元数R3_to_Q:"<<std::endl<<w3_to_q.coeffs()<<std::endl;

    //把欧拉角eulerAngle3转换为四元数euler3_to_q
    Vector3d eulerAngle3(2,1,0);
    std::cout<<"欧拉角eulerAngle3(偏航角yaw-2,俯仰角pitch-1,滚转角roll-0):"<<std::endl<<eulerAngle3<<std::endl;
    Quaterniond euler3_to_q;
    //先把欧拉角转换为各个轴（Z,Y,Z）的旋转向量，再把它相乘得总的旋转向量
    AngleAxisd euler_to_q;
    AngleAxisd yawAngle2( AngleAxisd(eulerAngle3(0),Vector3d::UnitZ()) );
    AngleAxisd pitchAngle2( AngleAxisd(eulerAngle3(1),Vector3d::UnitY()) );
    AngleAxisd rollAngle2( AngleAxisd(eulerAngle3(2),Vector3d::UnitX()) );
    euler3_to_q = yawAngle2*pitchAngle2*rollAngle2;
    std::cout<<"把欧拉角eulerAngle3转换为四元数euler3_to_q:"<<std::endl<<euler3_to_q.coeffs()<<std::endl;
}

void eigen_example_geometry(){
    //注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    /****旋转向量****/

    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    // 乘以该向量，表示进行一个坐标变换
    //任意旋转可用一个旋转轴和一个旋转角度来表示。
    //旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。

    AngleAxisd w(M_PI/4,Vector3d(1/sqrt(3),1/sqrt(3),1/sqrt(3)));
    std::cout<<"旋转向量w:\n"<<w.matrix()<<std::endl;
    std::cout<<"Vector3d(1,1,1):"<<Vector3d(1/ sqrt(3),1/sqrt(3),1/sqrt(3)).norm()<<std::endl;
    //对坐标v（1，0，0）使用旋转向量w进行坐标变换
    Vector3d v(1,0,0);
    std::cout<<"坐标v:\n"<<v.matrix()<<std::endl;
    Vector3d Angle_transf_v = w*v;
    std::cout<<"Angle_transf_v:\n"<<Angle_transf_v<<std::endl;

    //对坐标v(1,0,0)使用旋转矩阵R进行坐标变换
    Matrix3d R = w.matrix();
    std::cout<<"旋转矩阵R:\n"<<R<<std::endl;
    Vector3d R_transf_v = R*v;
    std::cout<<"R_transf_v:\n"<<R_transf_v<<std::endl;

    //对坐标v(1,0,0)使用四元数Q进行坐标变换
    Quaterniond Q;
    Q = Quaterniond(w);
    std::cout<<"四元数Q:\n"<<Q.coeffs()<<std::endl;
    Vector3d q_transf_v = Q*v;
    std::cout<<"q_transf_v:\n"<<q_transf_v<<std::endl;

    //对坐标v（1，0，0）使用欧式变换矩阵T（R，t）进行坐标变换
    Isometry3d T = Isometry3d::Identity();//进行变换矩阵初始化
    T.rotate(R);//令T(R,t)中的R为旋转矩阵R
    T.pretranslate(Vector3d(2,0,1));//令T(R,t)中的t为平移向量t
    Vector3d T_transf_v =  T*v;
    std::cout<<"T_transf_v:\n"<<T_transf_v<<std::endl;

    /*
     * 编程题目
	小萝卜１号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]'　　　世界坐标系到相机变换
	小萝卜２号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
	小萝卜１号看到某点P位于自身坐标系下p=[0.5,0,0.2]'
	求该向量在小萝卜２号下的坐标
	*/

//    Quaterniond q1(0.35,0.2,0.3,0.1);
//    Quaterniond q2(-0.5,0.4,-0.1,0.2);
//    //只有单位四元数表示旋转，因此必须进行归一化
//    q1 = q1.normalized();
//    q2 = q2.normalized();
//    Vector3d t1(0.3,0.1,0.1);
//    Vector3d t2(-0.1,0.5,0.3);
//    Isometry3d T1 = Isometry3d::Identity();
//    Isometry3d T2 = Isometry3d::Identity();
//    T1.rotate(q1.matrix());
//    T1.pretranslate(t1);
//    T2.rotate(q2.matrix());
//    T2.pretranslate(t2);
//    Isometry3d T2_T1 = T2*T1.inverse();
//    std::cout<<"T2_T1:\n"<<T2_T1.matrix()<<std::endl;
//    Vector3d P_T1(0.5,0,0.2);
//    Vector3d P_T2 = T2_T1*P_T1;
//    std::cout<<"P_T2:\n"<<P_T2<<std::endl;

    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);//wxyz q1.coeffs()  xyzw  q1.vec()  xyz
    //q1 << 0.35,0.2,0.3,0.1;
    Eigen::Matrix<double, 3, 1> t1;//float类型
    t1 << 0.3, 0.1, 0.1;
    Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
    //q2 << -0.5,0.4,-0.1,0.2;
    Eigen::Matrix<double, 3, 1> t2;//float类型
    t2 << -0.1, 0.5, 0.3;
    Eigen::Matrix<double, 3, 1> p1;//float类型
    p1 << 0.5, 0, 0.2;

    std::cout << "q1= \n" << q1.coeffs() << std::endl;
    std::cout << "t1= \n" << t1 << std::endl;
    std::cout << "q2= \n" << q2.coeffs() << std::endl;
    std::cout << "t2= \n" << t2 << std::endl;

    /*
    q1.setIdentity();
    cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl;
    q2.setIdentity();
    cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl;
    */

    q1 = q1.normalized();//规范化　　归一化   除以模长
    std::cout << "q1 after normalized\n" << q1.coeffs() << std::endl;
    q2 = q2.normalized();
    std::cout << "q2 after normalized \n" << q2.coeffs() << std::endl;

    Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
    q1rotation_matrix = q1.toRotationMatrix();
    Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵　　齐次坐标

    Tc1w.rotate(q1rotation_matrix);                                    // 按照q1rotation_matrix进行旋转
    Tc1w.pretranslate(t1);                                                     // 把平移向量设成t1

    //Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆变换　　成　c1到world
    Eigen::Matrix<double, 3, 1> pw = Tc1w.inverse()*p1;    //将c1坐标系下的点p1变换到world坐标系下

    Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
    q2rotation_matrix = q2.toRotationMatrix();
    Eigen::Isometry3d Tc2w = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵　　齐次坐标

    Tc2w.rotate(q2rotation_matrix);                                    // 按照q2rotation_matrix进行旋转
    Tc2w.pretranslate(t2);                                                     // 把平移向量设成t2

    Eigen::Matrix<double, 3, 1> p2 = Tc2w*pw;    //将world坐标系下的点pw变换到c2坐标系下
    std::cout << "the loc of p1 in c1  = \n" << p1 << std::endl;
    std::cout << "the loc of p1 in world  = \n" << pw << std::endl;
    std::cout << "the loc of p1 in c2 = \n" << p2 << std::endl;

/*
 * the loc of p1 in c2 =
-0.0309731
   0.73499
  0.296108
 */
}

int main(){
    eigen_geometry();
    eigen_transform_geometry();
    eigen_example_geometry();
    return 0;
}