【自看】Eigen 库 函数

1.矩阵定义

template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
class Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols >

Eigen::Matrix<int, 3, 4> mat1;              //  3x4 的 int 类型的矩阵 mat1
Eigen::Matrix<double, 3, Dynamic> mat2;     //  3x? 的 double 类型的矩阵 mat2
Eigen::Matrix<float, Dynamic, 4> mat3;      //  ?x4 的 float 类型的矩阵 mat3
Eigen::Matrix<long, Dynamic, Dynamic> mat4; //  ?x? 的 long 类型的矩阵 mat4

Matrix3d E;    //3*3的矩阵E

MatrixXd  E：表示任意大小的元素类型为double的矩阵变量，其大小只有在运行时被赋值之后才能知道。

matrix.block(i,j,p,q)：提取矩阵块大小为(p,q),起始于(i,j);

Eigen::Vector3d euler_t = rot.eulerAngles(2, 1, 0);//代表xyz顺序，euler_t(0)放的是Z的值;

Eigen::AngleAxisd Y(euler(2), Eigen::Vector3d::UnitZ());//代表z的位置（第三个），同上

矩阵输入：Matrix<int, 3, 4, ColMajor> Acolmajor;

输入：Acolmajor << 8, 2, 2, 9,
             9, 1, 4, 4,
             3, 5, 4, 5;

输出：  8, 2, 2, 9,
             9, 1, 4, 4,
             3, 5, 4, 5;（不受行列影响）

但是存储的结果不一样：

for (int i = 0; i < Acolmajor.size(); i++)
  cout << *(Acolmajor.data() + i) << "  ";

结果为：8  9  3  2  1  5  2  4  4  9  4  5

列的话就是：8  2  2  9  9  1  4  4  3  5  4  5  

RowMajor，那么矩阵或数组就是按列存储。如果设置成ColMajor, 就是按行存储的。 默认是按列存储

2.库各种形式表达式

旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1）:Eigen::AngleAxisd
四元数（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d

3.旋转向量赋值

 //对旋转向量（轴角）赋值的三大种方法

    //1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
    AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
    cout << "Rotation_vector1" << endl << V1.matrix() << endl;

    //2.使用旋转矩阵转旋转向量的方式

    //2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
    AngleAxisd V2;
    V2.fromRotationMatrix(t_R);
    cout << "Rotation_vector2" << endl << V2.matrix() << endl;

    //2.2 直接使用旋转矩阵来对旋转向量赋值
    AngleAxisd V3;
    V3 = t_R;
    cout << "Rotation_vector3" << endl << V3.matrix() << endl;

    //2.3 使用旋转矩阵来对旋转向量进行初始化
    AngleAxisd V4(t_R);
    cout << "Rotation_vector4" << endl << V4.matrix() << endl;

    //3. 使用四元数来对旋转向量进行赋值

    //3.1 直接使用四元数来对旋转向量赋值
    AngleAxisd V5;
    V5 = t_Q;
    cout << "Rotation_vector5" << endl << V5.matrix() << endl;

    //3.2 使用四元数来对旋转向量进行初始化
    AngleAxisd V6(t_Q);
    cout << "Rotation_vector6" << endl << V6.matrix() << endl;

4.四元数赋值

  //1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
    Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
    //第一种输出四元数的方式
    cout << "Quaternion1" << endl << Q1.coeffs() << endl;

    //第二种输出四元数的方式
    cout << Q1.x() << endl << endl;
    cout << Q1.y() << endl << endl;
    cout << Q1.z() << endl << endl;
    cout << Q1.w() << endl << endl;

    //2. 使用旋转矩阵转四元數的方式

    //2.1 直接使用旋转矩阵来对旋转向量赋值
    Quaterniond Q2;
    Q2 = t_R;
    cout << "Quaternion2" << endl << Q2.coeffs() << endl;

    //2.2 使用旋转矩阵来对四元數进行初始化
    Quaterniond Q3(t_R);
    cout << "Quaternion3" << endl << Q3.coeffs() << endl;

    //3. 使用旋转向量对四元数来进行赋值

    //3.1 直接使用旋转向量对四元数来赋值
    Quaterniond Q4;
    Q4 = t_V;
    cout << "Quaternion4" << endl << Q4.coeffs() << endl;

    //3.2 使用旋转向量来对四元数进行初始化
    Quaterniond Q5(t_V);
    cout << "Quaternion5" << endl << Q5.coeffs() << endl;

5.对旋转矩阵赋值

//对旋转矩阵赋值的三大种方法

    //1.使用旋转矩阵的函数来初始化旋转矩阵
    Matrix3d R1=Matrix3d::Identity();
    cout << "Rotation_matrix1" << endl << R1 << endl;

    //2. 使用旋转向量转旋转矩阵来对旋转矩阵赋值

    //2.1 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R2;
    R2 = t_V.matrix();
    cout << "Rotation_matrix2" << endl << R2 << endl;

    //2.2 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R3;
    R3 = t_V.toRotationMatrix();
    cout << "Rotation_matrix3" << endl << R3 << endl;

    //3. 使用四元数转旋转矩阵来对旋转矩阵赋值

    //3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R4;
    R4 = t_Q.matrix();
    cout << "Rotation_matrix4" << endl << R4 << endl;

    //3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R5;
    R5 = t_Q.toRotationMatrix();
    cout << "Rotation_matrix5" << endl << R5 << endl;
 

6.SVD分解：

Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);

A = U * S * VT

JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);

MatrixXd U = svd.matrixU();

MatrixXd V = svd.matrixV();

MatrixXd S = U.transpose()*E*V;

MatrixXd R = U*V.transpose();

Vector3d t  = P1 - R*P2; //p1和p2之间的位移

7.根据E求R，t

SVD分解E

AngleAxisd rotZ(M_PI/2,Vector3d(0,0,1));
AngleAxisd rotZ_(-M_PI/2,Vector3d(0,0,1));
t_wedge1 = U * rotZ * S * U.transpose();
t_wedge2 = U * rotZ_ * S * U.transpose();
R1 = U * rotZ * S * V.transpose();
R2 = U * rotZ_ * S * V.transpose();

8.orb特征提取

Ptr< Feature2D > detector = ORB::create();
vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints; 
        Mat descriptor;
        detector->detectAndCompute( image, Mat(), keypoints, descriptor );
        descriptors.push_back( descriptor );
    }

//读取图像
Mat img_1 = imread("../Images/L10.jpg", CV_LOAD_IMAGE_COLOR);
Mat img_2 = imread("../Images/R10.jpg", CV_LOAD_IMAGE_COLOR);
assert(img_1.data != nullptr && img_2.data != nullptr);

//初始化
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create();
Ptr<FeatureDetector> descriptor = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

//第一步，检测Oriented FAST 角点位置
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);

//第二部，根据角点位置计算BRIEF描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<doub``le>>(t2 - t1);
cout << "extract ORB cost = " << time_used.count() << "seconds." << endl;

Mat outimg1;
drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
imshow("ORB features", outimg1);