51 lines
1.4 KiB
C++
51 lines
1.4 KiB
C++
#include <opencv.hpp>
|
|
#include <iostream>
|
|
|
|
static double degToRad(double deg) {
|
|
return deg * M_PI / 180.0;
|
|
}
|
|
static double radToDeg(double deg) {
|
|
return deg / M_PI * 180.0;
|
|
}
|
|
static cv::Vec3d radToDeg(cv::Vec3d rad) {
|
|
rad[0] /= M_PI / 180.0;
|
|
rad[1] /= M_PI / 180.0;
|
|
rad[2] /= M_PI / 180.0;
|
|
return rad;
|
|
}
|
|
// 计算旋转矩阵
|
|
static cv::Mat get_rotation_matrix(double alpha, double beta, double gamma) {
|
|
// Roll (x-axis rotation)
|
|
cv::Mat R_x = (cv::Mat_<double>(3, 3) <<
|
|
1, 0, 0,
|
|
0, cos(alpha), -sin(alpha),
|
|
0, sin(alpha), cos(alpha));
|
|
|
|
// Pitch (y-axis rotation)
|
|
cv::Mat R_y = (cv::Mat_<double>(3, 3) <<
|
|
cos(beta), 0, sin(beta),
|
|
0, 1, 0,
|
|
-sin(beta), 0, cos(beta));
|
|
|
|
// Yaw (z-axis rotation)
|
|
cv::Mat R_z = (cv::Mat_<double>(3, 3) <<
|
|
cos(gamma), -sin(gamma), 0,
|
|
sin(gamma), cos(gamma), 0,
|
|
0, 0, 1);
|
|
|
|
// Combined rotation matrix
|
|
return R_z * R_y * R_x;
|
|
}
|
|
|
|
// 主函数
|
|
static cv::Mat transform_coordinates(cv::Mat coords, cv::Vec3d radInput1, cv::Vec3d radInput2) {
|
|
// 原始旋转矩阵
|
|
cv::Mat R_old = get_rotation_matrix(radInput1[0], radInput1[1], radInput1[2]);
|
|
// 新的旋转矩阵
|
|
cv::Mat R_new = get_rotation_matrix(radInput2[0], radInput2[1], radInput2[2]);
|
|
// 计算新坐标
|
|
cv::Mat new_coords = R_new * R_old.inv() * coords;
|
|
return new_coords;
|
|
}
|
|
|
|
#pragma once |