【C++】一个完整的位姿（Pose）计算系统，主要用于处理三维空间中的坐标系变换

---

1. 旋转矩阵计算

给定旋转角度 ( $RX=\varphi$ )、( $RY=\theta$ )、( $RZ=\psi$ )，旋转矩阵 ( $R$ ) 按 ZYX 顺序计算：

$$R=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$$

其中：
$$R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right],R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right],R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

最终旋转矩阵 ( $R$ ) 为：
$$R=\left[\begin{array}{ccc}{c}_{\theta }{c}_{\psi }& s_{\varphi }{s}_{\theta }{c}_{\psi }-c_{\varphi }{s}_{\psi }& c_{\varphi }{s}_{\theta }{c}_{\psi }+s_{\varphi }{s}_{\psi }\\ c_{\theta }{s}_{\psi }& s_{\varphi }{s}_{\theta }{s}_{\psi }+c_{\varphi }{c}_{\psi }& c_{\varphi }{s}_{\theta }{s}_{\psi }-s_{\varphi }{c}_{\psi }\\ -s_{\theta }& s_{\varphi }{c}_{\theta }& c_{\varphi }{c}_{\theta }\end{array}\right]$$
其中 ( $c_{\alpha }=\cos \alpha$ )，( $s_{\alpha }=\sin \alpha$ )。

---

2. 位姿矩阵计算

给定位置 ( $\mathbf{t}=[x,y,z{]}^T$ ) 和旋转矩阵 ( $R$ )，位姿矩阵 ( $T$ ) 为：
$$T=\left[\begin{array}{cc}R& \mathbf{t}\\ 0^T& 1\end{array}\right]$$

---

3. 逆位姿矩阵计算

位姿矩阵 ( $T$ ) 的逆矩阵 ( $T^{-1}$ ) 为：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^T\mathbf{t}\\ 0^T& 1\end{array}\right]$$

---

4. 相对位姿矩阵计算

给定两个位姿矩阵 ( $T_A$ ) 和 ( $T_B$ )，B 相对于 A 的位姿矩阵 ( $T_{B/A}$ ) 为：
$$T_{B/A}=T_A^{-1}\cdot T_B$$

---

5. RPY 角度提取

从旋转矩阵 ( $R$ ) 中提取 RPY 角度（Roll ( $\varphi$ )、Pitch ( $\theta$ )、Yaw ( $\psi$ )）：
$$\begin{gathered} \psi =\text{atan2}(R_{10},R_{00}) \\ \theta =\text{atan2}(-R_{20},\sqrt{R_{21}^2+R_{22}^2}) \\ \varphi =\text{atan2}(R_{21},R_{22}) \end{gathered}$$

---

6. 平移向量与欧几里得距离

从相对位姿矩阵 ( $T_{B/A}$ ) 中提取平移向量 ( ${\mathbf{t}}_{B/A}=[t_x,t_y,t_z{]}^T$ )，其欧几里得距离为：
$$\text{distance}=\Vert {\mathbf{t}}_{B/A}{\Vert }_2=\sqrt{t_x^2+t_y^2+t_z^2}$$

---

7. 测量距离与计算距离的差值

给定测量分量 ( ${\mathbf{t}}_{\text{measured}}=[x_m,y_m,z_m{]}^T$ )，测量距离为：
$$\text{measuredDistance}=\Vert {\mathbf{t}}_{\text{measured}}{\Vert }_2=\sqrt{x_m^2+y_m^2+z_m^2}$$

计算距离与测量距离的差值为：
$$\text{distanceDifference}=\left|\Vert {\mathbf{t}}_{B/A}{\Vert }_2-\Vert {\mathbf{t}}_{\text{measured}}{\Vert }_2\right|$$

---

总结

代码的核心数学公式如下：

1. 旋转矩阵：($R=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$ )
2. 位姿矩阵：( $T=\left[\begin{array}{cc}R& \mathbf{t}\\ 0^T& 1\end{array}\right]$ )
3. 逆位姿矩阵：( $T^{-1}=\left[\begin{array}{cc}{R}^T& -R^T\mathbf{t}\\ 0^T& 1\end{array}\right]$ )
4. 相对位姿矩阵：( $T_{B/A}=T_A^{-1}\cdot T_B$ )
5. RPY 角度：( $\psi =\text{atan2}(R_{10},R_{00})$ )，( $\theta =\text{atan2}(-R_{20},\sqrt{R_{21}^2+R_{22}^2})$ )，( $\varphi =\text{atan2}(R_{21},R_{22})$ )
6. 欧几里得距离：( $\Vert \mathbf{t}{\Vert }_2=\sqrt{t_x^2+t_y^2+t_z^2}$ )
7. 距离差值：( $\left|\Vert {\mathbf{t}}_{B/A}{\Vert }_2-\Vert {\mathbf{t}}_{\text{measured}}{\Vert }_2\right|$ )

这些公式构成了代码的数学基础，实现了位姿矩阵的计算、相对位姿的求解、RPY 角度的提取以及距离的计算。

#include <iostream>
#include <cmath>
#include <iomanip>

// 定义PI常量
const double PI = 3.14159265358979323846;

// 将角度转换为弧度
double deg2rad(double deg) {
    return deg * PI / 180.0;
}

// 计算旋转矩阵
void calculateRotationMatrix(double rx, double ry, double rz, double R[3][3]) {
    double cx = cos(deg2rad(rx));
    double sx = sin(deg2rad(rx));
    double cy = cos(deg2rad(ry));
    double sy = sin(deg2rad(ry));
    double cz = cos(deg2rad(rz));
    double sz = sin(deg2rad(rz));

    R[0][0] = cy * cz;
    R[0][1] = sx * sy * cz - cx * sz;
    R[0][2] = cx * sy * cz + sx * sz;

    R[1][0] = cy * sz;
    R[1][1] = sx * sy * sz + cx * cz;
    R[1][2] = cx * sy * sz - sx * cz;

    R[2][0] = -sy;
    R[2][1] = sx * cy;
    R[2][2] = cx * cy;
}

// 计算位姿矩阵
void calculatePoseMatrix(double x, double y, double z, double rx, double ry, double rz, double T[4][4]) {
    double R[3][3];
    calculateRotationMatrix(rx, ry, rz, R);

    T[0][0] = R[0][0]; T[0][1] = R[0][1]; T[0][2] = R[0][2]; T[0][3] = x;
    T[1][0] = R[1][0]; T[1][1] = R[1][1]; T[1][2] = R[1][2]; T[1][3] = y;
    T[2][0] = R[2][0]; T[2][1] = R[2][1]; T[2][2] = R[2][2]; T[2][3] = z;
    T[3][0] = 0;       T[3][1] = 0;       T[3][2] = 0;       T[3][3] = 1;
}

// 计算矩阵的逆
void inverseMatrix(double T[4][4], double invT[4][4]) {
    double R[3][3] = {
        {T[0][0], T[0][1], T[0][2]},
        {T[1][0], T[1][1], T[1][2]},
        {T[2][0], T[2][1], T[2][2]}
    };

    double det = R[0][0] * (R[1][1] * R[2][2] - R[1][2] * R[2][1])
        - R[0][1] * (R[1][0] * R[2][2] - R[1][2] * R[2][0])
        + R[0][2] * (R[1][0] * R[2][1] - R[1][1] * R[2][0]);

    double invR[3][3] = {
        {(R[1][1] * R[2][2] - R[1][2] * R[2][1]) / det, (R[0][2] * R[2][1] - R[0][1] * R[2][2]) / det, (R[0][1] * R[1][2] - R[0][2] * R[1][1]) / det},
        {(R[1][2] * R[2][0] - R[1][0] * R[2][2]) / det, (R[0][0] * R[2][2] - R[0][2] * R[2][0]) / det, (R[0][2] * R[1][0] - R[0][0] * R[1][2]) / det},
        {(R[1][0] * R[2][1] - R[1][1] * R[2][0]) / det, (R[0][1] * R[2][0] - R[0][0] * R[2][1]) / det, (R[0][0] * R[1][1] - R[0][1] * R[1][0]) / det}
    };

    double t[3] = { T[0][3], T[1][3], T[2][3] };
    double invt[3] = {
        -(invR[0][0] * t[0] + invR[0][1] * t[1] + invR[0][2] * t[2]),
        -(invR[1][0] * t[0] + invR[1][1] * t[1] + invR[1][2] * t[2]),
        -(invR[2][0] * t[0] + invR[2][1] * t[1] + invR[2][2] * t[2])
    };

    invT[0][0] = invR[0][0]; invT[0][1] = invR[0][1]; invT[0][2] = invR[0][2]; invT[0][3] = invt[0];
    invT[1][0] = invR[1][0]; invT[1][1] = invR[1][1]; invT[1][2] = invR[1][2]; invT[1][3] = invt[1];
    invT[2][0] = invR[2][0]; invT[2][1] = invR[2][1]; invT[2][2] = invR[2][2]; invT[2][3] = invt[2];
    invT[3][0] = 0;          invT[3][1] = 0;          invT[3][2] = 0;          invT[3][3] = 1;
}

// 计算两个矩阵的乘积
void multiplyMatrices(double A[4][4], double B[4][4], double result[4][4]) {
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            result[i][j] = 0;
            for (int k = 0; k < 4; k++) {
                result[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

// 计算RPY角度
void calculateRPY(double R[3][3], double& rz, double& ry, double& rx) {
    rz = atan2(R[1][0], R[0][0]) * 180.0 / PI;
    ry = atan2(-R[2][0], sqrt(R[2][1] * R[2][1] + R[2][2] * R[2][2])) * 180.0 / PI;
    rx = atan2(R[2][1], R[2][2]) * 180.0 / PI;
}

// 计算向量的二范数
double calculateEuclideanNorm(double x, double y, double z) {
    return sqrt(x * x + y * y + z * z);
}

int main() {
    // 输入A的位姿参数
    double A_x, A_y, A_z, A_rx, A_ry, A_rz;
    std::cout << "Enter A X: ";
    std::cin >> A_x;
    std::cout << "Enter A Y: ";
    std::cin >> A_y;
    std::cout << "Enter A Z: ";
    std::cin >> A_z;
    std::cout << "Enter A RX (in degrees): ";
    std::cin >> A_rx;
    std::cout << "Enter A RY (in degrees): ";
    std::cin >> A_ry;
    std::cout << "Enter A RZ (in degrees): ";
    std::cin >> A_rz;

    // 计算A的位姿矩阵
    double A_T[4][4];
    calculatePoseMatrix(A_x, A_y, A_z, A_rx, A_ry, A_rz, A_T);

    // 输出A的位姿矩阵
    std::cout << "A pose params: X=" << A_x << ", Y=" << A_y << ", Z=" << A_z << ", RX=" << A_rx << ", RY=" << A_ry << ", RZ=" << A_rz << std::endl;
    std::cout << "A 相对global frame Pose Matrix:" << std::endl;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            std::cout << std::setw(12) << A_T[i][j] << " ";
        }
        std::cout << std::endl;
    }

    // 输入B的位姿参数
    double B_x, B_y, B_z, B_rx, B_ry, B_rz;
    std::cout << "Enter B X: ";
    std::cin >> B_x;
    std::cout << "Enter B Y: ";
    std::cin >> B_y;
    std::cout << "Enter B Z: ";
    std::cin >> B_z;
    std::cout << "Enter B RX (in degrees): ";
    std::cin >> B_rx;
    std::cout << "Enter B RY (in degrees): ";
    std::cin >> B_ry;
    std::cout << "Enter B RZ (in degrees): ";
    std::cin >> B_rz;

    // 计算B的位姿矩阵
    double B_T[4][4];
    calculatePoseMatrix(B_x, B_y, B_z, B_rx, B_ry, B_rz, B_T);

    // 输出B的位姿矩阵
    std::cout << "B pose params: X=" << B_x << ", Y=" << B_y << ", Z=" << B_z << ", RX=" << B_rx << ", RY=" << B_ry << ", RZ=" << B_rz << std::endl;
    std::cout << "B 相对global frame Pose Matrix:" << std::endl;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            std::cout << std::setw(12) << B_T[i][j] << " ";
        }
        std::cout << std::endl;
    }

    // 计算B相对于A的位姿矩阵
    double invA_T[4][4];
    inverseMatrix(A_T, invA_T);

    double B_relative_to_A_T[4][4];
    multiplyMatrices(invA_T, B_T, B_relative_to_A_T);

    // 输出B相对于A的位姿矩阵
    std::cout << "B相对A的位姿矩阵 :" << std::endl;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            std::cout << std::setw(12) << B_relative_to_A_T[i][j] << " ";
        }
        std::cout << std::endl;
    }

    // 计算B相对于A的RPY角度
    double R[3][3] = {
        {B_relative_to_A_T[0][0], B_relative_to_A_T[0][1], B_relative_to_A_T[0][2]},
        {B_relative_to_A_T[1][0], B_relative_to_A_T[1][1], B_relative_to_A_T[1][2]},
        {B_relative_to_A_T[2][0], B_relative_to_A_T[2][1], B_relative_to_A_T[2][2]}
    };

    double rz, ry, rx;
    calculateRPY(R, rz, ry, rx);

    // 输出B相对于A的RPY角度
    std::cout << "B相对A : RPY [即 RZ RY RX] (degree):" << std::endl;
    std::cout << rz << std::endl;
    std::cout << ry << std::endl;
    std::cout << rx << std::endl;

    // 输出B相对于A的平移向量
    std::cout << "B相对A : Translation (X, Y, Z):" << std::endl;
    std::cout << B_relative_to_A_T[0][3] << std::endl;
    std::cout << B_relative_to_A_T[1][3] << std::endl;
    std::cout << B_relative_to_A_T[2][3] << std::endl;

    // 计算平移向量的二范数
    double translationNorm = calculateEuclideanNorm(B_relative_to_A_T[0][3], B_relative_to_A_T[1][3], B_relative_to_A_T[2][3]);
    std::cout << "计算B相对A的Translation的二范数 (Euclidean norm): " << translationNorm << std::endl;

    // 输入B与A的测量分量
    double measured_x, measured_y, measured_z;
    std::cout << "Enter B与A的测量分量 X: ";
    std::cin >> measured_x;
    std::cout << "Enter B与A的测量分量 Y: ";
    std::cin >> measured_y;
    std::cout << "Enter B与A的测量分量 Z: ";
    std::cin >> measured_z;

    // 计算测量的B原点到A原点距离
    double measuredDistance = calculateEuclideanNorm(measured_x, measured_y, measured_z);
    std::cout << "测量的B原点到A原点距离: " << measuredDistance << std::endl;

    // 计算测量的距离与计算的距离之差
    double distanceDifference = fabs(translationNorm - measuredDistance);
    std::cout << "测量的距离与计算的距离之差：" << distanceDifference << std::endl;

    return 0;
}

以下是对代码的总结：

---

代码功能概述

这段代码实现了一个完整的位姿（Pose）计算系统，主要用于处理三维空间中的坐标系变换。它能够：

1. 输入两个坐标系（A 和 B）的位姿参数（位置和旋转角度）。
2. 计算每个坐标系相对于全局坐标系的位姿矩阵。
3. 计算 B 坐标系相对于 A 坐标系的位姿矩阵。
4. 从相对位姿矩阵中提取旋转角度（RPY，即 Roll、Pitch、Yaw）。
5. 计算 B 相对于 A 的平移向量及其欧几里得距离（二范数）。
6. 输入测量分量并计算测量距离，与计算距离进行比较。

---

代码结构

代码分为以下几个部分：

1. 工具函数

• deg2rad：将角度转换为弧度。
• calculateRotationMatrix：根据给定的旋转角度（RX、RY、RZ）计算 3x3 旋转矩阵。
• calculatePoseMatrix：根据位置（X、Y、Z）和旋转角度计算 4x4 位姿矩阵。
• inverseMatrix：计算 4x4 位姿矩阵的逆矩阵。
• multiplyMatrices：计算两个 4x4 矩阵的乘积。
• calculateRPY：从 3x3 旋转矩阵中提取 RPY 角度（Roll、Pitch、Yaw）。
• calculateEuclideanNorm：计算三维向量的欧几里得范数（二范数）。

2. 主函数 main

• 输入 A 的位姿参数：读取 A 的位置（X、Y、Z）和旋转角度（RX、RY、RZ）。
• 计算 A 的位姿矩阵：调用 calculatePoseMatrix 计算 A 的位姿矩阵，并输出。
• 输入 B 的位姿参数：读取 B 的位置（X、Y、Z）和旋转角度（RX、RY、RZ）。
• 计算 B 的位姿矩阵：调用 calculatePoseMatrix 计算 B 的位姿矩阵，并输出。
• 计算 B 相对于 A 的位姿矩阵：
  • 调用 inverseMatrix 计算 A 的位姿矩阵的逆矩阵。
  • 调用 multiplyMatrices 计算 B 相对于 A 的位姿矩阵，并输出。
• 计算 B 相对于 A 的 RPY 角度：调用 calculateRPY 提取 RPY 角度，并输出。
• 计算 B 相对于 A 的平移向量：从相对位姿矩阵中提取平移向量，并输出。
• 计算平移向量的二范数：调用 calculateEuclideanNorm 计算平移距离，并输出。
• 输入测量分量并计算测量距离：读取测量分量，计算测量距离，并与计算距离进行比较。

---

关键算法

1. 旋转矩阵计算：

   • 通过欧拉角（RX、RY、RZ）计算旋转矩阵，公式基于 ZYX 顺序。
   • 旋转矩阵用于描述坐标系在三维空间中的旋转。

2. 位姿矩阵计算：

   • 位姿矩阵是一个 4x4 的齐次变换矩阵，包含旋转和平移信息。
   • 公式：
     $$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
     其中，( $R$ ) 是 3x3 旋转矩阵，( $t$) 是 3x1 平移向量。

3. 逆矩阵计算：

   • 通过旋转矩阵的逆和平移向量的逆计算位姿矩阵的逆矩阵。
   • 公式：
     $$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$$

4. RPY 角度提取：

   • 从旋转矩阵中提取 Roll（绕 X 轴）、Pitch（绕 Y 轴）、Yaw（绕 Z 轴）角度。
   • 使用 atan2 函数确保角度的正确性。

5. 欧几里得距离计算：

   • 计算平移向量的二范数，公式：
     $$\text{distance}=\sqrt{x^2+y^2+z^2}$$

---

输入输出

输入

• A 和 B 的位姿参数：
  • 位置：X、Y、Z。
  • 旋转角度：RX、RY、RZ（以度为单位）。
• B 与 A 的测量分量：X、Y、Z。

输出

• A 和 B 的位姿矩阵。
• B 相对于 A 的位姿矩阵。
• B 相对于 A 的 RPY 角度。
• B 相对于 A 的平移向量。
• 平移向量的二范数（欧几里得距离）。
• 测量距离与计算距离的差值。

---

代码特点

1. 模块化设计：
   • 每个功能都封装为独立的函数，便于维护和扩展。
2. 数学计算准确：
   • 使用标准的数学公式计算旋转矩阵、逆矩阵和 RPY 角度，确保结果的准确性。
3. 用户交互友好：
   • 通过控制台输入输出，用户可以直观地输入参数并查看结果。
4. 可扩展性强：
   • 可以轻松扩展以支持更多的坐标系或更复杂的变换。

---

示例运行

输入：

Enter A X: -1977.53
Enter A Y: 1079.95
Enter A Z: 2108.81
Enter A RX (in degrees): 180
Enter A RY (in degrees): 0
Enter A RZ (in degrees): 62.31
Enter B X: -3229.91
Enter B Y: 809.92
Enter B Z: 816.10
Enter B RX (in degrees): -7.83
Enter B RY (in degrees): -11.96
Enter B RZ (in degrees): 16.17
Enter B与A的测量分量 X: 1252.38
Enter B与A的测量分量 Y: 270
Enter B与A的测量分量 Z: 1292.70

输出：

A pose params: X=-1977.53, Y=1079.95, Z=2108.81, RX=180, RY=0, RZ=62.31
A 相对global frame Pose Matrix:
    0.464688     0.885475  1.08439e-16     -1977.53
    0.885475    -0.464688 -5.69078e-17      1079.95
          -0  1.22465e-16           -1      2108.81
           0            0            0            1
B pose params: X=-3229.91, Y=809.92, Z=816.1, RX=-7.83, RY=-11.96, RZ=16.17
B 相对global frame Pose Matrix:
    0.939591    -0.248777    -0.235115     -3229.91
    0.272443     0.959347    0.0736721       809.92
    0.207229    -0.133277     0.969172        816.1
           0            0            0            1
B相对A的位姿矩阵 :
    0.677858     0.733874   -0.0440201      -821.07
    0.705383    -0.666082    -0.242423     -983.471
   -0.207229     0.133277    -0.969172      1292.71
           0            0            0            1
B相对A : RPY [即 RZ RY RX] (degree):
46.14
11.96
172.17
B相对A : Translation (X, Y, Z):
-821.07
-983.471
1292.71
计算B相对A的Translation的二范数 (Euclidean norm): 1820.02
测量的B原点到A原点距离: 1820.01
测量的距离与计算的距离之差：0.0115535

---

总结

这段代码是一个功能完整的三维位姿计算工具，适用于机器人学、计算机图形学等领域。它通过清晰的模块化设计和准确的数学计算，能够高效地处理坐标系变换问题。