SLAM高翔十四讲（三）第三讲 三维空间刚体运动

一、三维空间刚体运动

1.1 定义

1. 刚体运动：坐标系的运动由旋转+平移组成
2. 欧式变换：旋转+平移 ，即a1=R12*a2+t12，R12表示2到1，t12表示1到2

1.2 描述方式

1. 旋转矩阵：SO(n)=R，R正交且行列式为1
2. 变换矩阵：SE(n)
3. 旋转向量/轴角/角轴：旋转轴+旋转角（罗德里格斯公式：旋转向量->旋转矩阵）
4. 欧拉角：使用3个分离的转角（问题：万象锁）
5. 四元数：一个实部+三个虚部

1.3 旋转矩阵-旋转向量-四元数-欧拉角之间的转换

二、Eigen的基本使用（矩阵、向量）

2.1 介绍：

Eigen是一个C++开源线性代数库，提供快速线性运算和解方程等功能。它是一个纯头文件搭建的库，因此无需链接库文件，即target_link_libraries()。

2.2 CMakeLists.text文件：

cmake_minimum_required(VERSION 2.8)
project(useEigen)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)

2.3 演示：（声明、赋值、取值、运算、求解方程）

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 核心部分
#include <eigen3/Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <eigen3/Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/
int main(int argc, char **argv) {
    // 1. Eigen的声明
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Matrix<float, 2, 3> matrix_23;

    // 1.1 声明一个向量Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 1）Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
    Vector3d v_3d;
    // 2）
    Matrix<float, 3, 1> vd_3d;

    // 1.2 Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零
    // 1.3 
    // 1）动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    // 2）简化
    MatrixXd matrix_x;
    

    // 2. 对Eigen阵的操作
    // 2.1 初始化
    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

    // 2.2 访问矩阵中的元素
    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;
    }

    // 2.3 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;

    // 数据类型应该相同，这里采用显式转换xxx.cast<类型>()
    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;


    // 3. 矩阵运算
    // 3.1 四则运算就不演示了，直接用+-*/即可
    // 3.2 其他运算
    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;    // 行列式

    // 3.3 特征值
    //     实对称矩阵可以保证对角化成功
    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; //特征向量

    // 4. 解方程
    // 我们求解 A * x = b 这个方程
    // N的大小在前边的宏里定义，它由随机数生成

    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> A
        = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    A = A * A.transpose();  // 保证半正定
    Matrix<double, MATRIX_SIZE, 1> b = MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock(); // 计时
    // 4.1 直接求逆，运算量大
    Matrix<double, MATRIX_SIZE, 1> x = A.inverse() * b;
    cout << "time of normal inverse is "
        << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

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

    // 4.3 对于正定矩阵，还可以用cholesky分解来解方程
    time_stt = clock();
    x = A.ldlt().solve(b);
    cout << "time of ldlt decomposition is "
        << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;
    return 0;
}

2.4 结果展示：

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.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.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.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.0242899
 0.992154
  1.80558
Eigen vectors = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 0.076ms
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.048ms
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.02ms
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

三、Eigen演示（旋转矩阵-旋转向量-四元数-欧拉角）转换

3.1 CMakeLists.text文件：

cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable(eigenGeometry eigenGeometry.cpp)

3.2 代码展示：

#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
// 本程序演示了 Eigen 几何模块的使用方法
// Eigen/Geometry 模块提供了各种旋转和平移的表示
int main(int argc, char **argv) {

  // 1.旋转矩阵和旋转向量
  // 旋转矩阵：Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量：AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度 + 平移
  cout.precision(3); //保留三位小数
  cout << "旋转矩阵 =\n" << rotation_matrix << endl;
  
  // 1.1 旋转向量->旋转矩阵
  // 1）旋转向量.matrix()
  cout << "旋转向量->旋转矩阵1 =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 2）旋转向量.toRotationMatrix()：直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  cout << "旋转向量->旋转矩阵2 =\n" << rotation_matrix << endl;
  
  // 1.2 坐标变换
  // 1）利用旋转向量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;
  // 2）利用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "坐标变换(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 2 欧拉角: 旋转矩阵->欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
  cout << "欧拉角yaw pitch roll（ZYX） = " << euler_angles.transpose() << endl;

  // 3 （欧氏）变换矩阵：旋转+平移
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                            // 旋转
  T.pretranslate(Vector3d(1, 3, 4));                    // 平移
  cout << "变换矩阵 = \n" << T.matrix() << endl;

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

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

  // 4 四元数：旋转向量/旋转矩阵->四元数。反之亦然
  // 输出顺序：coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "旋转向量->四元数 = " << q.coeffs().transpose()<< endl;
  q = Quaterniond(rotation_matrix);
  cout << "旋转矩阵->四元数 = " << q.coeffs().transpose() << endl;
  
  // 4.1 坐标变换
  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.3 结果：

旋转矩阵 =
1 0 0
0 1 0
0 0 1
旋转向量->旋转矩阵1 =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
旋转向量->旋转矩阵2 =
 0.707 -0.707      0
 0.707  0.707      0
     0      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（ZYX） = 0.785    -0     0
变换矩阵 = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
坐标变换v tranformed = 1.71 3.71    4
旋转向量->四元数 =     0     0 0.383 0.924
旋转矩阵->四元数 =     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

四、Pangolin可视化演示

4.1 CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )

set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

# 添加Pangolin依赖
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )

add_executable( visualizeGeometry visualizeGeometry.cpp )
target_link_libraries( visualizeGeometry ${Pangolin_LIBRARIES} )

4.2 代码演示

#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));

  // ui
  pangolin::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 axis
    glLineWidth(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();
  }
}

4.3 结果