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

1. 旋转矩阵计算

给定旋转角度 ( R X = ϕ RX = \phi RX=ϕ )、( R Y = θ RY = \theta RY=θ )、( R Z = ψ RZ = \psi RZ=ψ )，旋转矩阵 ( R R R ) 按 ZYX 顺序计算：

R = R z ( ψ ) ⋅ R y ( θ ) ⋅ R x ( ϕ ) R = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi) R=Rz​(ψ)⋅Ry​(θ)⋅Rx​(ϕ)

其中：
R x ( ϕ ) = [ 1 0 0 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ cos ⁡ ϕ ] , R y ( θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] , R z ( ψ ) = [ cos ⁡ ψ − sin ⁡ ψ 0 sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}, \quad R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, \quad R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} Rx​(ϕ)= ​100​0cosϕsinϕ​0−sinϕcosϕ​ ​,Ry​(θ)= ​cosθ0−sinθ​010​sinθ0cosθ​ ​,Rz​(ψ)= ​cosψsinψ0​−sinψcosψ0​001​ ​

最终旋转矩阵 ( R R R ) 为：
R = [ c θ c ψ s ϕ s θ c ψ − c ϕ s ψ c ϕ s θ c ψ + s ϕ s ψ c θ s ψ s ϕ s θ s ψ + c ϕ c ψ c ϕ s θ s ψ − s ϕ c ψ − s θ s ϕ c θ c ϕ c θ ] R = \begin{bmatrix} c_\theta c_\psi & s_\phi s_\theta c_\psi - c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \\ c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{bmatrix} R= ​cθ​cψ​cθ​sψ​−sθ​​sϕ​sθ​cψ​−cϕ​sψ​sϕ​sθ​sψ​+cϕ​cψ​sϕ​cθ​​cϕ​sθ​cψ​+sϕ​sψ​cϕ​sθ​sψ​−sϕ​cψ​cϕ​cθ​​ ​
其中 ( c α = cos ⁡ α c_\alpha = \cos\alpha cα​=cosα )，( s α = sin ⁡ α s_\alpha = \sin\alpha sα​=sinα )。

2. 位姿矩阵计算

给定位置 ( t = [ x , y , z ] T \mathbf{t} = [x, y, z]^T t=[x,y,z]T ) 和旋转矩阵 ( R R R )，位姿矩阵 ( T T T ) 为：
T = [ R t 0 T 1 ] T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T=[R0T​t1​]

3. 逆位姿矩阵计算

位姿矩阵 ( T T T ) 的逆矩阵 ( T − 1 T^{-1} T−1 ) 为：
T − 1 = [ R T − R T t 0 T 1 ] T^{-1} = \begin{bmatrix} R^T & -R^T \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T−1=[RT0T​−RTt1​]

4. 相对位姿矩阵计算

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

5. RPY 角度提取

从旋转矩阵 ( R R R ) 中提取 RPY 角度（Roll ( ϕ \phi ϕ )、Pitch ( θ \theta θ )、Yaw ( ψ \psi ψ )）：
ψ = atan2 ( R 10 , R 00 ) θ = atan2 ( − R 20 , R 21 2 + R 22 2 ) ϕ = atan2 ( R 21 , R 22 ) \psi = \text{atan2}(R_{10}, R_{00}) \\ \theta = \text{atan2}(-R_{20}, \sqrt{R_{21}^2 + R_{22}^2}) \\ \phi = \text{atan2}(R_{21}, R_{22}) ψ=atan2(R10​,R00​)θ=atan2(−R20​,R212​+R222​ ​)ϕ=atan2(R21​,R22​)

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

从相对位姿矩阵 ( T B / A T_{B/A} TB/A​ ) 中提取平移向量 ( t B / A = [ t x , t y , t z ] T \mathbf{t}_{B/A} = [t_x, t_y, t_z]^T tB/A​=[tx​,ty​,tz​]T )，其欧几里得距离为：
distance = ∥ t B / A ∥ 2 = t x 2 + t y 2 + t z 2 \text{distance} = \|\mathbf{t}_{B/A}\|_2 = \sqrt{t_x^2 + t_y^2 + t_z^2} distance=∥tB/A​∥2​=tx2​+ty2​+tz2​ ​

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

给定测量分量 ( t measured = [ x m , y m , z m ] T \mathbf{t}_{\text{measured}} = [x_m, y_m, z_m]^T tmeasured​=[xm​,ym​,zm​]T )，测量距离为：
measuredDistance = ∥ t measured ∥ 2 = x m 2 + y m 2 + z m 2 \text{measuredDistance} = \|\mathbf{t}_{\text{measured}}\|_2 = \sqrt{x_m^2 + y_m^2 + z_m^2} measuredDistance=∥tmeasured​∥2​=xm2​+ym2​+zm2​ ​

计算距离与测量距离的差值为：
distanceDifference = ∣ ∥ t B / A ∥ 2 − ∥ t measured ∥ 2 ∣ \text{distanceDifference} = \left| \|\mathbf{t}_{B/A}\|_2 - \|\mathbf{t}_{\text{measured}}\|_2 \right| distanceDifference= ​∥tB/A​∥2​−∥tmeasured​∥2​ ​

总结

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

1. 旋转矩阵：( R = R z ( ψ ) ⋅ R y ( θ ) ⋅ R x ( ϕ ) R = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi) R=Rz​(ψ)⋅Ry​(θ)⋅Rx​(ϕ) )
2. 位姿矩阵：( T = [ R t 0 T 1 ] T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T=[R0T​t1​] )
3. 逆位姿矩阵：( T − 1 = [ R T − R T t 0 T 1 ] T^{-1} = \begin{bmatrix} R^T & -R^T \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} T−1=[RT0T​−RTt1​] )
4. 相对位姿矩阵：( T B / A = T A − 1 ⋅ T B T_{B/A} = T_A^{-1} \cdot T_B TB/A​=TA−1​⋅TB​ )
5. RPY 角度：( ψ = atan2 ( R 10 , R 00 ) \psi = \text{atan2}(R_{10}, R_{00}) ψ=atan2(R10​,R00​) )，( θ = atan2 ( − R 20 , R 21 2 + R 22 2 ) \theta = \text{atan2}(-R_{20}, \sqrt{R_{21}^2 + R_{22}^2}) θ=atan2(−R20​,R212​+R222​ ​) )，( ϕ = atan2 ( R 21 , R 22 ) \phi = \text{atan2}(R_{21}, R_{22}) ϕ=atan2(R21​,R22​) )
6. 欧几里得距离：( ∥ t ∥ 2 = t x 2 + t y 2 + t z 2 \|\mathbf{t}\|_2 = \sqrt{t_x^2 + t_y^2 + t_z^2} ∥t∥2​=tx2​+ty2​+tz2​ ​ )
7. 距离差值：( ∣ ∥ t B / A ∥ 2 − ∥ t measured ∥ 2 ∣ \left| \|\mathbf{t}_{B/A}\|_2 - \|\mathbf{t}_{\text{measured}}\|_2 \right| ​∥tB/A​∥2​−∥tmeasured​∥2​ ​ )

这些公式构成了代码的数学基础，实现了位姿矩阵的计算、相对位姿的求解、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 = [ R t 0 1 ] T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} T=[R0​t1​]
     其中，( R R R ) 是 3x3 旋转矩阵，( t t t) 是 3x1 平移向量。

3. 逆矩阵计算：

   • 通过旋转矩阵的逆和平移向量的逆计算位姿矩阵的逆矩阵。
   • 公式：
     T − 1 = [ R T − R T t 0 1 ] T^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0 & 1 \end{bmatrix} T−1=[RT0​−RTt1​]

4. RPY 角度提取：

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

5. 欧几里得距离计算：

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

输入输出

输入

• 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

总结

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