【Eigen】【Eigen实践】【Eigen的使用学习记录】

【Eigen】【Eigen实践】【Eigen的使用学习记录】

  • 0 前言
  • 1 Eigen使用
    • 1.1 头文件的使用
    • 1.2 定义和初始化
      • 1.2.1 定义n*m矩阵
      • 1.2.2 定义n*1向量
      • 1.2.3 定义n*n矩阵|初始化为0
      • 1.2.4 定义动态矩阵
    • 1.3 对Eigen操作
      • 1.3.1 输入数据
      • 1.3.2 矩阵整个输出
      • 1.3.3 用()访问矩阵中的元素
      • 1.3.4 矩阵和向量相乘|类型转换
      • 1.3.5 得到矩阵的指定块元素
    • 1.4 矩阵运算（随机数、转置、各元素和、迹、数乘、逆、行列式、特征值、特特征向量、叉乘、点乘、求模、归一化、“开根号”）
    • 1.5 解方程
      • 1.5.1 直接求逆
      • 1.5.2 QR分解
      • 1.5.3 cholesky分解
      • 1.5.4 SVD分解
    • 1.6 旋转和平移的表示、转换和使用
      • 1.6.0 数据类型汇总
      • 1.6.1 旋转矩阵
        • 1.6.1.1 初始化
        • 1.6.1.2 坐标变换
        • 1.6.2.3 旋转向量->旋转矩阵
      • 1.6.2 旋转向量
        • 1.6.2.1 初始化
        • 1.6.2.3 坐标变换
      • 1.6.3 欧拉角
        • 1.6.3.1 初始化
        • 1.6.3.2 旋转矩阵->欧拉角
      • 1.6.4 欧式变换矩阵se
        • 1.6.4.1 初始化
        • 1.6.4.2 旋转向量->se.T|se->旋转矩阵
        • 1.6.4.3 四元数->se.T
        • 1.6.4.4 坐标变换
      • 1.6.5 四元数
        • 1.6.5.1 初始化
        • 1.6.5.2 旋转向量->四元数
        • 1.6.5.3 旋转矩阵->四元数
        • 1.6.5.4 坐标变换
        • 1.6.5.5 取虚部`q.vec()`
  • 2 slam14的cap3的useigen
    • 2.1 eigenMatrix.cpp
    • 2.2 CMakeLists.txt
    • 2.3 输出
  • 3 slam14的cap3的useigen
    • 3.1 useGeometry.cpp
    • 3.2 CMakeLists.txt
    • 3.3 输出
  • 4 实际的坐标转换例子
    • 4.1 coordinateTransform.cpp
    • 4.2 CMakeLists.txt
    • 4.3 输出
  • 5 显示运动轨迹
    • 5.1 plotTrajectory.cpp
    • 5.2 CMakeLists.txt
    • 5.3 输出
  • 6 显示相机的位姿
    • 6.1 visualizeGeometry.cpp
    • 6.2 CMakeLists.txt
    • 6.3 输出
  • 7 使用QR和Cholesly分解求解示例
    • 7.0 说明：自创建100大小的动态矩阵，并使用QR和Cholesly分解求解
    • 7.1 qiujie.cpp
    • 7.2 CMakeLists.txt

0 前言

1 Eigen使用

1.1 头文件的使用

  1. 核心
// Eigen 核心部分
#include <Eigen/Core>
  1. 稠密矩阵的代数运算（逆，特征值等）
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
  1. 提供各种旋转和平移的表示
#include<Eigen/Geometry>

1.2 定义和初始化

  • Eigen固定大小矩阵最大支持到50

1.2.1 定义n*m矩阵

    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列// 声明一个2*3的float矩阵
Matrix<float, 2, 3> matrix_23;

1.2.2 定义n*1向量

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量Vector3d v_3d;// 这是一样的Matrix<float, 3, 1> vd_3d;

1.2.3 定义n*n矩阵|初始化为0

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零

1.2.4 定义动态矩阵

    // 如果不确定矩阵大小，可以使用动态大小的矩阵Matrix<double, Dynamic, Dynamic> matrix_dynamic;// 更简单的MatrixXd matrix_x;// 这种类型还有很多，我们不一一列举

1.3 对Eigen操作

1.3.1 输入数据

    // 输入数据（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;

1.3.2 矩阵整个输出

    // 输出cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

1.3.3 用()访问矩阵中的元素

    cout << "print matrix 2x3: " << endl;for (int i = 0; i < 2; i++) {for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";cout << endl;}

1.3.4 矩阵和向量相乘|类型转换

    v_3d << 3, 2, 1;vd_3d << 4, 5, 6;// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 应该显式转换Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;//隐式转换Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

1.3.5 得到矩阵的指定块元素

    int n_state = all_frame_count * 3 + 3 + 1;MatrixXd A{n_state, n_state};A.setZero();VectorXd b{n_state};b.setZero();MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); b.segment<6>(i * 3) += r_b.head<6>();A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();b.tail<4>() += r_b.tail<4>();
  1. 取左上矩阵块赋予自由指定矩阵块
        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
  1. 取顶部向量块赋予自由指定向量块
        b.segment<6>(i * 3) += r_b.head<6>();
  1. 取右下矩阵块赋予右下矩阵块
        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
  1. 取底部向量块赋予底部向量块
        b.tail<4>() += r_b.tail<4>();

1.4 矩阵运算（随机数、转置、各元素和、迹、数乘、逆、行列式、特征值、特特征向量、叉乘、点乘、求模、归一化、“开根号”）

    // 一些矩阵运算// 四则运算就不演示了，直接用+-*/即可。matrix_33 = Matrix3d::Random();      // 随机数矩阵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 << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆cout << "det: " << matrix_33.determinant() << endl;    // 行列式

    // 特征值// 实对称矩阵可以保证对角化成功SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    matrix1_33 = Matrix3d::Random();      // 随机数矩阵matrix2_33 = Matrix3d::Random();      // 随机数矩阵matrix3_33 = matrix1_33.cross(matrix2_33);//叉乘matrix4_33 = matrix1_33.dot(matrix2_33);//叉乘double mo = matrix3_33.norm();//求模

Eigen::Vector3d ljm_norm;
ljm_norm.normalize();//向量的归一化

//开根号Eigen::VectorXd S ;Eigen::VectorXd S_sqrt = S.cwiseSqrt(); // 这个求得就是 S^(1/2)，不过这里是向量还不是矩阵

1.5 解方程

初始化求解方程：

    // 解方程// 我们求解 matrix_NN * x = v_Nd 这个方程// N的大小在前边的宏里定义，它由随机数生成// 直接求逆自然是最直接的，但是求逆运算量大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);

1.5.1 直接求逆

    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 = " << x.transpose() << endl;

1.5.2 QR分解

    // 通常用矩阵分解来求，例如QR分解，速度会快很多time_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);cout << "time of Qr decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;

1.5.3 cholesky分解

  • cholesky分解需要是正定矩阵
    // 对于正定矩阵，还可以用cholesky分解来解方程time_stt = clock();x = matrix_NN.ldlt().solve(v_Nd);cout << "time of ldlt decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;

1.5.4 SVD分解

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();//初始化...Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3d U = svd.matrixU();//得到U矩阵Eigen::Matrix3d V = svd.matrixV();//得到V矩阵

1.6 旋转和平移的表示、转换和使用

1.6.0 数据类型汇总

  • 每种都有double 和float类型，把后面的d改为f就可以了
  1. 旋转矩阵（3*3）：Eigen::Matrix3d
  2. 旋转向量（3*1）：Eigen::AngleAxisd
  3. 欧拉角（3*1）：Eigen::Vector3d
  4. 四元数（4*1）：Eigen::Quaterniond
  5. 欧式变换矩阵（4*4）：Eigen::Isometry3d
  6. 仿射变换（4*4）：Eigen::Affine3d
  7. 射影变换（4*4）：Eigen::Projective3d

1.6.1 旋转矩阵

1.6.1.1 初始化

  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fMatrix3d rotation_matrix = Matrix3d::Identity();

1.6.1.2 坐标变换

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

输出：

(1,0,0) after rotation (by matrix) = 0.707 0.707     0

1.6.2.3 旋转向量->旋转矩阵

    cout.precision(3);cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵

输出：

rotation matrix =0.707 -0.707      00.707  0.707      00      0      1

    // 也可以直接赋值rotation_matrix = rotation_vector.toRotationMatrix();

同1

1.6.2 旋转向量

1.6.2.1 初始化

  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，//但运算可以当作矩阵（因为重载了运算符）//沿 Z 轴旋转 45 度AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));

1.6.2.3 坐标变换

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

输出：

(1,0,0) after rotation (by angle axis) = 0.707 0.707     0

1.6.3 欧拉角

1.6.3.1 初始化

1.6.3.2 旋转矩阵->欧拉角

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

输出：

yaw pitch roll = 0.785    -0     0

1.6.4 欧式变换矩阵se

1.6.4.1 初始化

    // 欧氏变换矩阵使用 Eigen::IsometryIsometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵

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

1.6.4.2 旋转向量->se.T|se->旋转矩阵

    T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)cout << "Transform matrix = \n" << T.matrix() << endl;

输出：

Transform matrix = 0.707 -0.707      0      10.707  0.707      0      30      0      1      40      0      0      1

1.6.4.3 四元数->se.T

    Isometry3d T1w(q1), T2w(q2);

1.6.4.4 坐标变换

    // 用变换矩阵进行坐标变换Vector3d v_transformed = T * v;                              // 相当于R*v+tcout << "v tranformed = " << v_transformed.transpose() << endl;

输出：

v tranformed = 1.71 3.71    4

1.6.5 四元数

1.6.5.1 初始化

  • 使用四元数进行坐标变换之前要进行归一化处理q.normalize()
    初始化是w在前
Quaterniond(qw, qx, qy, qz)

1.6.5.2 旋转向量->四元数

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

输出：

quaternion from rotation vector =     0     0 0.383 0.924

1.6.5.3 旋转矩阵->四元数

  • q = rotation_matrix;好像直接这样也可以，不加Quaterniond()，加了更加清晰
    // 也可以把旋转矩阵赋给它q = Quaterniond(rotation_matrix);cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;

输出：

quaternion from rotation matrix =     0     0 0.383 0.924

1.6.5.4 坐标变换

    // 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q * v; // 注意数学上是qvq^{-1}cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;// 用常规向量乘法表示，则应该如下计算cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

输出：

(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

1.6.5.5 取虚部q.vec()

      Eigen::Quaterniond corrected_delta_q ,Qi,Qj;Eigen::Matrix<double, 15, 1> residuals;residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();

2 slam14的cap3的useigen

2.1 eigenMatrix.cpp

#include <iostream>using namespace std;#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>using namespace Eigen;#define MATRIX_SIZE 50/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/int main(int argc, char **argv) {// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列// 声明一个2*3的float矩阵Matrix<float, 2, 3> matrix_23;// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量Vector3d v_3d;// 这是一样的Matrix<float, 3, 1> vd_3d;// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零// 如果不确定矩阵大小，可以使用动态大小的矩阵Matrix<double, Dynamic, Dynamic> matrix_dynamic;// 更简单的MatrixXd matrix_x;// 这种类型还有很多，我们不一一列举// 下面是对Eigen阵的操作// 输入数据（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 输出cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;// 用()访问矩阵中的元素cout << "print matrix 2x3: " << endl;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;// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 应该显式转换Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;// 同样你不能搞错矩阵的维度// 试着取消下面的注释，看看Eigen会报什么错// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;// 一些矩阵运算// 四则运算就不演示了，直接用+-*/即可。matrix_33 = Matrix3d::Random();      // 随机数矩阵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 << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆cout << "det: " << matrix_33.determinant() << endl;    // 行列式// 特征值// 实对称矩阵可以保证对角化成功SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;// 解方程// 我们求解 matrix_NN * x = v_Nd 这个方程// N的大小在前边的宏里定义，它由随机数生成// 直接求逆自然是最直接的，但是求逆运算量大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 << "time of normal inverse is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 通常用矩阵分解来求，例如QR分解，速度会快很多time_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);cout << "time of Qr decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 对于正定矩阵，还可以用cholesky分解来解方程time_stt = clock();x = matrix_NN.ldlt().solve(v_Nd);cout << "time of ldlt decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;return 0;
}

2.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(useEigen)set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 11)include_directories("/usr/local/include/eigen3")
add_executable(eigenMatrix src/eigenMatrix.cpp)

2.3 输出

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234  0.5661980.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.364595.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.83571.00605 -0.555135  -1.41603-1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.02428990.9921541.80558
Eigen vectors = 
-0.549013 -0.735943  0.3961980.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 0.073ms
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 Qr decomposition is 0.043ms
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 ldlt decomposition is 0.018ms
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进程已结束,退出代码0

3 slam14的cap3的useigen

3.1 useGeometry.cpp

#include <iostream>
#include <cmath>using namespace std;#include <Eigen/Core>
#include <Eigen/Geometry>using namespace Eigen;// 本程序演示了 Eigen 几何模块的使用方法int main(int argc, char **argv) {// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fMatrix3d rotation_matrix = Matrix3d::Identity();// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度cout.precision(3);cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵// 也可以直接赋值rotation_matrix = rotation_vector.toRotationMatrix();//cout << "rotation matrix =\n" << rotation_matrix << endl;// 用 AngleAxis 可以进行坐标变换Vector3d v(1, 0, 0);Vector3d v_rotated = rotation_vector * v;cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;// 或者用旋转矩阵v_rotated = rotation_matrix * v;cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;// 欧拉角: 可以将旋转矩阵直接转换成欧拉角Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序cout << "yaw pitch roll = " << euler_angles.transpose() << endl;// 欧氏变换矩阵使用 Eigen::IsometryIsometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)cout << "Transform matrix = \n" << T.matrix() << endl;// 用变换矩阵进行坐标变换Vector3d v_transformed = T * v;                              // 相当于R*v+tcout << "v tranformed = " << v_transformed.transpose() << endl;// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略// 四元数// 可以直接把AngleAxis赋值给四元数，反之亦然Quaterniond q = Quaterniond(rotation_vector);cout << "quaternion from rotation vector = " << q.coeffs().transpose()<< endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部// 也可以把旋转矩阵赋给它q = Quaterniond(rotation_matrix);cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;// 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q * v; // 注意数学上是qvq^{-1}cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;// 用常规向量乘法表示，则应该如下计算cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;return 0;
}

3.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(geometry)include_directories("/usr/include/eigen3")add_executable(eigenFeometry src/useGeometry.cpp)

3.3 输出

rotation matrix =0.707 -0.707      00.707  0.707      00      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix = 0.707 -0.707      0      10.707  0.707      0      30      0      1      40      0      0      1
v tranformed = 1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707     0
should be equal to 0.707 0.707     0     0进程已结束,退出代码0

4 实际的坐标转换例子

4.1 coordinateTransform.cpp

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;
using namespace Eigen;int main(int argc, char ** argv)
{Quaterniond q1(0.35, 0.2,0.3,0.1), q2(-0.5,0.4,-0.1,0.2);q1.normalize();q2.normalize();Vector3d t1(0.3,0.1,0.1),t2(-0.1,0.5,0.3);Vector3d p1(0.5,0,0.2);Isometry3d T1w(q1), T2w(q2);T1w.pretranslate(t1);T2w.pretranslate(t2);Vector3d p2 = T2w*T1w.inverse()*p1;cout << endl << p2.transpose() << endl;return 0;
}

4.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(coordinateTransform)include_directories("/usr/include/eigen3")add_executable(coordinatetransform src/coordinateTransform.cpp)

4.3 输出

-0.0309731    0.73499   0.296108

5 显示运动轨迹

  • 该历程需要一个trajectory.txt的位姿数据，自取：链接: https://pan.baidu.com/s/1ITzeIl_DsG2ckMU-eSEnhg 提取码: m35v

5.1 plotTrajectory.cpp

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>// 本例演示了如何画出一个预先存储的轨迹using namespace std;
using namespace Eigen;// path to trajectory file
string trajectory_file = "../src/trajectory.txt";void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);int main(int argc, char **argv) {vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;ifstream fin(trajectory_file);if (!fin) {cout << "cannot find trajectory file at " << trajectory_file << endl;return 1;}while (!fin.eof()) {double time, tx, ty, tz, qx, qy, qz, qw;fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;Isometry3d Twr(Quaterniond(qw, qx, qy, qz));Twr.pretranslate(Vector3d(tx, ty, tz));poses.push_back(Twr);}cout << "read total " << poses.size() << " pose entries" << endl;// draw trajectory in pangolinDrawTrajectory(poses);return 0;
}/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {// create pangolin window and plot the trajectorypangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glLineWidth(2);for (size_t i = 0; i < poses.size(); i++) {// 画每个位姿的三个坐标轴Vector3d Ow = poses[i].translation();Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));glBegin(GL_LINES);glColor3f(1.0, 0.0, 0.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Xw[0], Xw[1], Xw[2]);glColor3f(0.0, 1.0, 0.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Yw[0], Yw[1], Yw[2]);glColor3f(0.0, 0.0, 1.0);glVertex3d(Ow[0], Ow[1], Ow[2]);glVertex3d(Zw[0], Zw[1], Zw[2]);glEnd();}// 画出连线for (size_t i = 0; i < poses.size(); i++) {glColor3f(0.0, 0.0, 0.0);glBegin(GL_LINES);auto p1 = poses[i], p2 = poses[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}
}

5.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(plotTrajectory)include_directories("/usr/include/eigen3")
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})add_executable(plottrajectory src/plotTrajectory.cpp)
target_link_libraries(plottrajectory ${Pangolin_LIBRARIES})

5.3 输出

在这里插入图片描述

6 显示相机的位姿

6.1 visualizeGeometry.cpp

#include <iostream>
#include <iomanip>using namespace std;#include <Eigen/Core>
#include <Eigen/Geometry>using namespace Eigen;#include <pangolin/pangolin.h>struct RotationMatrix {Matrix3d matrix = Matrix3d::Identity();
};ostream &operator<<(ostream &out, const RotationMatrix &r) {out.setf(ios::fixed);Matrix3d matrix = r.matrix;out << '=';out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"<< "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"<< "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";return out;
}istream &operator>>(istream &in, RotationMatrix &r) {return in;
}struct TranslationVector {Vector3d trans = Vector3d(0, 0, 0);
};ostream &operator<<(ostream &out, const TranslationVector &t) {out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";return out;
}istream &operator>>(istream &in, TranslationVector &t) {return in;
}struct QuaternionDraw {Quaterniond q;
};ostream &operator<<(ostream &out, const QuaternionDraw quat) {auto c = quat.q.coeffs();out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";return out;
}istream &operator>>(istream &in, const QuaternionDraw quat) {return in;
}int main(int argc, char **argv) {pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);glEnable(GL_DEPTH_TEST);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY));const int UI_WIDTH = 500;pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).SetHandler(new pangolin::Handler3D(s_cam));// uipangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));while (!pangolin::ShouldQuit()) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();Matrix<double, 4, 4> m = matrix;RotationMatrix R;for (int i = 0; i < 3; i++)for (int j = 0; j < 3; j++)R.matrix(i, j) = m(j, i);rotation_matrix = R;TranslationVector t;t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));t.trans = -R.matrix * t.trans;translation_vector = t;TranslationVector euler;euler.trans = R.matrix.eulerAngles(2, 1, 0);euler_angles = euler;QuaternionDraw quat;quat.q = Quaterniond(R.matrix);quaternion = quat;glColor3f(1.0, 1.0, 1.0);pangolin::glDrawColouredCube();// draw the original axisglLineWidth(3);glColor3f(0.8f, 0.f, 0.f);glBegin(GL_LINES);glVertex3f(0, 0, 0);glVertex3f(10, 0, 0);glColor3f(0.f, 0.8f, 0.f);glVertex3f(0, 0, 0);glVertex3f(0, 10, 0);glColor3f(0.2f, 0.2f, 1.f);glVertex3f(0, 0, 0);glVertex3f(0, 0, 10);glEnd();pangolin::FinishFrame();}
}

6.2 CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )set(CMAKE_CXX_STANDRAD 14)# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )# 添加Pangolin依赖
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )add_executable( visualizeGeometry src/visualizeGeometry.cpp )
target_link_libraries( visualizeGeometry ${Pangolin_LIBRARIES} )

6.3 输出

在这里插入图片描述

7 使用QR和Cholesly分解求解示例

7.0 说明：自创建100大小的动态矩阵，并使用QR和Cholesly分解求解

  • 这是salm14的第第二节课习题的2.5题

7.1 qiujie.cpp

#include <iostream>
using namespace std;#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>#define MATRIX_SIZE 100int main(int argc, char** argv)
{Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic> A;A = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);A = A.transpose() * A;//乔利斯基分解需要正定矩阵Eigen::Matrix<double, Eigen::Dynamic,1> B;B = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);Eigen::Matrix<double, Eigen::Dynamic,1> X;X = A.llt().solve(B);cout << "Cholesly's = " << X << endl;X = A.colPivHouseholderQr().solve(B);cout << "QR's = " << X << endl;return 0;}

7.2 CMakeLists.txt

CMAKE_MINIMUM_REQUIRED( VERSION 2.8)
PROJECT( qiujie )set( CMAKE_BUILD_TYPE "Release" )INCLUDE_DIRECTORIES( "/usr/include/eigen3" )ADD_EXECUTABLE( qiujie src/qiujie.cpp )

Published by

风君子

独自遨游何稽首 揭天掀地慰生平