【一】欧式空间、欧式变换

1.基

首先，基的数学定义如下：

先来看看线性相关：一个向量组里，只要有一个向量可以由其它向量线性表示，我们就称这个向量组线性相关。比如：

• {a1, a2, a3} 线性相关，因为 a1+a3 = a2;
• {a4, a5},{a4, a7}都线性相关

那么，线性无关：如果向量组里的任意一个向量都不能由其它向量线性表示，大家都是独一无二的存在。比如在二维平面上{a1, a3}线性无关，因为不共线

为了方便计算或者表示空间中的向量，我们一般选择正交基(例如：直角坐标系)

2.欧拉角：

欧拉角有三种：

• 俯仰：Pitch（绕y轴旋转）
• 偏航：Yaw（绕z轴旋转）
• 翻滚：Roll（绕x轴旋转）

常用的一种：偏航-俯仰-翻滚（yaw-pitch-roll）等价于ZYX轴旋转，向量表示为[r, p, y]

欧拉角的一个重大缺陷是会碰到万向锁问题：在俯仰角为±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失一个自由度。（万向锁问题） 

3.旋转的表示方式： 

• 旋转矩阵：绕坐标轴依次旋转
• 旋转向量：绕任意轴一次直接旋转一个角度得到

3.1.旋转矩阵

首先，旋转矩阵是一个3×3的正交矩阵，即R^TR=E，但是9个未知数，只有6个约束（对角约束相同），所以有3个自由度，当然可以类推，m阶矩阵共有m(m-1)/2个自由变量 。

  我们先绕某一个轴进行旋转，例如：x轴，那么分为两种情况：顺时针旋转和逆时针旋转，我们采用高中还是初中学的力的分解来求解旋转矩阵，将y轴和z轴视为一种力，然后将其正交分解在新的坐标轴y'和z'  ，然后再进行力的合成可以得到如下的结果： 

• 逆时针旋转  （旋转前：z,y，旋转后：z',y'）                       

• 顺时针旋转（旋转前：z',y'，旋转后：z,y） 

3.2.姿态求解

• 旋转z轴：旋转前坐标：（x0, y0, z0）, 旋转后坐标：（x1, y1, z1）

• 旋转y轴：旋转前坐标：（x1, y1, z1）, 旋转后坐标：（x2, y2, z2）

• 旋转x轴：旋转前坐标：（x2, y2, z2）, 旋转后坐标：（x3, y3, z3）

4.旋转向量

旋转矩阵表示的局限性：

1. SO(3)旋转矩阵有9个量，但自由度为3，因此表示方法有冗余，同理可得，变换矩阵16个量，但是自由度为6。
2. 旋转矩阵带有约束，它必须为正交矩阵，且行列式为1，优化时困难。

在三维中，旋转可以通过单一的旋转角 θ和所围绕的单位向量方向 v=(x,y,z)来定义。对于旋转向量r，其方向与旋转轴一致，长度等于旋转角θ

旋转向量到旋转矩阵：

旋转矩阵到旋转向量：

#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>

// 1. Eigen 实现

    // 旋转向量转旋转矩阵
    Eigen::Vector3d rvec (r_x, r_y, r_z);     
    double n_norm = rvec.norm();
    Eigen::AngleAxisd rotation_vector (n_norm, rvec/n_norm);
    Eigen::Matrix3d rotm;
    rotm = rotation_vector.toRotationMatrix();

    // 旋转矩阵转旋转向量
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
    Eigen::AngleAxisd rotation_vector;
    rotation_vector.fromRotationMatrix(rotation_matrix);
 
// 2. OpenCV 实现

    // 旋转向量转旋转矩阵
    cv::Mat rvec = (cv::Mat_<double>(3,1) << r_x, r_y, r_z);
    cv::Mat rotm;
    cv::Rodrigues(rvec, rotm);

    // 旋转矩阵转旋转向量
    cv::Mat rvec;
    cv::Mat rotm = (cv::Mat_<double>(3,3) << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22);
    cv::Rodrigues(rotm, rvec);

• 证明：罗德里格斯（Rodrigues）旋转方程推导

• 罗德里格斯公式 理解、推导

参考：

1. 机器人学之3D欧式变换理论与实践
2. 三维空间的刚体运动
3. 旋转矩阵
4. 三维坐标系旋转——旋转矩阵到旋转角之间的换算
5. 三维旋转矩阵推导
6. 旋转矩阵（Rotation Matrix）的推导及其应用