开发平台(Platform): (Ex: Win10, Linux, ...)
win10
编译器(Ex: GCC, clang, VC++...)+目标环境(跟开发平台不同的话需列出)
C++
额外使用到的函数库(Library Used): (Ex: OpenGL, ...)
eigen
问题(Question)：
各位前辈好
小弟我目前想要计算空间中的转换矩阵
A(Xa,Ya,Za)为某S面的中心点
B(Xb,Yb,Zb)为某W面的中心点
S面与W面有固定的位置关系
S面经过旋转及平移后的中心位置为A'(Xa',Ya',Za')
我的计算方式是
先将 旋转平移后S面的角度(单位:度) 减去 S面原先的角度
=> 得到的尤拉角计算出旋转矩阵
再将原本A(Xa,Ya,Za) 乘上 旋转矩阵 得到 A旋转完后的座标
再取 A' 点减此座标 得到平移矩阵
将平移矩阵与旋转矩阵合并乘 转换矩阵
再将B乘上转换矩阵 得到W面在相同旋转平移后B'的座标
不过得到的座标B'不太正确
想请教各位前辈 是不是小弟我逻辑上有错误
程式码的部分整理好后附上
喂入的资料(Input)：
A(7.48116 , -16.723 , 1237.61) S面夹角(-1.4951 ,0.440311 ,-151.567)
A'(71.769 , -17.6274, 12341.06) S'面夹角(-0.166662 ,1.80244 ,-151.815)
B(26.05982 ,-16.57224, 1236.45) W面夹角(1.536868 ,1.711784 ,-150.7084)
预期的正确结果(Expected Output)：
B'(90.4556, -17.1076 , 1241.89) W'面颊角(2.25271 ,2.95927, -151.271)
计算完可能会有误差，容许范围0.02或更小
错误结果(Wrong Output)：
程式码(Code)：(请善用置底文网页, 记得排版，禁止使用图档)
#include <iostream>
#include <eigen/Eigen>
#include <math.h>
typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 4, 4> Matrix4d;
typedef Eigen::Matrix<double, 4, 1> Vector4d;
typedef struct _Pose
{
double x;
double y;
double z;
double rx;
double ry;
double rz;
}POSE;
POSE GetFinalPose( POSE prePos_X, POSE Pos_X , POSE currentPose_Y)
{
double angleX, angleY, angleZ;
//计算角度差 同一平面
angleX = Pos_X.rx - prePos_X.rx;
angleY = Pos_X.ry - prePos_X.ry;
angleZ = Pos_X.rz - prePos_X.rz;
Matrix4d _Rotation;
//得到旋转矩阵
_Rotation(0, 0) = cos(angleZ) * cos(angleY);
_Rotation(0, 1) = cos(angleZ) * sin(angleX) * sin(angleY) - cos(angleX) *
sin(angleZ);
_Rotation(0, 2) = sin(angleZ) * sin(angleX) + cos(angleZ) * cos(angleX) *
sin(angleY);
_Rotation(0, 3) = 0;
_Rotation(1, 0) = cos(angleY) * sin(angleZ);
_Rotation(1, 1) = cos(angleZ) * cos(angleX) + sin(angleZ) * sin(angleX) *
sin(angleY);
_Rotation(1, 2) = cos(angleX) * sin(angleZ) * sin(angleY) - cos(angleZ) *
sin(angleX);
_Rotation(1, 3) = 0;
_Rotation(2, 0) = -sin(angleY);
_Rotation(2, 1) = cos(angleY) * sin(angleX);
_Rotation(2, 2) = cos(angleX) * cos(angleY);
_Rotation(2, 3) = 0;
_Rotation(3, 0) = 0;
_Rotation(3, 1) = 0;
_Rotation(3, 2) = 0;
_Rotation(3, 3) = 1;
+
//设置位置
Vector4d _Position;
_Position(0, 0) = prePos_X.x;
_Position(1, 0) = prePos_X.y;
_Position(2, 0) = prePos_X.z;
_Position(3, 0) = 1;
//得到平移矩阵 R*t
Vector4d _Translation;
_Translation = _Rotation * _Position;
//计算平移差距
_Translation(0, 0) = Pos_X.x - _Translation(0, 0) ;
_Translation(1, 0) = Pos_X.y - _Translation(1, 0);
_Translation(2, 0) = Pos_X.z - _Translation(2, 0);
_Translation(3, 0) = 1;
std::cout << _Translation << std::endl;
//得到转换矩阵
_Rotation.block<4, 1>(0, 3) = _Translation;
std::cout << _Rotation << std::endl;
//点
_Position(0, 0) = currentPose_Y.x;
_Position(1, 0) = currentPose_Y.y;
_Position(2, 0) = currentPose_Y.z;
_Position(3, 0) = 1;
//计算 点*转换矩阵
_Translation = _Rotation * _Position;
POSE calPose;
calPose.x = _Translation(0, 0);
calPose.y = _Translation(1, 0);
calPose.z = _Translation(2, 0);
return calPose;
}
补充说明(Supplement)：