旋转矩阵与坐标变换

旋转矩阵

旋转方向正负的定义

在笛卡尔坐标系中，当我们讨论旋转方向时，首先需要判断所讨论坐标系是“右手系”还是“左手系”，右手系的方向遵循“右手定则”，左手系则遵循“左手定则”，其含义为：用右手或左手握住坐标轴，大拇指朝向轴的正半部分，此时其余四指自然握紧形成的方向即为绕该轴旋转的“正方向”。

数学与物理中描述角度或角速度时，在没有特别说明是“左手系”的情况下，默认使用的是“右手系”。

正方向在公式中的体现为：sin或cos括号内数值的符号。而括号外的符号则由右手系或左手系决定。

旋转矩阵的定义：

右手系，旋转角值沿右手螺旋方向为正（即逆时针为正）

绕\(Z\)轴转\(\alpha\) \[ R_z = \begin{bmatrix} cos(\alpha) &-sin(\alpha) &0 \\ sin(\alpha) &cos(\alpha) &0 \\ 0 &0 &1 \end{bmatrix} \]

绕\(Y\)轴转\(\beta\) \[ R_y = \begin{bmatrix} cos(\beta) &0 &sin(\beta) \\ 0 &1 &0 \\ -sin(\beta) &0 &cos(\beta) \end{bmatrix} \]

绕\(X\)轴转\(\gamma\) \[ R_x = \begin{bmatrix} 1 &0 &0 \\ 0 &cos(\gamma) &-sin(\gamma) \\ 0 &sin(\gamma) &cos(\gamma) \end{bmatrix} \]

旋转矩阵的本质

旋转矩阵 \(R\) 的行代表在旋转后的坐标系下，原坐标系的单位向量\((\vec i,\vec j, \vec k)\)的坐标

旋转矩阵 \(R\) 的列代表在原坐标系下，旋转后的坐标系的单位向量\((\vec i,\vec j, \vec k)\)的坐标

以绕\(Z\)轴旋转\(\theta\)角为例 \[ R_z = \begin{bmatrix} cos(\alpha) &-sin(\alpha) &0 \\ sin(\alpha) &cos(\alpha) &0 \\ 0 &0 &1 \end{bmatrix} \]

欧拉角

欧拉角分为内旋（坐标系随动）和外旋（坐标系固定），外旋欧拉角也称为RPY（Roll Yaw Pitch）角，其参考的旋转系为Z轴向上的笛卡尔右手系。

在没有特殊说明的情况下，默认使用外旋欧拉角（也叫RPY角）。

内旋欧拉角与外旋欧拉角顺序相反时得出的旋转矩阵相同，例如\(R=Z_1X_2Y_3\)既可以表示Y->X->Z外旋，也可以表示Z->X->Y内旋，外旋时要左乘，内旋时要右乘。

只有在欧拉角为内旋，\(Pitch\)在中间，并且 \(Pitch=\cfrac{\pi}{2}\) 时出现万向锁。万向锁的具体表现形式是：任意 \((a,\frac{\pi}{2},b)\) 的旋转方式都可以用 \((a-b, \frac{\pi}{2},0)\) 来表示，即失去了一个旋转自由度。

内旋和外旋的矩阵转换关系：交换第一次和第三次旋转的顺序可得到相同的旋转矩阵。

坐标变换

平移向量

平移向量 \(\vec t\)（3x1）表示将原坐标系原点平移至目标坐标系原点的过程向量，遵守类比“左加右减”的规则。

例如有一个点 \(P(-2,0)\)，要求对坐标系进行平移后，点\(P\)在新坐标系中的坐标为\(P'(2,0)\)；如果平移的作用对象是点 \(P\)，那么很容易得出平移向量为 \(\vec t=(4,0)\)，但由于平移操作的实际作用对象是坐标系，因此需要取反，即平移向量应该是 \(\vec t=(-4,0)\)

坐标系间变换的标准描述方式

符号描述方式：

\(^AP\) ：点 \(P\) 在坐标系 \(A\) 中的坐标
\(^AP_{Borg}\) 或 \(^At_{Borg}\)或\(^At_B\)：坐标系 \(B\) 的原点在坐标系 \(A\) 中的坐标向量
\(^AR_B\)：坐标系 \(B\) 到坐标系 \(A\) 的旋转矩阵
\(^AT_B\)：坐标系 \(B\) 到坐标系 \(A\) 的齐次变换矩阵

基本变换公式：

\(^AP = {^AR_B}\space {^BP} + ^At_B\)
\(^AP = {^AT_B}^BP\)

坐标变换过程描述举例

有一个点\(P\)，基于（笛卡尔右手）坐标系\(A\)描述为\(^AP\)，此时坐标系\(A\)（按照外旋欧拉角/RPY角的方向）绕\(Z\)轴（沿笛卡尔右手系方向）旋转45度，并（使坐标系\(A\)的原点）沿着坐标系\(A\)的\(X、Y、Z\)方向分别移动\((1,2,3)\)，得到坐标系\(B\)，要求点\(P\)基于坐标系\(B\)的描述\(^BP\)

该描述使用上述旋转矩阵公式得到的\(R\)为\(^BR_A\)，即坐标系A到\(B\)的旋转矩阵
使用公式 \(^BP = {^BR_A}\space {^AP} + ^Bt_A\) 完成坐标变换

坐标系间变换推导实例

已知坐标系\(W\)与\(A\)的位姿变换关系，以及\(W\)与\(B\)的位姿变换关系，可求出\(A\)与\(B\)的位姿变换关系

例子：坐标系\(A\)的原点在\(A\)中描述 -> 在\(W\)中描述 -> 在\(B\)中描述

已知世界坐标系\(W\)，以及坐标系\(A\)和\(B\)，由上述关系可得两个转换式： \[ ^WP=^WR_A\space^AP+^Wt_{Aorg}\space \tag{1} \]

\[ ^WP=^WR_B\space^BP+^Wt_{Borg}\space \tag{2} \]

由 \((1)\) 可知坐标系 \(A\) 的原点在世界系 \(W\) 中的坐标为： \[ ^WP_{Aorg}=^WR_A\space^AP_{Aorg}+^Wt_{Aorg} \tag{3} \] 由 \((2)\) 可推得任意世界坐标系中的点\(^WP\)在坐标系 \(B\) 中的坐标为： \[ ^BP=(^WR_B)^{-1}(^WP-^Wt_{Borg}) \tag{4} \] 将 \((3)\) 中求出的\(A\)原点在世界系的坐标代入到 \((4)\) 中可得： \[ \begin{aligned} ^BP_{Aorg} &=(^WR_B)^{-1}(^WP_{Aorg}-^Wt_{Borg})\\\\ &=(^WR_B)^{-1}(^WR_A\space^AP_{Aorg}+^Wt_{Aorg}-^Wt_{Borg}) \\\\ &=((^WR_B)^{-1}\space^WR_A\space)^AP_{Aorg} + (^WR_B)^{-1}(^Wt_{Aorg}-^Wt_{Borg}) \end{aligned} \] 同时已知： \[ ^BP_{Aorg}=^BR_A\space^AP_{Aorg}+Bt_{Aorg} \tag{5} \] 因此易得从\(A\)系到\(B\)系的位姿变换关系： \[ ^BR_A = (^WR_B)^{-1}\space^WR_A \tag{6} \]

\[ ^Bt_A = (^WR_B)^{-1}(^Wt_{Aorg}-^Wt_{Borg}) \tag{7} \]

其中 \(^BR_A\) 表示从 \(A\) 系到 \(B\) 系的旋转变换（姿态），\(^Bt_A\)表示 \(A\) 系原点在 \(B\) 系中的坐标（位置）

cv::SolvePnP的输出

cv::SolvePnP用于计算解决2D-3D点对应的姿态估计问题，将物体坐标系的3D点映射到相机坐标系中。

函数输出数据有两个，分别是rvec和tvec：

rvec：世界坐标系到相机坐标系的旋转向量（也可以表述为世界坐标系相对于相机坐标系的旋转向量），该向量与旋转轴平行，向量长度为旋转角度（以弧度为单位），可通过罗德里格斯变换转为3x3旋转矩阵。
tvec：将世界坐标系原点平移至相机坐标系原点的过程向量，也可理解为世界坐标系原点在相机坐标系中的位置，或从相机坐标系原点指向世界坐标系原点的向量。

测试程序

分别使用 Eigen 与 OpenCV

一个点在同一个坐标系中的变换

#include <iomanip>
#include <iostream>
#include <opencv2/core.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

void calcR(float theta, cv::Mat& Rz)
{
    theta = theta / 180.0 * CV_PI;
    Rz = (cv::Mat_<float>(3,3) << cos(theta), sin(theta), 0, 
                                 -sin(theta), cos(theta), 0, 
                                 0,           0,          1);
}

int main()
{
    /**
     * @brief OpenCV实现
    */
    // 点P坐标
    cv::Mat P1 = (cv::Mat_<float>(3, 1) << 1.0, 2.0, 3.0);

    // 旋转矩阵计算函数
    auto calcR = [](float theta, cv::Mat& Rz){   
            theta = theta / 180.0 * CV_PI;
            Rz = (cv::Mat_<float>(3,3) << cos(theta), -sin(theta), 0, 
                                          sin(theta),  cos(theta), 0, 
                                           0,           0,         1);};

    // 点P向量绕坐标系（右手系）的Z轴旋转30度，求得旋转变换的矩阵
    cv::Mat R;
    calcR(90, R);

    // 旋转变换结果
    cv::Mat P2 = R * P1;
    std::cout << "P2 = " << P2 << std::endl;

    /**
     * @brief Eigen实现
    */
    // 点P坐标
    Eigen::Vector3d P1_(1.0, 2.0, 3.0); 

    // 点P向量绕坐标系（右手系）的Z轴旋转30度，求得旋转变换的矩阵
    Eigen::Matrix3d R_ = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix();

    // t，原坐标系原点在新坐标系中的位置（设为非0数以加入平移变换）
    Eigen::Vector3d t_(0, 0, 0);  

    // 坐标变换结果
    Eigen::Vector3d P2_ = R_ * P1_ + t_;
    std::cout << "P2_ = " << P2_ << std::endl;

    return 0;
}

一个点在不同坐标系之间的变换

#include <iomanip>
#include <iostream>
#include <opencv2/core.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

void calcR(float theta, cv::Mat& Rz)
{
    theta = theta / 180.0 * CV_PI;
    Rz = (cv::Mat_<float>(3,3) << cos(theta), sin(theta), 0, 
                                 -sin(theta), cos(theta), 0, 
                                 0,           0,          1);
}

int main() {

    /**
     * @brief OpenCV实现
    */
    cv::Mat w_P = (cv::Mat_<float>(3, 1) << 1.0, 2.0, 3.0);

    // 旋转矩阵计算函数
    auto calcR = [](float theta, cv::Mat& Rz){   
            theta = theta / 180.0 * CV_PI;
            Rz = (cv::Mat_<float>(3,3) << cos(theta), -sin(theta), 0, 
                                          sin(theta),  cos(theta), 0, 
                                           0,           0,         1);};

    // 绕Z轴旋转90度的旋转矩阵，描述了世界坐标系相对于相机坐标系的旋转（从世界系旋转到相机系）
    cv::Mat c_R_w;
    calcR(90, c_R_w);

    // 平移向量t，描述世界系原点在相机系中的位置
    cv::Mat c_t_w = (cv::Mat_<float>(3, 1) << 1.0, 1.0, 1.0);  

    // 点P在相机坐标系下的坐标
    cv::Mat c_P = c_R_w * w_P + c_t_w;
    
    // 输出
    std::cout << "c_P = " << c_P << std::endl;

    /**
     * @brief Eigen实现
    */

    // 世界坐标系下的点P
    Eigen::Vector3d w_P1(1.0, 2.0, 3.0);  

    // 绕Z轴旋转90度的旋转矩阵，描述了世界坐标系相对于相机坐标系的旋转（从世界系旋转到相机系）
    Eigen::Matrix3d c_R_w1 = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix();

    // 平移向量t，描述世界系原点在相机系中的位置
    Eigen::Vector3d c_t_w1(1.0, 1.0, 1.0);  

    // 点P在相机坐标系下的坐标
    Eigen::Vector3d c_P1 = c_R_w1 * w_P1 + c_t_w1;  

    // 输出
    std::cout << "c_P1: " << c_P1 << std::endl;
    return 0;
}