将机器人六轴坐标转为4*4矩阵(Opencv/C++)

已知机器人六轴坐标x,y,z,rx,ry,rz，其中xyz表示空间位置坐标，rx,ry,rz是欧拉角；

需要将这六个值转为4*4的矩阵以便后续其它处理运算。

代码如下：

#include <opencv2/core.hpp>
#include <iostream>
#include <cmath>

using namespace cv;
using namespace std;

// 将 xyz 和欧拉角转换为 4x4 矩阵
Mat xyzEulerToMatrix(double x, double y, double z, double rx, double ry, double rz) {
  // 转换为弧度
  rx = rx * CV_PI / 180.0;
  ry = ry * CV_PI / 180.0;
  rz = rz * CV_PI / 180.0;

  // 构建旋转矩阵
  Mat rotX = (Mat_<double>(3, 3) <<
    1.0, 0.0, 0.0,
    0.0, cos(rx), -sin(rx),
    0.0, sin(rx), cos(rx)
  );

  Mat rotY = (Mat_<double>(3, 3) <<
    cos(ry), 0.0, sin(ry),
    0.0, 1.0, 0.0,
    -sin(ry), 0.0, cos(ry)
  );

  Mat rotZ = (Mat_<double>(3, 3) <<
    cos(rz), -sin(rz), 0.0,
    sin(rz), cos(rz), 0.0,
    0.0, 0.0, 1.0
  );

  // 构建平移矩阵
  Mat translation = (Mat_<double>(4, 4) <<
    1.0, 0.0, 0.0, x,
    0.0, 1.0, 0.0, y,
    0.0, 0.0, 1.0, z,
    0.0, 0.0, 0.0, 1.0
  );

  // 旋转矩阵相乘
  Mat rotation = rotZ * rotY * rotX;

  // 合并旋转和平移
  Mat matrix = (Mat_<double>(4, 4) <<
    rotation.at<double>(0, 0), rotation.at<double>(0, 1), rotation.at<double>(0, 2), x,
    rotation.at<double>(1, 0), rotation.at<double>(1, 1), rotation.at<double>(1, 2), y,
    rotation.at<double>(2, 0), rotation.at<double>(2, 1), rotation.at<double>(2, 2), z,
    0.0, 0.0, 0.0, 1.0
  );

  return matrix;
}

int main() {
  // 定义参数
  double x = 111.0;
  double y = 222.0;
  double z = 333.0;
  double rx = 60.0;
  double ry = 30.0;
  double rz = -90.0;

  // 计算 4x4 矩阵
  Mat matrix = xyzEulerToMatrix(x, y, z, rx, ry, rz);

  // 打印结果
  cout << "4x4 矩阵：" << endl << matrix << endl;

  return 0;
}