Eigenl 库的基本使用以及Eigen 库的几何模型(SLAM 十四讲 ch3(1))

最新推荐文章于 2024-03-17 12:01:36 发布

slam已经入土

最新推荐文章于 2024-03-17 12:01:36 发布

阅读量343

点赞数

本文链接：https://blog.csdn.net/joun772/article/details/109246234

版权

SLAM eigen 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

本文通过两篇博客详细介绍了Eigen库的使用，包括矩阵的基本操作如初始化、乘法、转置、求逆、特征值及特征向量，以及几何变换中的旋转和平移。展示了如何利用AngleAxis、Quaternion和Isometry3d进行旋转和平移，并对比了不同的求解线性方程组的方法。

摘要由CSDN通过智能技术生成

1、Eigen 库的基本使用

//
// Created by wenbo on 2020/10/22.
//
//this .cpp was maked by myself
//熟悉Eigen库对于矩阵的基本使用
#include<iostream>
#include <ctime>
#include"Eigen/Core"
#include "Eigen/Dense"
using namespace std;
using namespace Eigen;
#define MATRIX_SIZE 50
/*
 * eigen库的演示by myself
 */
int main(int argc,char** argv)
{
    Matrix<float, 2 , 3> matrix_23;
    Vector3d v_3d;//double类型
    Matrix<float ,3,1> vd_3d;
    Matrix3d matrix_33=Matrix3d::Zero();
    Matrix<double, Dynamic,Dynamic> matrix_dynamic;
    MatrixXd matrix_x;
    //initialization
    matrix_23<< 1, 2, 3, 4, 5, 6;
    cout<< "matrix_23 = " << endl<< matrix_23 <<endl;
    //get the elements from matrix_23
    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;
    //
    // Matrix<double ,2 ,1> result_wrong=matrix_23*v_3d; 由于matrix_23是float类型，而v_3d是double类型，故不能相乘
    //两种不同类型的矩阵不能混合
    //可以显示转换
    Matrix<double, 2, 1> result = matrix_23.cast<double>()*v_3d;
    cout<< "( 强制将matrix_23转为float )matrix_23*v_3d" <<endl<<result<<endl;

    Matrix<float, 2, 1> result_2= matrix_23*vd_3d;

    cout<< "matrix_23*vd_3d = " <<endl<<result_2 <<endl;
    matrix_33 = Matrix3d::Random(); //set matrix_33 to a random matrix
    cout<< "matrix_33 = " <<endl<< matrix_33<<endl;
    cout<< "matrix_33.transpose() = " <<endl<<matrix_33.transpose() <<endl;
    cout<< " matrix_33各元素的和 = " << matrix_33.sum() <<endl;
    cout<< " matrix_33的trace = " << matrix_33.trace() <<endl;
    cout<< " 10*matrix_33 = " << 10*matrix_33 <<endl;
    cout<< " matrix_33的inverse = " << matrix_33.inverse() <<endl;
    cout<< " matrix_33的det = " << matrix_33.determinant()<<endl;
    //特征值和特征向量
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
    cout<< "eigen values of matrix_33.transpose()*matrix_33 = " << endl <<eigen_solver.eigenvalues()<<endl;
    cout<< "eigen vectors of matrix_33.transpose()*matrix_33 = " <<endl<<eigen_solver.eigenvectors()<<endl;
    //解方程
    //matrix_NN*x=v_Nd;
    //N 50
    //using 2 methods to solve linear equation
    //1、to get inverse directly
    //2、using QR 分解
    //1、**********
    cout<<"**********************************"<<endl;
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN=MatrixXd::Random();
    matrix_NN=matrix_NN*matrix_NN.transpose();//保证半正定？？
    Matrix<double, MATRIX_SIZE, 1> v_Nd=MatrixXd::Random();//随机的v_Nd
    clock_t time_stt=clock();
    Matrix<double, MATRIX_SIZE, 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 = " << endl <<x.transpose() <<endl;
    //using QR decomposition
    //matrix_NN*x=v_Nd
    //x=matrix_NN.colPivHouseholderQr().solve(v_Nd)
    cout<<"**********************************"<<endl;
    time_stt=clock();
    x=matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<"time of normal inverse is "
       <<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
    cout<< "X = " << endl <<x.transpose() <<endl;
    //using cholesky decomposition
    //matrix_NN*x=v_Nd
    //x=matrix_NN.colPivHouseholderQr().solve(v_Nd)
    cout<<"**********************************"<<endl;
    time_stt=clock();
    x=matrix_NN.ldlt().solve(v_Nd);
    cout<<"time of normal inverse is "
        <<1000*(clock()-time_stt)/(double) CLOCKS_PER_SEC<<"ms"<<endl;
    cout<< "X = " << endl <<x.transpose() <<endl;
    return 0;
}
运行结果如下：

```cpp
matrix_23 = 
1 2 3
4 5 6
1	2	3	
4	5	6	
( 强制将matrix_23转为float )matrix_23*v_3d
10
28
matrix_23*vd_3d = 
32
77
matrix_33 = 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
matrix_33.transpose() = 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
 matrix_33各元素的和 = 1.61307
 matrix_33的trace = 1.05922
 10*matrix_33 =  6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
 matrix_33的inverse = -0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
 matrix_33的det = 0.208598
eigen values of matrix_33.transpose()*matrix_33 = 
0.0242899
 0.992154
  1.80558
eigen vectors of matrix_33.transpose()*matrix_33 = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
**********************************
time of normal inverse is 0.203ms
X = 
-55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
**********************************
time of normal inverse is 0.099ms
X = 
-55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
**********************************
time of normal inverse is 0.028ms
X = 
-55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

2、 Eigen 库的几何模型

//
// Created by wenbo on 2020/10/23.
//this .cpp was maked by byself
//熟悉eigen库中几何模块的运用（旋转，平移）
#include<iostream>
#include <ctime>
#include"Eigen/Core"
#include "Eigen/Geometry"
using namespace std;
using namespace Eigen;
int main(int argc,char** argv)
{
    //eigen Geometry 提供了各种旋转和平移的表示
    Matrix3d rotatian_matrix=Matrix3d::Identity();//单位矩阵
    AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1));//rotation_vector无法输出？
    cout<<" rotatian_matrix = "<<endl<<rotation_vector.matrix()<<endl;
    rotatian_matrix=rotation_vector.toRotationMatrix();
    //cout<<" rotatian_matrix = "<<endl<<rotation_vector.matrix()<<endl;
    Vector3d v(1,0,0);
    Vector3d v_rotate=rotation_vector*v;
    cout<<"v_rotate = "<<v_rotate.transpose()<<" (using rotation_vector) "<<endl;
    v_rotate=rotatian_matrix*v;
    cout<<"v_rotate = "<<v_rotate.transpose()<<" (using rotation_matrix) "<<endl;
    Vector3d  euler_angles= rotatian_matrix.eulerAngles(0.707107,0.707107,0);//ZYX顺序，roll,pitch,yaw,即 偏航，俯仰，滚转
    cout<<" roll pitch yaw = "<< euler_angles.transpose() <<endl;
    //欧式变换矩阵Eigen::Isometry
    Isometry3d  T=Isometry3d ::Identity();//实际上是4x4矩阵
   // cout<<"T = "<<endl<< T <<endl;//无法直接输出T?? 因为T的初始化不够完整？？
    T.rotate(rotation_vector);
    T.pretranslate(Vector3d(1,3,4));
    cout<<"Transform matrix = "<< endl <<T.matrix() <<endl;//可以通过这个输出完整的T
    //cout<<"Transform matrix = "<< endl <<T.pretranslate() <<endl; 错误
    //用变换矩阵进行坐标变换
    Vector3d v_transformed =T*v; //相当于 R*v +t
    cout<<" v_transformed = "<< v_transformed.transpose() <<endl;
    //仿射变换 Affine3d和 射影变换Projective3d
    cout<<"*******仿射变换 Affine3d和 射影变换Projective3d********"<<endl;
    Affine3d Aff=Affine3d ::Identity();
    Aff.rotate(rotation_vector);
    Aff.pretranslate(Vector3d(1,3,4));
    cout<<"Affine matrix = "<< endl <<Aff.matrix() <<endl;
    Projective3d Pro=Projective3d ::Identity();
    Pro.rotate(rotation_vector);
    Pro.pretranslate(Vector3d(1,3,4));
    cout<<"Projective matrix = "<< endl <<Pro.matrix() <<endl;
    cout<<"******************四元数****************"<<endl;
    Quaterniond q= Quaterniond(rotation_vector);
    cout<<"Quaterniond from rotation_vector = "<<endl<<q.coeffs().transpose()<<endl;//请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    q=Quaterniond(rotatian_matrix);
    cout<<"Quaterniond from rotation_matrix = "<<endl<<q.coeffs().transpose()<<endl;//请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    v_rotate=q*v;//数学上是qvq(-1)?
    cout<<"(1,0,0)after rotation = "<< v_rotate.transpose() <<endl;
    cout<<"用数学方法表述 = q*quaterniond(0,1,0,0)*q(-1) = "<<(q*Quaterniond(0,1,0,0)*q.inverse()).coeffs().transpose()<<endl;
    return 0;
}


运行结果如下：


rotatian_matrix = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
v_rotate = 0.707107 0.707107        0 (using rotation_vector) 
v_rotate = 0.707107 0.707107        0 (using rotation_matrix) 
 roll pitch yaw =        0 0.785398        0
Transform matrix = 
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         3
        0         0         1         4
        0         0         0         1
 v_transformed = 1.70711 3.70711       4
*******仿射变换 Affine3d和 射影变换Projective3d********
Affine matrix = 
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         3
        0         0         1         4
        0         0         0         1
Projective matrix = 
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         3
        0         0         1         4
        0         0         0         1
******************四元数****************
Quaterniond from rotation_vector = 
       0        0 0.382683  0.92388
Quaterniond from rotation_matrix = 
       0        0 0.382683  0.92388
(1,0,0)after rotation = 0.707107 0.707107        0
用数学方法表述 = q*quaterniond(0,1,0,0)*q(-1) = 0.707107 0.707107        0        0