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

Working AdjointMap and hence also derived derivatives of retract/localCoordinates

release/4.3a0
dellaert 2015-07-21 16:25:58 -04:00
parent 2dbad989d1
commit 2dd7987478
3 changed files with 21 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/NavState.cpp
View File

@ -101,7 +101,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
Eigen::Block<const Vector9, 3, 1> v = dP(xi); Eigen::Block<const Vector9, 3, 1> v = dP(xi);
Eigen::Block<const Vector9, 3, 1> a = dV(xi); Eigen::Block<const Vector9, 3, 1> a = dV(xi);
// NOTE(frank): mostly copy/paste from Pose3 // NOTE(frank): See Pose3::Expmap
Rot3 nRb = Rot3::Expmap(n_omega_nb); Rot3 nRb = Rot3::Expmap(n_omega_nb);
double theta2 = n_omega_nb.dot(n_omega_nb); double theta2 = n_omega_nb.dot(n_omega_nb);
if (theta2 > std::numeric_limits<double>::epsilon()) { if (theta2 > std::numeric_limits<double>::epsilon()) {
@ -126,7 +126,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
TIE(nRb, n_p, n_v, nTb); TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p.vector(); Vector3 n_t = n_p.vector();
// NOTE(frank): mostly copy/paste from Pose3 // NOTE(frank): See Pose3::Logmap
Vector9 xi; Vector9 xi;
Vector3 n_omega_nb = Rot3::Logmap(nRb); Vector3 n_omega_nb = Rot3::Logmap(nRb);
double theta = n_omega_nb.norm(); double theta = n_omega_nb.norm();
@ -147,7 +147,14 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
} }
Matrix9 NavState::AdjointMap() const { Matrix9 NavState::AdjointMap() const {
throw std::runtime_error("NavState::AdjointMap not implemented yet"); // NOTE(frank): See Pose3::AdjointMap
const Matrix3 nRb = R();
Matrix3 pAr = skewSymmetric(t()) * nRb;
Matrix3 vAr = skewSymmetric(v()) * nRb;
Matrix9 adj;
// nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
return adj;
} }
Matrix7 NavState::wedge(const Vector9& xi) { Matrix7 NavState::wedge(const Vector9& xi) {

2
gtsam/navigation/NavState.h
View File

@ -189,7 +189,7 @@ public:
OptionalJacobian<9, 9> H = boost::none); OptionalJacobian<9, 9> H = boost::none);
/// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
/// it to a tangent vector at idenity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
Matrix9 AdjointMap() const; Matrix9 AdjointMap() const;
/// wedge creates Lie algebra element from tangent vector /// wedge creates Lie algebra element from tangent vector

20
gtsam/navigation/tests/testNavState.cpp
View File

@ -81,16 +81,16 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
// Check retract derivatives // Check retract derivatives
// Matrix9 aH1, aH2; Matrix9 aH1, aH2;
// kState1.retract(xi, aH1, aH2); kState1.retract(xi, aH1, aH2);
// Matrix eH1 = numericalDerivative11<NavState, NavState>( Matrix eH1 = numericalDerivative11<NavState, NavState>(
// boost::bind(&NavState::retract, _1, xi, boost::none, boost::none), boost::bind(&NavState::retract, _1, xi, boost::none, boost::none),
// kState1); kState1);
// EXPECT(assert_equal(eH1, aH1)); EXPECT(assert_equal(eH1, aH1));
// Matrix eH2 = numericalDerivative11<NavState, Vector9>( Matrix eH2 = numericalDerivative11<NavState, Vector9>(
// boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none), boost::bind(&NavState::retract, kState1, _1, boost::none, boost::none),
// xi); xi);
// EXPECT(assert_equal(eH2, aH2)); EXPECT(assert_equal(eH2, aH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */