Eigen 使用

头文件

#include <Eigen/Core>
//Eigen核心部分
#include <Eigen/Dense>
//稠密矩阵的代数运算（逆、特征值等）

常用接口

可以参考这里

Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换

这小节的内容参考博客。

一、旋转向量

1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

1.1 旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.2 旋转向量转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

1.3 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;
Quaterniond quaternion;

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

二、旋转矩阵

2.0 初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2.1 旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.2 旋转向量转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

2.3 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

三、欧拉角

3.0 初始化欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

3.1 欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3.2 欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.3 欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

四、四元数

4.0 初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);// 第一种方式
Eigen::Quaterniond quaternion(Vector4d(x, y, z, w));// 第二种方式
Eigen::Quaterniond quaternion(Matrix3d(R));// 第三种方式

quaternion.normalize(); // 最后进行归一化处理
std::cout<< "(" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << ")" << std::endl;	// 打印4元数

4.1 四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.2 四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

4.4 四元数转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

五、欧氏变换矩阵

以上都只是旋转变换函数，现在讨论旋转+平移的情况。3D空间中的变换，有欧氏变换、相似变换、仿射变换和射影变换，一共4种情况。这里只是讨论属于刚性变换的欧氏变换。其余还没用到，应该是大同小异。

旋转向量+平移向量 转 欧氏矩阵

Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
// 欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
//T.translate(Eigen::Vector3d(1, 3, 4));                    // 把平移向量设成(1,3,4)，没有pre，则看旋转和平移的代码位置，哪个在前就写在前面
//cout << "Transform matrix = \n" << T.matrix() << endl;
T.pretranslate(Eigen::Vector3d(1, 3, 4));                    // 把平移向量设成(1,3,4)，有pre，所以如果旋转没有pre，则写在前面，即先旋转；如果旋转也有pre，应该也是看谁在前面
/*  
 猜想如下：Eigen中，对于 有没有pre的效果，总结如下，假设原矩阵为M，操作矩阵为 G：
1、没有pre，则 M=M*G，即直接后乘，直接加到后面的
2、有pre，则 M=G*M，即直接前乘，直接加到前面

 例子如下，假设旋转矩阵为R，平移为T
1、两个都是没有pre，比如：如果是M.rotate，则M=M*R；再是M.translate，则为M=M*T；然后总的就是 M=M*R*T；
2、一个有pre，另一个没有pre：比如：M.prerotate 与 M.translate，则 M=R*M*T
3、两个都有 pre，比如：先M.prerotate 再 M.pretranslate，则M=T*R*M
*/
cout << "Transform matrix = \n" << T.matrix() << endl;

欧氏矩阵 转 旋转向量+平移向量

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();                // 初始化欧氏矩阵，这里用单位阵示范
Eigen::AngleAxisd rot_vect(T.rotation());	//转旋转向量
cout << "rot_vect = \n" << rot_vect.matrix() << endl;
Eigen::Vector3d tran_vect = T.translation(); //转平移向量
cout << "tran_vect = \n" << tran_vect << endl;
Eigen::Matrix4d rot_test = T.matrix();	//转4*4的矩阵
cout << "Transform matrix = \n" << rot_test << endl;

动态数组的使用

float row = 3, col = 4;
/* 定义，定义时默认没有初始化，必须自己初始化 */ 
Eigen::MatrixXf tmp_mat;
/* 初始化 */  
tmp_mat = Eigen::MatrixXf::Zero(row, col);

还可以写：4维矩阵转4元数和平移向量等

方程求解

使用Eigen求解比如：$Ax=b$这种方程，可以有QR分解、LU分解等做法，可以参考下博客

与opencv之间的转换

opencv转Eigen

// cvMat2Eigen
Eigen::Isometry3d cvMat2Eigen( cv::Vec3d& rvec, cv::Vec3d tvec )
{
    cv::Mat R;
    cv::Rodrigues( rvec, R );
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);
  
    // 将平移向量和旋转矩阵转换成变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    Eigen::AngleAxisd angle(r);
    Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
    T = angle;
    T(0,3) = tvec.at<double>(0,0); 
    T(1,3) = tvec.at<double>(0,1); 
    T(2,3) = tvec.at<double>(0,2);
    return T;
}

Eigen转opencv

#include <Eigen/Dense>
#include <iostream>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
 
using namespace std;
using namespace cv;
using namespace Eigen;
 
void main()
{
    Mat img = imread("example.jpg",CV_LOAD_IMAGE_GRAYSCALE);
    int row = img.rows;
    int col = img.cols;
    MatrixXd m(row, col);
    cv2eigen(img,m);
    return;
}