第3讲 三维空间刚体运动

3.1 旋转矩阵

3.1.1 点和向量、坐标系

位姿=位置+姿态

位置是指相机在空间中的哪个地方

姿态则是指相机的朝向

内积可以描述向量间的投影关系

外积表示向量的旋转

<br>

3.1.2 坐标系间的欧式变换

旋转矩阵R：行列式为1的正交矩阵

旋转矩阵可以描述相机的旋转

用一个旋转矩阵R和一个平移向量t完整地描述一个欧式空间的坐标变换关系

<br>

3.1.3 变换矩阵和齐次坐标

多次坐标系的变换用旋转矩阵表示则过于复杂，引入变换矩阵T

以下就是齐次坐标：

<br><br>

3.2 实践：Eigen

eigen库封装了矩阵和向量的运算

1. 安装eigen

sudo apt install libeigen3-dev

2. 查看是否安装成功和安装路径

sudo updatedb  //更新文件
locate eigen3  //定位查找文件

<br>

代码详解：

声明部分

Eigen::Matrix<float,2,3> matrix_23;   //2*3的矩阵

Eigen::Vector3d v_3d;              //3*1的向量  double类型
Eigen::Matrix<float,3,1> vd_3d;

Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();  //3*3矩阵；初始化为0  double类型

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;  //动态大小的矩阵
Eigen::MatrixXd matrix_x;

初始化部分

matrix_23 << 1,2,3,4,5,6;   //2*3

v_3d << 3,2,1;              //3*1
vd_3d << 4,5,6;

Eigen::Matrix<double,2,1> res = matrix_23.cast<double>() * v_3d;  //显示类型转换
Eigen::Matrix<float,2,1> res2 = matrix_23 * v_3d;

一些四则运算

matrix_33 = Eigen::Matrix3d::Random();   //随机数矩阵

cout << matrix_33.transpose() << endl;  //转置

cout << matrix_33.sum() << endl;  //求和

cout << matrix_33.trace() << endl;  //迹

cout << 10*matrix_33 << endl;  //数乘

cout << matrix_33.inverse() << endl;  //逆

cout << matrix_33.determinant() << endl;  //行列式

特征值和特征向量

Eigen::SelfAdjointEigenSolver<Eigen::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

Eigen::Matrix<double, 50, 50> matrix_NN;
matrix_NN = Eigen::MatirxXd::Random(50, 50);

Eigen::Matrix<double, 50, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(50, 1);

//1. 直接求逆
Eigen::Matrix<double, 50, 1> x = matrix_NN.inverse() * v_Nd;

//2. QR分解求
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);

<br><br>

3.3 旋转向量和欧拉角

3.3.1 旋转向量

旋转矩阵R用9个量表示3自由度的旋转，太过冗余

我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。称为旋转向量（Axis-Angle）

3.3.2 欧拉角

欧拉角也可以表示旋转

ZYX：偏航角yaw，俯仰角pitch，滚转角roll

欧拉角存在一个问题：万向锁问题，即在俯仰角为90°时，第一次旋转和第三次旋转为同一个轴，使得系统丢掉一个自由度

<br><br>

3.4 四元数

3.4.1 四元数的定义

• 旋转矩阵R使用9个量描述3自由度的旋转，具有冗余性
• 欧拉角和旋转向量是紧凑的，但具有奇异性

我们能用单位四元数表示三维空间的任意一个旋转。

虚四元数能对应一个空间点

可以用一个标量+一个向量表示四元数：

<br>

3.4.3 用四元数表示旋转

<br><br>

3.6 实践：Eigen几何模块

角轴、旋转矩阵、欧拉角、四元数都可以表示旋转；且可以相互转换

Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();       //初始化为单位矩阵

Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0,0,1));  //绕Z轴旋转45度
cout .precision(3);   //输出保留三位小数

//角轴->旋转矩阵
rotation_matrix = rotation_vector.matrix()
rotation_matrix = rotation_vector.toRotationMatrix();  //两者效果一样
cout << "rotation matrix = \n" << rotation_matrix << endl;

Eigen::Vector3d v(1,0,0);  //3*1
Eigen::Vector3d v_rotated = rotation_vector * v;   //用角轴进行坐标变换
v_rotated = rotation_matrix * v;                   //用旋转矩阵进行坐标变换

//旋转矩阵->欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
cout << "yaw pitch roll = " euler_angles.transpose() << endl;   //偏航角、俯仰角、滚转角

//变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::identity();
T.rotate(rotation_vector);  //旋转
T.pretranslate(Eigen::vector3d(1,3,4)); //平移
cout << "Transform matrix = \n" << T.matrix() << endl;

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

//四元数
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);  //角轴->四元数
q = Eigen::Quaterniond(rotation_matrix);  //旋转矩阵->四元数
cout << q.coeffs() << endl;

//使用四元数进行旋转
v_rotated = q * v;

<br><br>

3.7 可视化演示

这一小节有几个小问题

2. 跑3.7.2的代码编译可能的报错情况

terminate called after throwing an instance of std::runtime_error what(): Pangolin X11: Unable to retrieve framebuffer options

我是报了上面这个错误
说明下情况：我是在虚拟机上的Ubuntu16.04

3. 对应报错解决方法

步骤流程：

1. 安装个东西

sudo apt-get install mesa-utils

2. 找到对应文件并打开
   
3. 注释掉两行内容

staticint visual_attribs[] = 
{ 
    GLX_X_RENDERABLE , True, 
    GLX_DRAWABLE_TYPE , GLX_WINDOW_BIT, 
    GLX_RENDER_TYPE , GLX_RGBA_BIT, 
    GLX_X_VISUAL_TYPE , GLX_TRUE_COLOR, 
    GLX_RED_SIZE , 8, 
    GLX_GREEN_SIZE, 8, 
    GLX_BLUE_SIZE, 8, 
    GLX_ALPHA_SIZE, 8, 
    GLX_DEPTH_SIZE, 24, 
    GLX_STENCIL_SIZE, 8, 
    GLX_DOUBLEBUFFER , glx_doublebuffer ? True : False, 
    //注释这一行GLX_SAMPLE_BUFFERS , glx_sample_buffers, 
    //注释这一行 GLX_SAMPLES , glx_sample_buffers > 0 ? glx_samples : 0, 
    None 
};

4. 重新遍历Pangolin和安装

cd [path-to-pangolin]
mkdir build
cd build
cmake ..
make 
sudo make install 

5. 重新编译3.7.2的demo，成功实现！