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

Simplify code as we know the Jacobian = R

release/4.3a0
AndreMichelin 2024-08-12 19:55:06 -07:00
parent c22b76506c
commit c2145bdffc
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -75,13 +75,11 @@ Rot3 IncrementalRotation::operator()(
Vector3 correctedOmega = measuredOmega - bias; Vector3 correctedOmega = measuredOmega - bias;
// Then compensate for sensor-body displacement: we express the quantities // Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame. If Jacobian is // (originally in the IMU frame) into the body frame.
// requested, the rotation matrix is obtained as `rotate` Jacobian. // Note that the rotate Jacobian is just body_P_sensor->rotation().matrix().
Matrix3 body_R_sensor;
if (body_P_sensor) { if (body_P_sensor) {
// rotation rate vector in the body frame // rotation rate vector in the body frame
correctedOmega = body_P_sensor->rotation().rotate( correctedOmega = body_P_sensor->rotation() * correctedOmega;
correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
} }
// rotation vector describing rotation increment computed from the // rotation vector describing rotation increment computed from the
@ -90,7 +88,7 @@ Rot3 IncrementalRotation::operator()(
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
if (H_bias) { if (H_bias) {
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative *H_bias *= -deltaT; // Correct so accurately reflects bias derivative
if (body_P_sensor) *H_bias *= body_R_sensor; if (body_P_sensor) *H_bias *= body_P_sensor->rotation().matrix();
} }
return incrR; return incrR;
} }