C++ Eigen矩阵库

Posted on | In , |

矩阵

[TOC]

官方文档

Offical Eigen Doc

安装

https://gitlab.com/libeigen/eigen/-/releases

1
2
3
4
5
# Ubuntu 系统默认版本安装
sudo apt-get install libeigen3-dev

安装路径
    /usr/include/eigen3
1
2
3
4
5
6
7
8
9
mkdir build
cd build
cmake..
sudo make install

安装路径
    /usr/local/include/eigen3 
# 
make uninstall

Map

Map的定义：

1
Map< Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime> >

在这种默认的定义中，Map只需要一个模板参数-Matrix。
为了构造Map变量，需要另两个信息：一个指针，该指针指向用于定义数组元素的内存区域；另一个是希望得到的matrix或vector的形状。

比如：定义一个float类型、动态尺寸大小的matrix：

1
Map<MatrixXf> mf(pf,rows,columns);

其中pf是一个float *类型的指向数组内存的指针。

定义一个固定大小的、整形的、只读vector:

1
Map<const Vector4i> mi(pi);

其中，pi是一个int *类型的指针。在这个例子中不需要传递尺寸大小给构造函数，因为尺寸大小已经由Matrix/Array类型确定了。

注意:Map没有默认的构造函数，你需要传递一个指针来初始化对象。

Map能够足够灵活地去容纳多种不同的数据表示，其他的两个模板参数：

1
2
3
Map<typename MatrixType,
    int MapOptions,
    typename StrideType>

MapOptions标识指针是否是对齐的（Aligned or Unaligned），默认是Unaligned。
StrideType表示内存数组的组织方式：行列的步长。

Map矩阵赋值(数组转mxtrix)

blog: Map class:连接Eigen与C++的数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//matrix_map1.cpp
#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    int array[9];
    for(int i = 0; i < 9; ++i) array[i] = i;

    Map<VectorXi>  vi(array,9);
    Map<const Vector4i> fixed_v(array);    // unknown type name 'Vector5i' -- 这种模式size不能大于4
    cout<< "vector vi: "<< endl << vi << endl<< endl;
    cout<< "fixed-vector : "<< endl << fixed_v << endl<< endl;

    cout << "Column-major:\n" << Map<Matrix<int,2,4> >(array) << endl;
    cout << "Row-major:\n" << Map<Matrix<int,2,4,RowMajor> >(array) << endl;
    cout << "Row-major using stride:\n" <<
        Map<Matrix<int,2,4>, Unaligned, Stride<1,4> >(array) << endl;
}

Matrix to c++数组

1
2
3
4
5
6
7
8
Matrix3d eigMat;

// 法1
double* eigMatptr = eigMat.data();

// 法2
double* eigMatptrnew = new double[eigMat.size()];
Map<MatrixXd>(eigMatptrnew, eigMat.rows(), eigMat.cols()) = eigMat;

使用Map变量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
typedef Matrix<float,1,Dynamic> MatrixType;
typedef Map<MatrixType> MapType;
typedef Map<const MatrixType> MapTypeConst;   // a read-only map
const int n_dims = 5;

MatrixType m1(n_dims), m2(n_dims);
m1.setRandom();
m2.setRandom();
float *p = &m2(0);  // get the address storing the data for m2
MapType m2map(p,m2.size());   // m2map shares data with m2
MapTypeConst m2mapconst(p,m2.size());  // a read-only accessor for m2
cout << "m1: " << m1 << endl;
cout << "m2: " << m2 << endl;
cout << "Squared euclidean distance: " << (m1-m2).squaredNorm() << endl;
cout << "Squared euclidean distance, using map: " <<  (m1-m2map).squaredNorm() << endl;
m2map(3) = 7;   // this will change m2, since they share the same array
cout << "Updated m2: " << m2 << endl;
cout << "m2 coefficient 2, constant accessor: " << m2mapconst(2) << endl;
/* m2mapconst(2) = 5; */   // this yields a compile-time error

修改Map数组

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//matrix_map3.cpp
#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    int data[] = {1,2,3,4,5,6,7,8,9};
    Map<RowVectorXi> ov(data,9);
    cout << "Origin data: " << ov << endl; 

    Map<RowVectorXi> v(data,4);
    cout << "The mapped vector v is: " << v << "\n";

    // placement new 
    new (&v) Map<RowVectorXi>(data+4,5);
    cout << "Changed, Now v is: " << v << "\n";
    cout << "Again origin data: " << ov << endl; 
}

矩阵拼接

1.cv::Mat矩阵拼接（注意在拼接方向上的维度应该相同）

1
2
cv::vconcat(C, A, B); //垂直拼接
cv::hconcat(C, A, B); //水平拼接

2.Eigen矩阵拼接（注意矩阵拼接之前必须要确定大小! 否则会报错）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
MatrixXd A;
MatrixXd B;
MatrixXd C;
A.resize(3, 3);   //注意矩阵拼接之前必须要确定大小，否则会报错
B.resize(3, 9);   //注意矩阵拼接之前必须要确定大小，否则会报错
C.resize(9, 3);   //注意矩阵拼接之前必须要确定大小，否则会报错
    A << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    cout<<"A:"<<A<<endl;
    B << A,A,A; //水平拼接
    cout<<"B:"<<B<<endl;
  //垂直拼接
    C << A,
         A,
         A;
    cout<<"C:"<<C<<endl;

模块介绍

向量运算（.T,Sum,Trace,Inverse…）

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

1
2
3
4
5
6
7
8
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;      //求行列式

几何模块（四元数，欧拉角，旋转等）

使用Eigen库的几何模块时，需要声明头文件<Eigen/Geometry>，此模块支持进行四元数、欧拉角和旋转矩阵的运算。各种常见形式的表达方式如下所示：

1
2
3
4
5
6
7
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）

eigenGeometry Demo

  • 旋转
  • 四元数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>


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

    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    //Identity() -> 单位矩阵
    Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Identity();
    std::cout << "rotation matrix: \n" << rotation_matrix << std::endl;

    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    //沿 Z 轴旋转 45 度
    Eigen::AngleAxisf rotation_vector(M_PI/4,Eigen::Vector3f(0, 0, 1));
    std::cout.precision(3);
    std::cout << "rotation matrix: \n" << rotation_vector.matrix() << std::endl;

    rotation_matrix = rotation_vector.toRotationMatrix();
    std::cout << "rotation matrix: \n" << rotation_matrix << std::endl;


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

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

    //欧拉角: 可以将旋转矩阵直接转换成欧拉角
    // ZYX顺序，即yaw-pitch-roll顺序
    Eigen::Vector3f euler_angles = rotation_matrix.eulerAngles(2,1,0);
    std::cout << "yaw-pitch-roll = " << euler_angles.transpose() << std::endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    // 虽然称为3d，实质上是4＊4的矩阵
    Eigen::Isometry3f T = Eigen::Isometry3f::Identity();
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;
    T.rotate(rotation_vector);                                    // 按照rotation_vector进行旋转
    T.pretranslate(Eigen::Vector3f(1, 2, 3));         // 把平移向量设成(1,3,4)
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3f v_transformed = T * v;                       // 相当于R*v+t
    std::cout << "v tranformed = " << v_transformed.transpose() << std::endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaternionf q = Eigen::Quaternionf(rotation_vector);
    std::cout << "quaternion from rotation vector = " << q.coeffs().transpose()
         << std::endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaternionf(rotation_matrix);
    std::cout << "quaternion from rotation matrix = " << q.coeffs().transpose()
    << std::endl;

//    // 使用四元数旋转一个向量，使用重载的乘法即可
    Eigen::Vector3f v_rotated1 =  q * v ;// 注意数学上是qvq^{-1}
    std::cout << "(1,0,0) after rotation (by Quaternion) =    " << v_rotated1.transpose() << std::endl;

    // 用常规向量乘法表示，则应该如下计算
    std::cout << "should be equal to     = " << (q * Eigen::Quaternionf(0, 1, 0, 0) * q.inverse()).coeffs().transpose()
    << std::endl;

    return 0;
}

参考资料链接：