将机器人六轴坐标转为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;
}