From 06b1f381eab6e235c683a32e3db22a80d23da3d5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Dec 2015 12:34:11 -0800 Subject: [PATCH] Added position and velocity updates --- gtsam/navigation/ScenarioRunner.cpp | 116 ++++++++++++------ gtsam/navigation/ScenarioRunner.h | 11 +- gtsam/navigation/tests/testScenarioRunner.cpp | 44 +++---- 3 files changed, 111 insertions(+), 60 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 781f72aa5..df711c107 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -29,63 +29,109 @@ using namespace boost::assign; namespace gtsam { -using symbol_shorthand::T; -using symbol_shorthand::P; -using symbol_shorthand::V; +using symbol_shorthand::T; // for theta +using symbol_shorthand::P; // for position +using symbol_shorthand::V; // for velocity static const Symbol kBiasKey('B', 0); +static const noiseModel::Constrained::shared_ptr kAllConstrained = + noiseModel::Constrained::All(3); +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_->solve(biasValues); + VectorValues zetaValues = posterior_k_->optimize(biasValues); Vector9 zeta; - zeta << zetaValues.at(T(k_)), Vector3::Zero(), Vector3::Zero(); + zeta << zetaValues.at(T(k_)), zetaValues.at(P(k_)), zetaValues.at(V(k_)); return zeta; } +void PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc, + const Vector3& correctedOmega, + double dt) { + typedef map Terms; + + GaussianFactorGraph graph; + + // theta(1) = (measuredOmega - (bias + bias_delta)) * dt + graph.add({{T(k_ + 1), I_3x3}, {kBiasKey, omega_H_bias}}, + dt * correctedOmega, gyroscopeNoiseModel_); + + // pos(1) = 0 + graph.add({{P(k_ + 1), I_3x3}}, Vector3::Zero(), kAllConstrained); + + // vel(1) = (measuredAcc - (bias + bias_delta)) * dt + graph.add({{V(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias}}, + dt * correctedAcc, accelerometerNoiseModel_); + + // 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)); + posterior_k_ = graph.eliminatePartialSequential(keys, EliminateQR).first; + + k_ += 1; +} + void PreintegratedMeasurements2::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { typedef map Terms; - static const Matrix36 omega_D_bias = (Matrix36() << Z_3x3, I_3x3).finished(); // Correct measurements by subtracting bias + Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); - GaussianFactorGraph graph; - boost::shared_ptr bayesNet; - GaussianFactorGraph::shared_ptr shouldBeEmpty; - // Handle first time differently if (k_ == 0) { - // theta(1) = (measuredOmega - (bias + bias_delta)) * dt - graph.add({{T(k_ + 1), I_3x3}, {kBiasKey, omega_D_bias}}, - dt * correctedOmega, gyroscopeNoiseModel_); - - // eliminate all but biases - Ordering keys = list_of(T(k_ + 1)); - boost::tie(bayesNet, shouldBeEmpty) = - graph.eliminatePartialSequential(keys, EliminateQR); - } else { - // add previous posterior - graph.add(boost::static_pointer_cast(posterior_k_)); - - // theta(k+1) = theta(k) + inverse(H)*(measuredOmega - bias - bias_delta) dt - // => H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt - Matrix3 H = Rot3::ExpmapDerivative(theta_); - graph.add( - {{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias * dt}}, - dt * correctedOmega, gyroscopeNoiseModel_); - - // eliminate all but biases - Ordering keys = list_of(T(k_))(T(k_ + 1)); - boost::tie(bayesNet, shouldBeEmpty) = - graph.eliminatePartialSequential(keys, EliminateQR); + initPosterior(correctedAcc, correctedOmega, dt); + return; } + GaussianFactorGraph graph; + + // estimate current estimate from posterior + // TODO(frank): maybe we should store this + Vector9 zeta = currentEstimate(); + Vector3 theta_k = zeta.tail<3>(); + + // add previous posterior + for (const auto& conditional : *posterior_k_) + graph.add(boost::static_pointer_cast(conditional)); + + // theta(k+1) = theta(k) + inverse(H)*(measuredOmega - bias - bias_delta) dt + // => H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt + Matrix3 H = Rot3::ExpmapDerivative(theta_k); + graph.add({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias * dt}}, + dt * correctedOmega, gyroscopeNoiseModel_); + + // pos(k+1) = pos(k) + vel(k) dt + graph.add({{P(k_ + 1), I_3x3}, {P(k_), -I_3x3}, {V(k_), -I_3x3 * dt}}, + Vector3::Zero(), kAllConstrained); + + // vel(k+1) = vel(k) + Rk*(measuredAcc - bias - bias_delta) dt + // => Rkt*vel(k+1) - Rkt*vel(k) + bias_delta dt = (measuredAcc - bias) dt + Rot3 Rk = Rot3::Expmap(theta_k); + Matrix3 Rkt = Rk.transpose(); + graph.add( + {{V(k_ + 1), Rkt}, {V(k_), -Rkt}, {kBiasKey, acc_H_bias * dt}}, + dt * correctedAcc, accelerometerNoiseModel_); + + // eliminate all but biases + Ordering keys = list_of(P(k_))(V(k_))(T(k_))(P(k_ + 1))(V(k_ + 1))(T(k_ + 1)); + boost::shared_ptr bayesNet = + graph.eliminatePartialSequential(keys, EliminateQR).first; + // The bayesNet now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias) - // We marginalize zeta(k) by dropping the first factor - posterior_k_ = bayesNet->back(); + // We marginalize zeta(k) by only saving the conditionals of + // P(zeta(k+1)|bias): + posterior_k_ = boost::make_shared(); + for (const auto& conditional : *bayesNet) { + Symbol symbol(conditional->front()); + if (symbol.index() == k_ + 1) posterior_k_->push_back(conditional); + } + k_ += 1; } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index f8f580ac0..36627ac30 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -32,6 +32,8 @@ static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { return diagonal; } +class GaussianBayesNet; + /** * Class that integrates state estimate on the manifold. * We integrate zeta = [theta, position, velocity] @@ -42,11 +44,10 @@ class PreintegratedMeasurements2 { SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; const imuBias::ConstantBias estimatedBias_; - size_t k_; ///< index/count of measurements integrated - Vector3 theta_; ///< current estimate for theta + size_t k_; ///< index/count of measurements integrated /// posterior on current iterate, as a conditional P(zeta|bias_delta): - boost::shared_ptr posterior_k_; + boost::shared_ptr posterior_k_; public: typedef ImuFactor::PreintegratedMeasurements::Params Params; @@ -76,6 +77,10 @@ class PreintegratedMeasurements2 { OptionalJacobian<9, 6> H2 = boost::none) const; private: + // initialize posterior with first (corrected) IMU measurement + void initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, + double dt); + // 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 c6a563926..139967699 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -52,30 +52,30 @@ TEST(ScenarioRunner, Spin) { 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)); +} + +/* ************************************************************************* +/*/ +namespace forward { +const double v = 2; // m/s +ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} +/* ************************************************************************* +/*/ +TEST(ScenarioRunner, Forward) { + using namespace forward; + ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); + const double T = 0.1; // 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)); } -///* ************************************************************************* -///*/ -// namespace forward { -// const double v = 2; // m/s -// ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); -//} -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, Forward) { -// using namespace forward; -// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); -// const double T = 0.1; // 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)); -//} -// ///* ************************************************************************* ///*/ // TEST(ScenarioRunner, ForwardWithBias) {