C++学习笔记——Eigen模块（用于矩阵运算）

Eigen模块（用于矩阵运算）
个人理解应该类似python的numpy模块，提供了大量用于矩阵运算的库函数。
Eigen是使用纯粹头文件搭建的开源库，据一些资料说比OpenCV中的Mat更加高效。
参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔

1. 运行环境

运行时需要在CMakeLists.txt中加入：

# 推荐方式
find_package(Eigen3 REQUIRED)
# 不推荐方式
# include_directories("/usr/include/eigen3")
# 两种一起使用会导致编译时出现重复定义报错

然后在cpp中加入代码：

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

未编译时，VS会对include部分报错，在编译过一次之后，它不会再报错。
重新启动后需要再重复一次编译操作取消报错。

2. 基本矩阵操作

注：Eigen的赋值只能在函数内部进行，全局只能申明变量，对其赋值会导致出错！！！

// 根据索引访问矩阵中的元素
auto element = eigen_matrix(i, j);

#include <iostream>
using namespace std;

#include <Eigen/Core>
// 稠密矩阵的运算库
#include <Eigen/Dense>

using namespace Eigen;

int main(int argc, char **argv) {
    // 基本矩阵创建方式，定义数据类型，行数，列数
    Matrix<float, 5, 3> matrix_53;
    // 定义一个大小不确定的动态矩阵
    Matrix<int, Dynamic, Dynamic> matrix_dynamic;

    // 3代表3x3矩阵， f代表float类型矩阵， 等价于Matrix<float, 3, 3>
    // 3d则代表double类型的矩阵
    Matrix3f matrix_33;
    // 创建向量，等价于3x1的矩阵，数据类型为double
    Vector2d v_2d;
    // 为矩阵进行随机初始化
    matrix_33 = Matrix3f::Random();
    // 为矩阵进行置零初始化
    Matrix3f identity_matrix_33 = Matrix3f::Identity();
    // 用单位矩阵进行初始化
    Matrix3f zero_matrix_33 = Matrix3f::Zero();

    // 在屏幕上打印矩阵
    cout << "matrix_33随机赋值：" << endl << matrix_33 <<endl;
    cout << "matrix_33单位矩阵赋值：" << endl << identity_matrix_33 <<endl;
    cout << "matrix_33置零赋值：" << endl << zero_matrix_33 <<endl;

    // 为矩阵赋值，按照每一行的顺序用逗号分割，中间不能出现分号
    Matrix<float, 4, 2> matrix_42;
    matrix_42  << 1, 2, 3, 4, 5, 6, 7, 8;
    cout << "matrix_42赋值后为：" << matrix_42 << endl;

    // 遍历一个矩阵，依次取值
    cout << "遍历矩阵：" << endl;
    for (int i=0; i<4; i++){
        for (int j=0; j<2; j++){
            cout << matrix_42(i, j) << endl;
        }
    }

    // 显式转换矩阵的数据类型
    // Eigen不支持自动类型转换，必须显式转换矩阵到相同数据类型后才能进行计算
    // 与python不同，C++无法将已经定义了数据类型的变量修改为接收不同类型的数据
    // 只能定义一个新的变量并为它分配内存空间来接收新的数据
    Matrix<int, 3, 3> new_matrix_33 = matrix_33.cast<int>();
    cout << "显式类型转换后的矩阵为：" << new_matrix_33 << endl;

    return 0;
}

运行得到：

matrix_33随机赋值：
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
matrix_33单位矩阵赋值：
1 0 0
0 1 0
0 0 1
matrix_33置零赋值：
0 0 0
0 0 0
0 0 0
matrix_42赋值后为：1 2
3 4
5 6
7 8
遍历矩阵：
1
2
3
4
5
6
7
8
显式类型转换后的矩阵为：0 0 0
0 0 0
0 0 0

3. 矩阵的特殊计算

#include <iostream>
using namespace std;

#include <Eigen/Core>
// 稠密矩阵的运算库
#include <Eigen/Dense>

using namespace Eigen;

int main(int argc, char **argv) {
    // 为矩阵赋随机值
    Matrix<float, 3, 3> matrix_33;
    matrix_33  << Matrix<float, 3, 3>::Random();
    cout << "原始矩阵：" << endl << matrix_33 << endl;

    // 创建一个新的矩阵并且接收原先矩阵的转置
    Matrix<float, 3, 3> matrix_transpose = matrix_33.transpose();
    cout << "转置矩阵：" << endl << matrix_transpose << endl;

    // 创建一个新的矩阵并且接收原先矩阵的共轭
    Matrix<float, 3, 3> matrix_conjugate = matrix_33.conjugate();
    cout << "共轭矩阵：" << endl << matrix_conjugate << endl;

    // 创建一个新的矩阵并且接收原先矩阵的共轭转置（伴随矩阵）
    Matrix<float, 3, 3> matrix_adjoint = matrix_33.adjoint();
    cout << "共轭转置矩阵（伴随矩阵）：" << endl << matrix_adjoint << endl;

    // 矩阵求和，所有元素的和
    cout << "矩阵求和：" << endl << matrix_33.sum() << endl;

    // 矩阵求迹，对角线（从左上方至右下方的对角线）上各个元素的总和
    cout << "矩阵求迹：" << endl << matrix_33.trace() << endl;

    // 矩阵乘数，所有元素都乘同一个数
    cout << "矩阵乘数：" << endl << matrix_33 * 10 << endl;

    // 矩阵求逆，即求逆矩阵
    cout << "矩阵求逆：" << endl << matrix_33.inverse() << endl;

     // 矩阵求行列式
    cout << "矩阵求行列式：" << endl << matrix_33.determinant() << endl;

    // 特征值与特征向量
    EigenSolver<Matrix3f> es(matrix_33);

     // 矩阵求特征值和特征向量
     // 原始输出是复数
     // 特征值
    auto eigenvalues = es.eigenvalues();
    // 特征向量
    auto eigenvectors = es.eigenvectors();
    // 将复数矩阵去除虚部，只留下实数部分输出
    cout << "特征值：" << endl << eigenvalues.real() << endl;
    cout << "特征向量：" << endl << eigenvectors.real() << endl;

    return 0;
}

运行得到：

原始矩阵：
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
转置矩阵：
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
共轭矩阵：
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
共轭转置矩阵（伴随矩阵）：
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
矩阵求和：
1.61307
矩阵求迹：
1.05922
矩阵乘数：
 6.80375   5.9688 -3.29555
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
矩阵求逆：
-0.198521   2.22739    2.8357
  1.00605 -0.555134  -1.41603
 -1.62213   3.59308   3.28973
矩阵求行列式：
0.208598
特征值：
0.166468
0.166468
0.726282
特征向量：
 0.591262  0.591262  0.916358
-0.355044 -0.355044  0.245215
 0.551331  0.551331  0.316477

4. 求解矩阵方程

4.1 求解matrix_NN * x = v_Nd

参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔

#include <iostream>
using namespace std;

#include <Eigen/Core>
// 稠密矩阵的运算库
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 5

int main(int argc, char **argv) {
    // 求解矩阵方程 matrix_NN * x = v_Nd

    // 生成方程
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    // 保证半正定
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double, MATRIX_SIZE, 1> v_Nd  = MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock();
    // 直接求逆
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout <<  x  << endl << endl;

    // QR分解
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<  x  << endl << endl;

    // cholesky分解，适用于正定矩阵
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout <<  x  << endl << endl;

    return 0;
}

运行得到：

4.31165
-10.5316
-5.96907
-3.91549
 1.58948

 4.31165
-10.5316
-5.96907
-3.91549
 1.58948

 4.31165
-10.5316
-5.96907
-3.91549
 1.58948

可以看出三种解法的答案是一样的，理论上越后面的计算越快。

4.2 求解Ax = 0

使用于：八点法

// SVD分解
// A=USV^T
JacobiSVD<Eigen::MatrixXf> svd(matrix_a_eigen, ComputeThinU | ComputeThinV);
auto V = svd.matrixV();
auto U = svd.matrixU();
// 计算S中所有的奇异值
auto A = svd.singularValues();
cout << "V" << V << endl << endl;
cout << "A" << A << endl << endl;

// 使用V矩阵的最后一列，即最小奇异值对应的奇异向量作为x的通解
Eigen::Matrix<float, 9, 1> x = V.col(7);

4.3 求解Hx = b

使用于：PnP问题

// 解Hx=b
Vector6d dx;
x = H.ldlt().solve(b);
// 如果解Hx=b失败
if (isnan(x[0]))
{
    cout << "result is nan!" << endl;
    break;
}

5. 混淆解决方案eval()

警告！不能使用a=a.transpose();这样的写法！在原本的内存地址上直接操作会导致读写冲突！可以用a=a.transpose().eval();来解决这个问题。
eval()能够用临时变量来解决左右变量内存地址相同时产生的混淆问题。

6. 几何模块【Eigen/Geometry】

6.1 旋转

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// 导入Eigen几何模块
#include <Eigen/Geometry>

using namespace Eigen;

int main(int argc, char **argv) {

    // 三维空间中的3D旋转矩阵是一个3x3的矩阵，double或者float数据类型都可以
    Matrix3d rotation_matrix = Matrix3d::Identity();
    cout << "旋转矩阵：" << endl << rotation_matrix << endl;

    // 旋转向量并不是用Matrix实现的，但是在实际使用中可以直接与矩阵计算
    // 旋转轴为(0, 0, 1)【Z轴】, 旋转度数为45度【弧度值，π相当于180度】
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
    // 旋转向量无法直接用cout输出，会导致编译错误
    // cout << "旋转向量：" << endl << rotation_vector << endl;

    // 将旋转向量转换为旋转矩阵的两种方法
    cout << "旋转向量转换为矩阵方法1：" << endl << rotation_vector.matrix()  << endl;
    cout << "旋转向量转换为矩阵方法2：" << endl << rotation_vector.toRotationMatrix() << endl;

    // 设置初始坐标
    Vector3d vec(1, 0, 0);
    cout << "初始坐标为：" << vec.transpose() << endl;
    // 使用旋转向量进行坐标变换
    cout << "使用旋转向量进行坐标变换：" << (rotation_vector * vec).transpose() << endl;
    // 使用旋转矩阵进行坐标变换
    cout << "使用旋转矩阵进行坐标变换：" << (rotation_vector.toRotationMatrix() * vec).transpose() << endl;

    // 将旋转矩阵转换为欧拉角【ZYX顺序，也就是滚转角roll，俯仰角pitch， 偏航角yaw顺序】
    // 欧拉角的具体意义为：
    // 1. 绕物体的Z轴以偏航角zaw旋转。
    // 2. 绕“旋转之后”的Y轴以俯仰角pitch旋转。
    // 3. 绕“旋转之后”的X轴以滚转角roll旋转。
    // 从而得到旋转向量[r, p, y]来描述一次转动
    // 0, 1, 2分别对应X, Y, Z轴
    Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    cout << "旋转矩阵转换为欧拉角：" << euler_angles.transpose() << endl;
    
    return 0;
}

运行得到：

旋转矩阵：
1 0 0
0 1 0
0 0 1
旋转向量转换为矩阵1：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
旋转向量转换为矩阵2：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
初始坐标为：1 0 0
使用旋转向量进行坐标变换：0.707107 0.707107        0
使用旋转矩阵进行坐标变换：0.707107 0.707107        0
旋转矩阵转换为欧拉角： 0 -0  0

6.2 欧氏变换矩阵（旋转+平移）

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// 导入Eigen几何模块
#include <Eigen/Geometry>

using namespace Eigen;

int main(int argc, char **argv) {

    // 三维的欧氏变换矩阵写法是3d，但其实是4x4的矩阵
    // 首先用单位矩阵初始化
    Isometry3d euclidean_transformation_matrix = Isometry3d::Identity();
    // 首先加入旋转
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
    euclidean_transformation_matrix.rotate(rotation_vector);
    // 然后加入平移向量
    Vector3d translate_vector(1, 3, 4);
    euclidean_transformation_matrix.pretranslate(translate_vector);
    // 同时包含旋转与平移信息的欧氏变换矩阵
    // 欧氏变换矩阵也不能直接用cout显示，需要用matrix()转换格式
    cout << "欧氏变换矩阵：" << endl << euclidean_transformation_matrix.matrix() << endl; 
    
    // 用变换矩阵对坐标进行变换
    // 初始坐标
    Vector3d vec(1, 0, 0);
    // 变换，相当于【旋转矩阵*坐标+平移向量】
    cout << "欧氏变换矩阵变换的坐标为：" << endl << (euclidean_transformation_matrix * vec).transpose() << endl;  
    
    // 分解T得到旋转和平移
    // 从(0,0)开始的3x3的块
    R = T.block<3,3>(0,0);
    // 从(0,3)开始的3x1的块
    t = T.block<3,1>(0,3);
    
    return 0;
}

运行得到：

欧氏变换矩阵：
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         3
        0         0         1         4
        0         0         0         1
欧氏变换矩阵变换的坐标为：
1.70711 3.70711       4

6.3 仿射变换与射影变换

仿射变换Eigen::Affine3d
射影变换Eigen::Projective3d
具体细节以后研究了再补充。

6.4 四元数（紧凑且没有奇异性的旋转）

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// 导入Eigen几何模块
#include <Eigen/Geometry>

using namespace Eigen;

int main(int argc, char **argv) {

    // 四元数(x, y, z, w)，其中w是实部
    // 由1个实部和3个虚部构成的向量，用于描述三维空间中的旋转
    // 比旋转矩阵更加紧凑无冗余，并且没有旋转向量那样的奇异性
    // 将旋转向量赋值给四元数
    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
    Quaterniond q1 = Quaterniond(rotation_vector);
    // 显示四元数的方法
    // 输出所有系数：x, y, z, w
    cout <<  "旋转向量得到的四元数：" << q1.coeffs().transpose() << endl;
    // 输出虚部：x, y, z
    cout <<  "旋转向量得到的四元数虚部：" << q1.vec().transpose() << endl;
    // 单独访问不同的参数
    cout << "单独访问参数：w " << q1.w() << "  x " << q1.x() << "  y " << q1.y() << " z  " << q1.z() << endl;

    // 对四元数进行单位化
    // 只有单位化的四元数才能表示旋转矩阵
    Quaterniond q_norm = q1.normalized();
    cout <<  "单位化的四元数：" << q_norm.coeffs().transpose() << endl;

    // 将旋转矩阵赋值给四元数
    Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
    Quaterniond q2 = Quaterniond(rotation_matrix);
    cout <<  "旋转矩阵得到的四元数：" << q2.coeffs().transpose() << endl;

    // 使用四元数旋转同样可以直接乘法，基于重载的复杂运算省略了步骤
    // 初始坐标
    Vector3d vec(1, 0, 0);
    // 使用四元数选择
    Vector3d new_vec = q1 * vec;
    cout <<  "四元数旋转得到的坐标：" << new_vec.transpose() << endl;

    // 不使用重载封装的方法而是用运算原理旋转
    cout <<  "四元数运算原理得到的坐标：" << (q1 * Quaterniond(0, vec(0), vec(1), vec(2)) * q1.inverse()).vec().transpose() << endl;

    return 0;
}

运行得到：

旋转向量得到的四元数：       0        0 0.382683  0.92388
旋转向量得到的四元数虚部：       0        0 0.382683
单独访问参数：w 0.92388  x 0  y 0 z  0.382683
单位化的四元数：       0        0 0.382683  0.92388
旋转矩阵得到的四元数：       0        0 0.382683  0.92388
四元数旋转得到的坐标：0.707107 0.707107        0
四元数运算原理得到的坐标：0.707107 0.707107        0

6.5 旋转矩阵R与平移矩阵t合并为变幻矩阵T

Eigen::Matrix4d Twi = Eigen::Matrix4d::Identity();
Twi.topLeftCorner<3, 3>() = Rwi;
Twi.topRightCorner<3, 1>() = twi;