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

Merge pull request #1700 from borglab/fix-1690 

Fix #1690
release/4.3a0
Varun Agrawal 2024-01-02 16:11:16 -05:00 committed by GitHub
parent 5c87f97546 b15bcb166e
commit c79543b625
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/navigation/NavState.cpp
View File

@ -106,7 +106,8 @@ bool NavState::equals(const NavState& other, double tol) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::retract(const Vector9& xi, // NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
auto [nRb, n_t, n_v] = (*this); Rot3 nRb = R_;
Point3 n_t = t_, n_v = v_;
Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);