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

X = R^T, according to Barfoot Eq 8.83

release/4.3a0
Frank Dellaert 2025-01-15 18:09:26 -05:00
parent b0e9b16cc4
commit fdf8a51a44
2 changed files with 12 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Pose3.cpp
View File

@ -209,11 +209,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
if (Hxi) { if (Hxi) {
// The Jacobian of expmap is given by the right Jacobian of SO(3): // The Jacobian of expmap is given by the right Jacobian of SO(3):
const Matrix3 Jr = local.rightJacobian(); const Matrix3 Jr = local.rightJacobian();
// We multiply H, the derivative of applyLeftJacobian in omega, with // We multiply H, the derivative of applyLeftJacobian in omega, with R^T
// X = Jr * Jl^{-1}, // to translate from left to right for our right expmap convention:
// which translates from left to right for our right expmap convention: const Matrix3 Q = R.matrix().transpose() * H;
const Matrix3 X = Jr * local.leftJacobianInverse();
const Matrix3 Q = X * H;
*Hxi << Jr, Z_3x3, // *Hxi << Jr, Z_3x3, //
Q, Jr; Q, Jr;
} }
@ -290,9 +288,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
Matrix3 H; Matrix3 H;
local.applyLeftJacobian(v, H); local.applyLeftJacobian(v, H);
// Multiply with X, translates from left to right for our expmap convention: // Multiply with R^T, translates from left to right for our expmap convention:
const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); return local.expmap().transpose() * H;
return X * H;
} }
/* ************************************************************************* */ /* ************************************************************************* */

14
gtsam/navigation/NavState.cpp
View File

@ -136,10 +136,10 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
if (Hxi) { if (Hxi) {
// See Pose3::Expamp for explanation of the Jacobians // See Pose3::Expamp for explanation of the Jacobians
const Matrix3 Jr = local.rightJacobian(); const Matrix3 Jr = local.rightJacobian();
const Matrix3 X = Jr * local.leftJacobianInverse(); const Matrix3 M = R.matrix();
*Hxi << Jr, Z_3x3, Z_3x3, // *Hxi << Jr, Z_3x3, Z_3x3, //
X * H_t_w, Jr, Z_3x3, // M.transpose() * H_t_w, Jr, Z_3x3, //
X * H_v_w, Z_3x3, Jr; M.transpose() * H_v_w, Z_3x3, Jr;
} }
return NavState(R, t, v); return NavState(R, t, v);
@ -250,10 +250,10 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
local.applyLeftJacobian(rho, H_t_w); local.applyLeftJacobian(rho, H_t_w);
local.applyLeftJacobian(nu, H_v_w); local.applyLeftJacobian(nu, H_v_w);
// Multiply with X, translates from left to right for our expmap convention: // Multiply with R^T, translates from left to right for our expmap convention:
const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); const Matrix3 R = local.expmap();
const Matrix3 Qt = X * H_t_w; const Matrix3 Qt = R.transpose() * H_t_w;
const Matrix3 Qv = X * H_v_w; const Matrix3 Qv = R.transpose() * H_v_w;
const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qt2 = -Jw * Qt * Jw;