1.矩阵运算命令及实例

使用Eigen库的矩阵(包括向量)运算时，需要声明头文件，矩阵执行常见的运算指令：

1.1 Eigen block

//① 行列
 Eigen::MatrixXd::Zero(pcl.size(),3); n*3 (rows.)
m8.row(i);
m8.col(j);

//② block<>()
Matrix<double,p,q> m9 = m8.block(i,j,p,q);//起点(i, j)，块大小(p, q)，构建一个动态尺寸的block
m8.block<p,q>(i,j);

Eigen::MatrixXf m4(2,2);
  m4 << 1,2,3,4;
  Eigen::MatrixXf m5(4,4);
  m5 << m4, m4 / 10, m4 * 10, m4;//将m5分了四块赋值
  std::cout<<" m5 = "<<m5<<std::endl;
  Eigen::MatrixXf m6 =m5.block(1,0,2,3); //m5.block<1,0>(1,1);
  std::cout<<" m6 = "<<m6.size()<<std::endl;
  std::cout<<" m6 rows= "<<m6.rows()<<std::endl;
  std::cout<<" m6 cols= "<<m6.cols()<<std::endl;

  Eigen::MatrixXd m_cloud = Eigen::MatrixXd::Zero(pcl.size(),3);
  std::cout<<" m_cloud rows= "<<m_cloud.rows()<<std::endl;
  std::cout<<" m_cloud cols= "<<m_cloud.cols()<<std::endl;

  for (int i = 0; i < pcl.size(); ++i) {
    m_cloud.block(i,0,1,3)<<pcl[i].x(),pcl[i].y(),pcl[i].y();
  }
  std::cout<<" m_cloud = "<<m_cloud.size()<<std::endl;

1.1 基本运算

matrix_33 = Matrix3d::Random();                           //生成一个3*3的随机矩阵
cout << "random matrix: n"<< matrix_33 << endl;
cout << "transpose : n"<< matrix_33.transpose() << endl;  //转置
cout << "sum :" << matrix_33.sum() << endl;                //求和
cout << "trace : "<< matrix_33.trace() << endl;            //求迹
cout << "time 10: n"<< 10 * matrix_33 << endl;            //数乘
cout << "inverse : n"<< matrix_33.inverse() << endl;      //求逆
cout << "det : n"<< matrix_33.determinant() << endl;      //求行列式

1.2 Eigen矩阵初始化，求特征值，求逆，矩阵分解

#include <iostream>
using namespace std;
#include <ctime>
//eigen核心部分
#include <Eigen/Core>
//稠密矩阵的代数运算（逆、特征值等）
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SEZE 50

int main(int argc, char **argv)
{
    //声明一个2*3的矩阵，eigen中所有的向量矩阵均为Eigen::Matrix，是一个模板类。
    //三个参数分别为数据类型、行数、列数
    Matrix<float, 2, 3> matrix_23;

    //声明一个三维向量，Vector_3d实际上是一个通过typedef定义的内置类型，本质上仍然是Eigen::Matrix模板类
    Vector3d v_3d;
    //Matrix<float, 2, 3> vd_3d;

    //声明一个3*3的矩阵，并初始化为0矩阵
    Matrix3d matrix_33 = Matrix3d::Zero();

    //不确定矩阵大小时，可以使用动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;

    //或者更简单的方式
    MatrixXd matrix_x;

    //准备对Eigen矩阵初始化
    //输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    //shuchu
    cout << "matrix 2*3 from 1 to 6 : n"<< matrix_23 << endl;

    //使用（）访问矩阵中的元素
    cout << "print matirx 2*3 : "<< endl;
    for (int i = 0; i < 2; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            cout << matrix_23(i,j) << "t";
        }
        cout << endl;
    }

    //初始化两个三维向量
    v_3d << 3, 2, 1;
    //vd_3d << 4, 5, 6;
    //不能混乘两种类型的矩阵，因此需要显示转换类型：.cast<type>
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1] = "<< result.transpose() << endl;
    //.transpose()是转置函数
    cout << "not transpose [1,2,3;4,5,6]*[3,2,1] = "<< result << endl;

    Matrix<float, 2, 1> result2 = matrix_23 * v_3d.cast<float>();
    cout << "[1,2,3;4,5,6]*[3,2,1] = "<< result2.transpose() << endl;

    //生成一个随机矩阵
    matrix_33 = Matrix3d::Random();
    cout << "random matrix: n"<< matrix_33 << endl;
    cout << "transpose : n"<< matrix_33.transpose() << endl;  //转置
    cout << "sum :" << matrix_33.sum() << endl;                //求和
    cout << "trace : "<< matrix_33.trace() << endl;            //求迹
    cout << "time 10: n"<< 10 * matrix_33 << endl;            //数乘
    cout << "inverse : n"<< matrix_33.inverse() << endl;      //求逆
    cout << "det : n"<< matrix_33.determinant() << endl;      //求行列式

    //求特征值
    //实对称矩阵可以保证对角化成功
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);             //SelfAdjointEigenSolver<Matrix3d>y用于求解特征值和特征向量
    cout << "Eigen values = n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = n" << eigen_solver.eigenvectors() << endl;

    //练习求解方程
    //求解Matrix_NN * x = v_NN方程
    Matrix<double, MATRIX_SEZE, MATRIX_SEZE> matrix_NN
        = MatrixXd::Random(MATRIX_SEZE,MATRIX_SEZE);     //MATRIX_SEZE在前面宏定义中
    matrix_NN = matrix_NN *matrix_NN.transpose();        //此处理可以保证矩阵是半正定的
    Matrix<double, MATRIX_SEZE, 1> v_Nd = MatrixXd::Random(MATRIX_SEZE, 1);

    clock_t time_stt = clock();                          //用于计时

    //直接求逆
    Matrix<double, MATRIX_SEZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time of normal inverse is"
        << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = "<< x.transpose() << endl;

    //再用矩阵分解，例如QR分解求解，速度将会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    //对于正定矩阵，还可以用cholesky分解来分解方程
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    return 0;
}

2 几何模块命令及实例（四元数、欧拉角和旋转矩阵）

2.1 使用Eigen库的几何模块时，需要声明头文件，此模块支持进行四元数、欧拉角和旋转矩阵的运算。

各种常见形式的表达方式如下所示：

Eigen::Matrix3d      //旋转矩阵（3*3）
Eigen::AngleAxisd    //旋转向量（3*1）
Eigen::Vector3d      //欧拉角（3*1）
Eigen::Quaterniond   //四元数（4*1）
Eigen::Isometry3d    //欧式变换矩阵（4*4）
Eigen::Affine3d      //放射变换矩阵（4*4）
Eigen::Projective3d  //射影变换矩阵（4*4）

2.2 上述数据类型均为双精度(double)类型，若要改为单精度(float)类型，把最后的d改为f即可。

下面给出程序实例，看完实例应该能基本了解了几何变换的形式，若对相关数学知识不了解，可以相应去学习一下：

//本程序将演示Geometry几何模块的使用
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
//Geometry模块提供了各种旋转和平移的表示
using namespace Eigen;

int main(int argc, char ** argv)
{
    //3d旋转矩阵可以直接使用Matrix3d或者Matrix3f
    Matrix3d rotation_matrix = Matrix3d :: Identity();

    //旋转向量使用AngleAxis，运算可以当做矩阵
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0,0,1));     //眼Z轴旋转45°
    cout.precision(3);                                         //输出精度为小数点后两位
    cout << "rotation matrix = n" << rotation_vector.matrix() << endl;
    //用matrix转换成矩阵可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();

    //使用Amgleanxis可以进行坐标变换
    Vector3d v(1, 0, 0);
    Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;

    //使用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

    //欧拉角：可以将矩阵直接转换成欧拉角
    Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);       //按照ZYX顺序
    cout << "yaw pitch row = "<< euler_angles.transpose() << endl;

    //欧式变换矩阵使用Eigen::Isometry
    Isometry3d T = Isometry3d::Identity();      //实质为4*4的矩阵
    T.rotate(rotation_vector);                  //按照rotation_vector进行转化
    T.pretranslate(Vector3d(1, 3, 4));          //平移向量设为（1， 3， 4）
    cout << "Transform matrix = n" << T.matrix() <<endl;

    //变换矩阵进行坐标变换
    Vector3d v_transformed = T *v;
    cout << "v transormed =" << v_transformed.transpose() << endl;

    //四元数
    //直接把AngleAxis赋值给四元数，反之亦然
    Quaterniond q = Quaterniond(rotation_vector);
    cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
    q = Quaterniond(rotation_matrix);
    cout << "quaternion from rotation matrix = "<< q.coeffs().transpose() << endl;

    //使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
    cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

    return 0;
}