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

Small change, prototype inline retract

release/4.3a0
dellaert 2016-06-04 16:22:31 -07:00
parent 87446914df
commit 3d30f4ac5a
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/navigation/NavState.cpp
View File

@ -104,9 +104,16 @@ NavState NavState::inverse() const {
NavState NavState::operator*(const NavState& bTc) const {
TIE(nRb, n_t, n_v, *this);
TIE(bRc, b_t, b_v, bTc);
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v);
}
//------------------------------------------------------------------------------
//NavState NavState::retract(const Vector9& xi) const {
// TIE(nRb, n_t, n_v, *this);
// const Rot3 bRc = Rot3::Expmap(dR(xi));
// return NavState(nRb * bRc, n_t + nRb * dP(xi), n_v + nRb * dV(xi));
//}
//------------------------------------------------------------------------------
NavState::PositionAndVelocity //
NavState::operator*(const PositionAndVelocity& b_tv) const {