From 0dfd44f26cd7731582b5d84517c5a8bc5ac5f56c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Dec 2015 09:03:13 -0800 Subject: [PATCH] A first implementation of noiseModel and covariance --- gtsam/navigation/ScenarioRunner.cpp | 44 ++++++++++++++++--- gtsam/navigation/ScenarioRunner.h | 16 +++++-- gtsam/navigation/tests/testScenarioRunner.cpp | 6 +-- 3 files changed, 52 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index f0c9fd5a0..4b546c5c5 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -38,7 +38,6 @@ static const Matrix36 acc_H_bias = (Matrix36() << I_3x3, Z_3x3).finished(); static const Matrix36 omega_H_bias = (Matrix36() << Z_3x3, I_3x3).finished(); Vector9 PreintegratedMeasurements2::currentEstimate() const { - // TODO(frank): make faster version just for theta VectorValues biasValues; biasValues.insert(kBiasKey, estimatedBias_.vector()); VectorValues zetaValues = posterior_k_->optimize(biasValues); @@ -47,6 +46,14 @@ Vector9 PreintegratedMeasurements2::currentEstimate() const { return zeta; } +Vector3 PreintegratedMeasurements2::currentTheta() const { + // TODO(frank): make faster version theta = inv(R)*d + VectorValues biasValues; + biasValues.insert(kBiasKey, estimatedBias_.vector()); + VectorValues zetaValues = posterior_k_->optimize(biasValues); + return zetaValues.at(T(k_)); +} + PreintegratedMeasurements2::SharedBayesNet PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, @@ -73,7 +80,7 @@ PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc, // eliminate all but biases // NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias) - Ordering keys = list_of(P(k_ + 1))(V(k_ + 1))(T(k_ + 1)); + Ordering keys = list_of(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); return graph.eliminatePartialSequential(keys, EliminateQR).first; } @@ -85,10 +92,8 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc, GaussianFactorGraph graph; - // estimate current estimate from posterior - // TODO(frank): maybe we should store this - or only recover theta = inv(R)*d - Vector9 zeta = currentEstimate(); - Vector3 theta_k = zeta.head<3>(); + // estimate current theta from posterior + Vector3 theta_k = currentTheta(); Rot3 Rk = Rot3::Expmap(theta_k); Matrix3 Rkt = Rk.transpose(); @@ -119,12 +124,14 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc, correctedAcc * dt, accelerometerNoiseModel_); // eliminate all but biases - Ordering keys = list_of(P(k_))(V(k_))(T(k_))(P(k_ + 1))(V(k_ + 1))(T(k_ + 1)); + // TODO(frank): does not seem to eliminate in order I want. What gives? + Ordering keys = list_of(T(k_))(P(k_))(V(k_))(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); SharedBayesNet bayesNet = graph.eliminatePartialSequential(keys, EliminateQR).first; // The Bayes net now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias) // We marginalize zeta(k) by removing the conditionals on zeta(k) + // TODO(frank): could use erase(begin, begin+3) if order above was correct SharedBayesNet marginal = boost::make_shared(); for (const auto& conditional : *bayesNet) { Symbol symbol(conditional->front()); @@ -156,17 +163,40 @@ void PreintegratedMeasurements2::integrateMeasurement( NavState PreintegratedMeasurements2::predict( const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { + // Get mean of current posterior on zeta // TODO(frank): handle bias Vector9 zeta = currentEstimate(); + + // Correct for initial velocity and gravity Rot3 Ri = state_i.attitude(); Matrix3 Rit = Ri.transpose(); Vector3 gt = deltaTij_ * p_->n_gravity; zeta.segment<3>(3) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); zeta.segment<3>(6) += Rit * gt; + + // Convert local coordinates to manifold near state_i return state_i.retract(zeta); } +SharedGaussian PreintegratedMeasurements2::noiseModel() const { + Matrix RS; + Vector d; + GTSAM_PRINT(*posterior_k_); + boost::tie(RS, d) = posterior_k_->matrix(); + cout << RS << endl + << endl; + cout << d.transpose() << endl; + + // R'*R = A'*A = inv(Cov) + // TODO(frank): think of a faster way - implement in noiseModel + return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false); +} + +Matrix9 PreintegratedMeasurements2::preintMeasCov() const { + return noiseModel()->covariance(); +} + //////////////////////////////////////////////////////////////////////////////////////////// static double intNoiseVar = 0.0000001; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index c7b0d19ba..485a0a51f 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -80,9 +80,20 @@ class PreintegratedMeasurements2 { OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; - Matrix9 preintMeasCov() const { return Matrix9::Zero(); } + /// Return Gaussian noise model on prediction + SharedGaussian noiseModel() const; + + /// @deprecated: Explicitly calculate covariance + Matrix9 preintMeasCov() const; private: + // estimate zeta given estimated biases + // calculates conditional mean of P(zeta|bias_delta) + Vector9 currentEstimate() const; + + // estimate theta given estimated biases + Vector3 currentTheta() const; + // initialize posterior with first (corrected) IMU measurement SharedBayesNet initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, double dt) const; @@ -92,9 +103,6 @@ class PreintegratedMeasurements2 { const Vector3& correctedOmega, double dt) const; - // estimate zeta given estimated biases - // calculates conditional mean of P(zeta|bias_delta) - Vector9 currentEstimate() const; }; /* diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index b483fa17b..c681c6e06 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -47,13 +47,13 @@ TEST(ScenarioRunner, Spin) { const ExpmapScenario scenario(W, V); ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); - const double T = 0.1; // seconds + const double T = 0.5; // seconds auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */