C++线性代数运算库eigen3的使用，空间欧式变换的表示和运算，旋转四元数的球面插值

1.初始化Matrix和Vector
2.eigen基础线性代数运算，详细参考官网教程
3.Eigen用作空间变换运算，各种旋转表示之间的便变换
4.用旋转角(角轴AngleAxis)初始化旋转矩阵，动轴旋转和定轴旋转
5.使用Eigen求不同坐标系下坐标转换
6.旋转四元数的球面插值R1.slerp(t,R2)
7.CMakeLists.txt文件

Eigen中所有的向量和矩阵都是模板类 Eigen::Matrix，就像matlab一样，都是矩阵，能够进行各种矩阵的运算，都要显式地声明矩阵地大小尺寸，并进行初始化，不初始化可能导致被随机值填充。
！！！所以每个矩阵都是个对象，有各种成员函数，通过各种成员函数求矩阵的各种值，行列式、元素和、迹、转置、逆等等main.cpp
各种旋转平移运算main1.cpp，旋转矩阵R，旋转向量theta_n，旋转角eulerAngles，四元数Quaternion，变换矩阵T

1. 初始化Matrix和Vector

   Eigen::Matrix3d 相当于Eigen::Matrix<double,3,3>
   Eigen::Vector3i 相当与Eigen::Matrix<int,3,1>
   Eigen::Matrix<double, 2, 2> m22d;    //相当于Eigen::Matrix2d
   m22d << 1, 2, 3, 4;                  //逗号初始化
   m22d = Eigen::Matrix2d::Random();    //浮点数，则0-1随机数，否则是0-最大整数
   m22d = Eigen::Matrix2d::Ones();      //全是1
   m22d = Eigen::Matrix2d::Zero();      //全是0
   m22d = Eigen::Matrix2d::Identity();  //单位矩阵
   m22d = Eigen::Matrix2d::Constant(4); //所有元素一样
   //Vector其实还是Matrix
   Eigen::Vector3i v3i(1, 2, 3);  //相当于Eigen::Matrix
   v3i << 4, 5, 6;
   v3i.setZero();
   v3i.setOnes();
   v3i.setConstant(7);
   v3i.setLinSpaced(3, 5);
   
   //块操作
   Eigen::Matrix<double, 5, 6> m56;
   m56.setConstant(2);  //元素全部设置成2
   cout << "m56.setConstant(2)--------\n" << m56 << endl;
   //块操作method1  block(i,j,rows,cols)  起始位置(i,j),行列rows,cols
   m56.block(1, 2, 2, 3).setConstant(1);  
   cout << "m56.block(1, 2, 2, 3).setConstant(1)--------\n" << m56 << endl;
   //块操作method2  block(i,j)  起始位置(i,j),行列rows,cols
   m56.block<2, 3>(2, 2).setConstant(5);
   cout << "m56.block<2,3>(2,2).setConstant(5)---------\n" << m56 << endl;
   

2. eigen基础线性代数运算，详细参考官网教程

   #include 
   #include 
   #include 
   #include   //稠密矩阵代数运算
   
   using namespace std;
   const int N = 50;
   
   int main(int argc, char **argv)
   {
       // Eigen中所有的向量和矩阵都是模板类 Eigen::Matrix ，就像matlab一样，都是矩阵
       Eigen::Matrix<float, 2, 3> matrix23;
       Eigen::Matrix<double, 3, 1> v3d; //等同于Eigen::Vector3d v3d;
       //动态大小矩阵
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
       Eigen::MatrixXd matrix_dynamic1;
   
       //初始化为0矩阵  Zero()静态成员函数
       Eigen::Matrix<double, 3, 3> matrix33 = Eigen::Matrix<double, 3, 3>::Zero();
       //输入数据初始化
       matrix23 << 1, 2, 3, 4, 5, 6;
       v3d << 3, 2, 1;
       cout << "matrix23 = " << endl
            << matrix23 << endl;
       //随机数初始化  Random()静态成员函数
       matrix33 = Eigen::Matrix<double, 3, 3>::Random();
       cout << "matrix33 = \n"
            << matrix33 << endl;
   
       //转置 成员函数 .transpose()
       Eigen::Matrix<float, 3, 2> matrix32 = matrix23.transpose();
       cout << "matrix32 = " << endl
            << matrix32 << endl;
   
       //元素的和 .sum()
       double sum = matrix23.sum();
   
       //迹 对角线元素的和  .trace()
       double trace = matrix33.trace();
       double mincoe = matrix33.minCoeff();  //最小元素
       double maxcoe = matrix33.maxCoeff();  //最大元素
       double product = matrix33.prod();  //元素的积
       double mean = matrix33.mean();  //均值
       //数乘   *
       Eigen::Matrix<double, 2, 3> matrix23_10 = 10 * matrix23.cast<double>();
   
       //逆   .inverse()
       Eigen::Matrix<double, 3, 3> matrix1;
       matrix1 << 3, 0, 0, 0, 4, 0, 0, 0, 5;
       Eigen::Matrix<double, 3, 3> matrix33inv = matrix1.inverse();
       cout << "matrix33inv = \n"
            << matrix33inv << endl;
   
       //行列式 .determinant()
       double det = matrix33.determinant();
       cout << "det = " << det << endl;
   
       //访问数据,用小括号(i,j)访问，不同于数组的中括号[]
       cout << "matrix23 = \n";
       for (int i = 0; i < 2; i++)
       {
           for (int j = 0; j < 3; j++)
           {
               cout << matrix23(i, j) << "\t";
           }
           cout << endl;
       }
   
       //矩阵乘法
       //不同数据类型的元素不能相乘，要显式的类型转换成员函数 .cast<>()  和c++的不同是个函数 static_cast()
       Eigen::Matrix<double, 2, 1> result21 = matrix23.cast<double>() * v3d;
       cout << "[1,2,3;4,5,6]*[3,2,1] = \n"
            << result21 << endl;
   
       //求特征值， 特征向量
       Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> eigen_solver(v3d * v3d.transpose());
       cout << "特征值 = \n"
            << eigen_solver.eigenvalues() << endl;
       cout << "特征向量 = \n"
            << eigen_solver.eigenvectors() << endl;
   
       //解方程
       // matrixNN * x =vNd, 求x
       Eigen::Matrix<double, N, N> matrixNN = Eigen::Matrix<double, N, N>::Random();
       matrixNN = matrixNN * matrixNN.transpose(); //保证半正定
       Eigen::Matrix<double, N, 1> vNd = Eigen::Matrix<double, N, 1>::Random();
       Eigen::Matrix<double, N, 1> x;
       //方法1,直接求逆，简单直接
       clock_t time1 = clock();
       x = matrixNN.inverse() * vNd;
       cout << "x = \n"
            << x.transpose() << endl;
       cout << "time cost " << 1000 * (clock() - time1) / double(CLOCKS_PER_SEC) << "ms" << endl;
       //方法2,矩阵分解  实验证明更快
       time1 = clock();
       x = matrixNN.colPivHouseholderQr().solve(vNd);
       cout << "x = \n"
            << x.transpose() << endl;
       cout << "time cost " << 1000 * (clock() - time1) / double(CLOCKS_PER_SEC) << "ms" << endl;
       //对于正定矩阵，方法3cholesky分解  实验证明更更快
       time1 = clock();
       x = matrixNN.ldlt().solve(vNd);
       cout << "x = \n"
            << x.transpose() << endl;
       cout << "time cost " << 1000 * (clock() - time1) / double(CLOCKS_PER_SEC) << "ms" << endl;
   
       return 0;
   }
   

3. Eigen用作空间变换运算
   注意1：坐标系其实就是位姿，因此统一用两个下标，表示是从哪到哪的位姿变换，例如Tw1就表示在w坐标系下，坐标系1的位姿，因此坐标系1就表示为Tw1。同理，从坐标系1到坐标系2的位姿变换表示为T12。空间点就只需要一个下标，表示在哪个坐标系下的坐标，如Pw表示该点在w坐标系下坐标，P1表示在坐标系1下的坐标。
   注意2：对坐标系进行变换的时候是右乘，如Tw2 = Tw1T12。而对点进行变换的时候是左乘，P2 = T21P1。并且可以发现，点的坐标从坐标系1变换到坐标系2的变换T21是坐标系1变换到坐标系2的变换位姿T12的逆。
   其实很好理解，因为不管在哪个坐标系下的坐标，是同一个三维点，统一转换到w坐标系是一样的，也就是Tw1P1 = Tw1T12P2，所以P2 = T12.inverse() * P1 = T21 * P1;
   还有一种情况，只是在同一个坐标系下，点P1经过旋转（绕X轴Y轴Z轴）R之后再平移变成P2，这种情况是两个不同的点。此时P2 = R * P1 + t，此时也是左乘

   #include 
   #include 
   #include 
   #include  //各种平移和旋转运算
   using namespace std;
   
   int main(int argc, char **argv)
   {
   	//Pw = Tw1*P1
   	Eigen::Matrix<double, 3, 3> Rw1_matrix = Eigen::Matrix<double, 3, 3>::Identity(); // 单位矩阵
   	Eigen::Matrix<double, 3, 1> P1(1, 0, 0);
   
   	// 旋转向量,绕z轴旋转PI/4，Z轴也可以表示为 Eigen::Vector3d::UnitZ()
   	Eigen::AngleAxisd rotation_vector_w1(M_PI / 4, Eigen::Matrix<double, 3, 1>(0, 0, 1));
   
    	// 旋转向量 -- 旋转矩阵R
   	Eigen::Matrix<double, 3, 3> Rw1 = rotation_vector_w1.matrix();
   	Rw1_matrix = rotation_vector_w1.toRotationMatrix(); // 和上面一样的
   
   	// Rw1 -- 欧拉角
   	Eigen::Vector3d euler_angles = Rw1.eulerAngles(2, 1, 0); // ZYX，roll pitch yow
   
   	// 变换矩阵T   Isometry3d类型, 一定要初始化为Identity()，否则为随机数
   	Eigen::Isometry3d Tw1 = Eigen::Isometry3d::Identity();
   	Tw1.rotate(Rw1);
   	Eigen::Matrix<double, 3, 1> tw1(1, 3, 4);
   	Tw1.pretranslate(tw1);
   	cout << "Tw1 = \n" << Tw1.matrix() << endl;
    	Eigen::Matrix<double, 3, 1> Pw = Tw1 * P1; // 用T坐标变换，相当于 Rw1 * P1 + tw1
   
   	// 四元数
   	Eigen::Quaterniond Qw1 = Eigen::Quaterniond(Rw1); // Rw1 -- Qw1
   	Qw1 = Eigen::Quaterniond(rotation_vector_w1);      // 和上面一样  旋转向量--Qw1
   	cout << "Qw1 = " << endl << Qw1.coeffs() << endl; //(qx,qy,qz,qw) 实部在后面
   
   	Eigen::Matrix<double, 3, 1> P = rotation_vector_w1 * P1; // 用旋转向量变换坐标
   	P = Rw1 * P1;                                           // 旋转矩阵坐标变换，结果是一样的
   	P = Qw1 * P1;                                           // 用四元数坐标变换
   
   	return 0;
   }
   

4. 用旋转角(角轴AngleAxis)初始化旋转矩阵
   关于定轴旋转和动轴旋转，eigen的AngleAxisd(angle, axis)AngleAxisd(angle2, axis2)…从左往右按顺序看，是动轴旋转。
   而定轴旋转就等于把动轴旋转的第一三次旋转交换，也就相当于从右向左反着看。例如从左向右正这看，先绕X轴30度，再绕旋转后的Y轴旋转45度，再绕旋转后的Z轴旋转60度。也就相当于从右向左反着看，先绕Z轴旋转60度，再绕原来的Y轴旋转45度，再绕原来的X轴旋转30度。

   #include 
   #include 
   #include 
   #include  //各种平移和旋转运算
   using namespace std;
   
   int main(int argc, char **argv)
   {
   	Eigen::Matrix3d R12;
   	// 注意⚠️是动轴旋转，可以理解为一次次分开旋转，后一次旋转发生在前一次的基础上
   	// ⚠️从左往右动轴旋转，其实就是从右往左定轴旋转，所以加入知道的是定轴旋转先x再y再z多少度，就等于动轴先Z再Y再X,AngleAxisd( , Z) * AngleAxisd( , Y) * AngleAxisd( , X)
   	// 因为是右乘，所以顺序是对的，从前往后依次旋转，先绕着X轴逆时针M_PI/3, 再绕着Y逆时针M_PI/4，再绕着Z逆时针M_PI/6
   	// Eigen::Vector3d(1,0,0)和Eigen::Vector3d::UnitX()一样
   	R12 = Eigen::AngleAxisd(M_PI / 3, Eigen::Vector3d(1, 0, 0)) * Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 6, Eigen::Vector3d::UnitZ());
   	Eigen::Quaterniond Q12(R12);
   	//平移是在原始坐标系基准，不是旋转后的基准
   	Eigen::Vector3d tw1(1, 2, 3);
   	Eigen::Isometry3d T12 = Eigen::Isometry3d::Identity();
   	T12.rotate(R12);
   	T12.pretranslate(tw1);
   	cout << "T12 = \n" << T12.matrix() << endl;
   	cout << "旋转\n" << T12.rotation() << endl;
   	cout << "平移\n" << T12.translation() << endl;
   
   	Eigen::Vector3d eulerAngle12 = R12.eulerAngles(0, 1, 2); // xyz,  roll, pitch, yaw
   	// 转换成角度制，结果确实是60，45，30
   	cout << "eulerAngle12 = \n" << eulerAngle12 * 180 / M_PI << endl; 
   	// P1 = T12*P2，因为同一个点在1坐标系下坐标肯定是一样的
   	// 所以 P2 = T12.inverse() * P1 = T21 * P1
   	Eigen::Vector3d P1(6, 3, 9);
   	Eigen::Vector3d P2 = T12.inverse() * P1;
   	cout<< "P2 = \n" << P2 << endl;
   	// 旋转向量Eigen::AngleAxisd          到R的成员函数是.toRotationMatrix()
   	// 旋转矩阵Eigen::Matrix3d
   	// 旋转四元数Eigen::Quaterniond          也有成员函数.toRotationMatrix()
   	// 都可以直接乘以三维点Eigen::Vector3d,进行旋转
   	// 变换矩阵Eigen::Isometry3d, 乘以Eigen::Vector3d, 表示旋转之后再平移
   	Eigen::Vector3d ang1(M_PI/3, M_PI/4, M_PI/6);
   	// 不能这样转换，欧拉角具有旋转锁，必须将欧拉角的三个元素用上面R12定义的形式
   	// Eigen::Matrix3d R1 = ang1.eulerAngles(0,1,2);
   	// cout << "R1 = " << endl << R1 << endl;
   	return 0;
   }
   

5. 使用Eigen求不同坐标系下坐标转换

   #include 
   #include 
   #include 
   using namespace std;
   int main(int argc, char **argv)
   {
       //坐标系 Tw1，仅平移（1，0，0）
       Eigen::Matrix3d Rw1 = Eigen::Matrix3d::Identity();
       Eigen::Matrix<double, 3, 1> tw1(1, 0, 0);
   
       //坐标系 Tw2
       Eigen::Matrix3d Rw2;
       //动轴旋转，相当于分开乘，在旋转后的基础上再旋转，从前往后，先Y轴转PI，再X轴转PI/2，再Z轴PI/2，然后（在原坐标系下）平移（3，4，6）
       Rw2 = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
       Eigen::Matrix<double, 3, 1> tw2(3, 4, 6);
   
       //坐标系1下坐标
       Eigen::Matrix<double, 3, 1> P1(10, 10, 10);
       //所以理论上 Pw = (11,10,10)
       //P2 = ()
   
       Eigen::Isometry3d Tw1 = Eigen::Isometry3d::Identity();
       Tw1.rotate(Rw1);
       Tw1.pretranslate(tw1);
       cout << "Tw1 = \n" << Tw1.matrix() << endl;
   
       Eigen::Isometry3d Tw2 = Eigen::Isometry3d::Identity();
       Tw2.rotate(Rw2);
       Tw2.pretranslate(tw2);
       cout << "Tw2 = \n" << Tw2.matrix() << endl;
   
       //则可以算出坐标系1，2之间的变换 T12 = T1w * Tw2 = Tw1.inverse() * Tw2
       Eigen::Isometry3d T12 = Tw1.inverse() * Tw2;
   
       //求坐标系2下坐标 P2, P1 = T12*P2, P2 = T12.inverse() * P1
       Eigen::Matrix<double, 3, 1> P2 = Tw2.inverse() * Tw1 * P1;
       // P2 = T12.inverse() * P1;
       cout << "P2 = \n" << P2 << endl;
   
       //求在w坐标系下的坐标Pw, Pw = Tw1 * P1
       Eigen::Vector3d Pw = Tw1 * P1;
       cout << "Pw = \n" << Pw << endl;
   
       return 0;
   }
   

   运行结果

   Tw1 = 
   1 0 0 1
   0 1 0 0
   0 0 1 0
   0 0 0 1
   Tw2 = 
              0            1  5.55112e-17            3
    2.22045e-16  1.11022e-16           -1            4
             -1 -5.55112e-17 -2.22045e-16            6
              0            0            0            1
   P2 = 
   -4
    8
   -6
   Pw = 
   11
   10
   10
   

6. 旋转四元数的球面插值Rw1.slerp(t,Rw2),t在(0.0 - 1.0)范围当成标量的话相当于Rw1 + t*(Rw2-Rw1)
   四元数的插值主要有
   1、线性插值，再进行归一化.normalization()
   2、球面插值，更合理.slerp()只支持Quaternion和Rotation2D
   eigen四元数球面插值代码实例

   #include 
   #include 
   #include 
   #include  //各种平移和旋转运算
   using namespace std;
   int main(int argc, char **argv)
   {
       Eigen::Vector3d Pw(1, 2, 3);
       //用四元数表示旋转
       Eigen::Quaterniond Rw1, Rw2, R12;
       Rw1 = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX());
       R12 = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY());
       Rw2 = Rw1 * R12;
       //eigen球面插值 Q1.slerp(t, Q2)
       //Rw1和Rw2的1/2球面插值，相当于旋转R12的一半，45度
       Eigen::Quaterniond insert_Q1 = Eigen::Quaterniond::Identity().slerp(0.5, R12);
       Eigen::Quaterniond Rw2_1 = Rw1 * insert_Q1;
       //可以直接插值Rw1和Rw2之间插值
       Rw2_1 = Rw1.slerp(0.5, Rw2);
       //Rw1和Rw2的1/3球面插值，相当于旋转R12的1/3，30度
       Eigen::Quaterniond insert_Q2 = Eigen::Quaterniond::Identity().slerp(1.0/3, R12);
       Eigen::Quaterniond Rw2_2 = Rw1 * insert_Q2;
       //直接插值
       Rw2_2 = Rw1.slerp(1.0/3, Rw2);
       //计算P2, P2_1, P2_2坐标
       Eigen::Vector3d P2 = Rw2.inverse() * Pw;
       Eigen::Vector3d P2_1 = Rw2_1.inverse() * Pw;
       Eigen::Vector3d P2_2 = Rw2_2.inverse() * Pw;
       //理论上P2(2,3,1)
       cout << "P2 = " << endl << P2.transpose() << endl; 
       //理论上P2_1(sqrt(2)+1.0/sqrt(2), 3, -1.0/sqrt(2))
       cout << "P2_1 = " << endl << P2_1.transpose() << endl;
       //验证结果是对的
       cout << "P2_1 理论上 = \n" << sqrt(2.0) + 1.0 / sqrt(2.0) << "\t" << 3 << "\t" << -1.0 / sqrt(2.0) << endl;
       //理论上P2_2(1+sqrt(3)/2, 3, -(sqrt(3)-1.0/2))
       cout << "P2_2 = " << endl << P2_2.transpose() << endl;
       cout << "P2_2 理论上 = \n" << 1.0 + sqrt(3.0)/ 2.0 << "\t" << 3 << "\t" << 0.5 - sqrt(3.0) << endl;
       return 0;
   }
   

7. CMakeLists.txt文件

   cmake_minimum_required(VERSION 3.10)
   project(UseEigen)
   set(CMAKE_BUILD_TYPE "Release")
   set(CMAKE_CXX_FLAGS "-std=c++11 -g -Wall -O2")
   set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
   set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
   //eigen只有头文件，没有需要链接的库文件
   include_directories("/usr/local/include/eigen3")
   add_executable(UseEigen src/main.cpp)