From afe20d83bd123dd76d2238eac55ab5e6b904e4bf Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Dec 2014 20:04:28 +0100 Subject: [PATCH] Dealing with Pose3 mojo loss --- gtsam/slam/tests/testAntiFactor.cpp | 4 ++-- gtsam_unstable/dynamics/FullIMUFactor.h | 2 +- gtsam_unstable/dynamics/IMUFactor.h | 2 +- .../dynamics/tests/testSimpleHelicopter.cpp | 12 +++++------- gtsam_unstable/slam/PosePriorFactor.h | 2 +- 5 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 5d73bcab8..1dc897599 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -41,7 +41,7 @@ TEST( AntiFactor, NegativeHessian) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); // Create a configuration corresponding to the ground truth Values values; @@ -92,7 +92,7 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; graph.push_back(PriorFactor(1, pose1, sigma)); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1c2807e7a..66538e802 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -100,7 +100,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 73fc58fa6..5ed079acb 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -90,7 +90,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); // no corresponding factor here - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index bdc71fea2..62cf07982 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -21,7 +21,7 @@ Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); -Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); +Pose3 g2(g1.expmap(h*V1_g1)); //Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; @@ -52,12 +52,10 @@ Vector testExpmapDeriv(const Vector6& v) { TEST(Reconstruction, ExpmapInvDeriv) { Matrix numericalExpmap = numericalDerivative11( - boost::function( - boost::bind(testExpmapDeriv, _1) - ), - Vector6(Vector::Zero(6)), 1e-5 - ); - Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); + boost::function(boost::bind(testExpmapDeriv, _1)), + Vector6(Vector::Zero(6)), 1e-5); + Pose3 newPose = Pose3::Expmap(h * V1_g1); + Matrix dExpInv = Pose3::LogmapDerivative(newPose); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); } diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 24f083907..991aae1fd 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -88,7 +88,7 @@ namespace gtsam { // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); } else { - if(H) (*H) = eye(p.dim()); + if(H) (*H) = I_6x6; // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p); }