Eigen模块(用于矩阵运算)
个人理解应该类似python的numpy模块,提供了大量用于矩阵运算的库函数。
Eigen是使用纯粹头文件搭建的开源库,据一些资料说比OpenCV中的Mat更加高效。
参考书籍:《视觉SLAM十四讲-从理论到实践》——高翔
运行时需要在CMakeLists.txt中加入:
# 推荐方式
find_package(Eigen3 REQUIRED)
# 不推荐方式
# include_directories("/usr/include/eigen3")
# 两种一起使用会导致编译时出现重复定义报错
然后在cpp中加入代码:
#include
#include
using namespace Eigen;
未编译时,VS会对include部分报错,在编译过一次之后,它不会再报错。
重新启动后需要再重复一次编译操作取消报错。
注:Eigen的赋值只能在函数内部进行,全局只能申明变量,对其赋值会导致出错!!!
// 根据索引访问矩阵中的元素
auto element = eigen_matrix(i, j);
#include
using namespace std;
#include
// 稠密矩阵的运算库
#include
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
// 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
#include
using namespace std;
#include
// 稠密矩阵的运算库
#include
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
参考书籍:《视觉SLAM十四讲-从理论到实践》——高翔
#include
using namespace std;
#include
// 稠密矩阵的运算库
#include
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
可以看出三种解法的答案是一样的,理论上越后面的计算越快。
使用于:八点法
// 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);
使用于:PnP问题
// 解Hx=b
Vector6d dx;
x = H.ldlt().solve(b);
// 如果解Hx=b失败
if (isnan(x[0]))
{
cout << "result is nan!" << endl;
break;
}
警告!不能使用a=a.transpose();
这样的写法!在原本的内存地址上直接操作会导致读写冲突!可以用a=a.transpose().eval();
来解决这个问题。
eval()能够用临时变量来解决左右变量内存地址相同时产生的混淆问题。
#include
#include
using namespace std;
#include
// 导入Eigen几何模块
#include
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
#include
#include
using namespace std;
#include
// 导入Eigen几何模块
#include
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
仿射变换Eigen::Affine3d
射影变换Eigen::Projective3d
具体细节以后研究了再补充。
#include
#include
using namespace std;
#include
// 导入Eigen几何模块
#include
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
Eigen::Matrix4d Twi = Eigen::Matrix4d::Identity();
Twi.topLeftCorner<3, 3>() = Rwi;
Twi.topRightCorner<3, 1>() = twi;