12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleanup equation

release/4.3a0
Fan Jiang 2021-10-12 20:30:06 -04:00
parent dae409373c
commit aa6b432911
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/SO3.cpp
View File

@ -267,19 +267,19 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
if (max_elem == 2) {
const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R33);
const double correction = 1.0 - M_1_PI / r * (R21 - R12);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33);
const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12);
omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33);
} else if (max_elem == 1) {
const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R22);
const double correction = 1.0 - M_1_PI / r * (R13 - R31);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32);
const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31);
omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32);
} else {
// if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit
const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0;
const double r = sqrt(2.0 + 2.0 * R11);
const double correction = 1.0 - M_1_PI / r * (R32 - R23);
omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31);
const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23);
omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31);
}
} else {
double magnitude;