slam框架

slam研究任务

  • 建图：估计环境
  • 定位：估计自身状态


    刚体运动描述：
  • 旋转矩阵——冗余
  • 旋转向量——不直观
  • 欧拉角——有奇异
  • 四元数——较好描述

Eigen库

Eigen 以矩阵为基本数据单元。它是一个模板类。它的前三个参数为：数据类型，行，列。
Eigen 通过typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix，例如

  • Vector3d 实质上是Eigen::Matrix<double, 3, 1>
  • Matrix3d 实质上是Eigen::Matrix<double, 3, 3>
    如果不确定矩阵大小，可以使用动态大小的矩阵

• 旋转矩阵（3 × 3）：Eigen::Matrix3d。
• 旋转向量（3 × 1）：Eigen::AngleAxisd。
• 欧拉角（3 × 1）：Eigen::Vector3d。
• 四元数（4 × 1）：Eigen::Quaterniond。
• 欧氏变换矩阵（4 × 4）：Eigen::Isometry3d。
• 仿射变换（4 × 4）：Eigen::Affine3d。
• 射影变换（4 × 4）：Eigen::Projective3d

定义

#include <iostream>
#include <ctime>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

using namespace std;
using namespace Eigen;

const int Matrix_size = 50;

int main()
{
    Matrix<float, 2, 3> matrix_23;
    Vector3d v_3d;
    Matrix3d matrix_33 = Matrix3d::Zero(); // init == 0
    Matrix<double, Dynamic, Dynamic> matrix_dynamic; // uncertainly
    MatrixXd matrix_x;

    // operator for matrix
    matrix_23 << 1,2,3,4,5,6;
    cout << matrix_23 << endl;
    // 用()访问矩阵中的元素
    for (int i = 0; i < 1; i++)
        for (int j = 0; j < 2; j++)
            cout << matrix_23(i, j) << endl;
    v_3d << 3, 2, 1;
    return 0;
}

矩阵操作

矩阵和向量相乘（实际上仍是矩阵和矩阵），但是在这里你不能混合两种不同类型的矩阵
注意类型一致和维数匹配

 matrix_23 << 1,2,3,4,5,6;
    cout << matrix_23 << endl;
    // 用()访问矩阵中的元素
    for (int i = 0; i < 1; i++)
        for (int j = 0; j < 2; j++)
            cout << matrix_23(i, j) << endl;

    v_3d << 3, 2, 1;
    // type and dim is important
    Matrix<double, 2, 1> res = matrix_23.cast<double>() * v_3d;

    matrix_33 = Matrix3d::Random();
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl; // T
    cout << matrix_33.sum() << endl;
    cout << matrix_33 * 10 << endl;
    cout << matrix_33.inverse() << endl; // inv
    cout << matrix_33.determinant() << endl; // det