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

Remove TODO

release/4.3a0
Frank Dellaert 2024-12-14 01:54:29 -05:00
parent 693eafb074
commit 3eba54c05c
1 changed files with 0 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/navigation/NavState.cpp
View File

@ -294,30 +294,6 @@ NavState NavState::retract(const Vector9& xi, //
//------------------------------------------------------------------------------
Vector9 NavState::localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
// return LieGroup<NavState, 9>::localCoordinates(g, H1, H2);
//TODO(Varun) Fix so that test on L680 passes
// Matrix3 D_dR_R, D_dt_R, D_dv_R;
// const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
// const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
// const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
// Vector9 xi;
// Matrix3 D_xi_R;
// xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
// if (H1) {
// *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
// D_dt_R, -I_3x3, Z_3x3, //
// D_dv_R, Z_3x3, -I_3x3;
// }
// if (H2) {
// *H2 << D_xi_R, Z_3x3, Z_3x3, //
// Z_3x3, dR.matrix(), Z_3x3, //
// Z_3x3, Z_3x3, dR.matrix();
// }
// return xi;
Matrix3 D_dR_R, D_dt_R, D_dv_R;
const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);