fuxiaoer/KF-GINS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add singularity warning log in matrix2euler function

Browse Source
This commit is contained in:
liqiang 2022-12-02 21:34:47 +08:00
parent 90a1505e06
commit 57db2fa74b
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/common/rotation.h
View File

@ -24,6 +24,7 @@
#define ROTATION_H #define ROTATION_H
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <iostream>
using Eigen::Matrix3d; using Eigen::Matrix3d;
using Eigen::Quaterniond; using Eigen::Quaterniond;
@ -47,11 +48,13 @@ public:
euler[1] = atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2))); euler[1] = atan(-dcm(2, 0) / sqrt(dcm(2, 1) * dcm(2, 1) + dcm(2, 2) * dcm(2, 2)));
if (dcm(2, 0) <= -0.999) { if (dcm(2, 0) <= -0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2)); euler[0] = 0;
euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1))); euler[2] = atan2((dcm(1, 2) - dcm(0, 1)), (dcm(0, 2) + dcm(1, 1)));
std::cout << "[WARNING] Rotation::matrix2euler: Singular Euler Angle! Set the roll angle to 0!" << std::endl;
} else if (dcm(2, 0) >= 0.999) { } else if (dcm(2, 0) >= 0.999) {
euler[0] = atan2(dcm(2, 1), dcm(2, 2)); euler[0] = 0;
euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1))); euler[2] = M_PI + atan2((dcm(1, 2) + dcm(0, 1)), (dcm(0, 2) - dcm(1, 1)));
std::cout << "[WARNING] Rotation::matrix2euler: Singular Euler Angle! Set the roll angle to 0!" << std::endl;
} else { } else {
euler[0] = atan2(dcm(2, 1), dcm(2, 2)); euler[0] = atan2(dcm(2, 1), dcm(2, 2));
euler[2] = atan2(dcm(1, 0), dcm(0, 0)); euler[2] = atan2(dcm(1, 0), dcm(0, 0));