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

Fix comments about R^T

release/4.3a0
Frank Dellaert 2025-01-16 00:41:08 -05:00
parent fdf8a51a44
commit e3e4736e5e
2 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Pose3.cpp
View File

@ -209,8 +209,8 @@ 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 R^T // We are creating a Pose3, so we still need to chain H with R^T, the
// to translate from left to right for our right expmap convention: // Jacobian of Pose3::Create with respect to t.
const Matrix3 Q = R.matrix().transpose() * H; const Matrix3 Q = R.matrix().transpose() * H;
*Hxi << Jr, Z_3x3, // *Hxi << Jr, Z_3x3, //
Q, Jr; Q, Jr;
@ -288,8 +288,9 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
Matrix3 H; Matrix3 H;
local.applyLeftJacobian(v, H); local.applyLeftJacobian(v, H);
// Multiply with R^T, translates from left to right for our expmap convention: // Multiply with R^T to account for the Pose3::Create Jacobian.
return local.expmap().transpose() * H; const Matrix3 R = local.expmap();
return R.transpose() * H;
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/navigation/NavState.cpp
View File

@ -134,8 +134,9 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
if (Hxi) { if (Hxi) {
// See Pose3::Expamp for explanation of the Jacobians
const Matrix3 Jr = local.rightJacobian(); const Matrix3 Jr = local.rightJacobian();
// We are creating a NavState, so we still need to chain H_t_w and H_v_w
// with R^T, the Jacobian of Navstate::Create with respect to both t and v.
const Matrix3 M = R.matrix(); const Matrix3 M = R.matrix();
*Hxi << Jr, Z_3x3, Z_3x3, // *Hxi << Jr, Z_3x3, Z_3x3, //
M.transpose() * H_t_w, Jr, Z_3x3, // M.transpose() * H_t_w, Jr, Z_3x3, //
@ -250,11 +251,12 @@ 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 R^T, translates from left to right for our expmap convention: // Multiply with R^T to account for NavState::Create Jacobian.
const Matrix3 R = local.expmap(); const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w; const Matrix3 Qt = R.transpose() * H_t_w;
const Matrix3 Qv = R.transpose() * H_v_w; const Matrix3 Qv = R.transpose() * H_v_w;
// Now compute the blocks of the LogmapDerivative Jacobian
const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw;