開發平台(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)：