记录以下踩的坑。
在对四元数操作的时候,没有进行归一化,所以导致R矩阵非正交的结果。
本来旋转矩阵是正交的,但是如果没有对四元数归一化,生成的旋转矩阵就无法满足正交性。也就是说。
贴上代码:
#include <Eigen/Core>
#include <iostream>
#include <Eigen/Geometry>
using namespace std;
int main(){
Eigen::Quaterniond q = {0.1,0.35,0.2,0.3};
q.normalize();
Eigen::Matrix3d R = q.toRotationMatrix();
// cout << R << endl;
Eigen::Matrix3d R_transpose = R.transpose();
cout << R_transpose << endl << endl;
Eigen::Matrix3d R_inverse = R.inverse();
cout << R_inverse << endl << endl;
Eigen::Matrix3d R_zhengjiao = R * R.transpose();
cout << R_zhengjiao << endl << endl;
return 0;
}
以下为输出结果:
0.00952381 0.761905 0.647619
0.304762 -0.619048 0.72381
0.952381 0.190476 -0.238095
0.00952381 0.761905 0.647619
0.304762 -0.619048 0.72381
0.952381 0.190476 -0.238095
1 -2.57606e-16 -2.62811e-16
-2.57606e-16 1 -1.66533e-16
-2.62811e-16 -1.66533e-16 1