From d3d3b8399da305f1927a93892c48c66bd31e5422 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Dec 2015 13:17:41 -0800 Subject: [PATCH] Correct for gravity and V0 --- gtsam/navigation/ScenarioRunner.cpp | 14 ++ gtsam/navigation/ScenarioRunner.h | 22 +- gtsam/navigation/tests/testScenarioRunner.cpp | 219 +++++++++--------- 3 files changed, 133 insertions(+), 122 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index df711c107..680ee082c 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -83,6 +83,9 @@ void PreintegratedMeasurements2::integrateMeasurement( Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); + // increment time + deltaTij_ += dt; + // Handle first time differently if (k_ == 0) { initPosterior(correctedAcc, correctedOmega, dt); @@ -140,6 +143,17 @@ NavState PreintegratedMeasurements2::predict( OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { // TODO(frank): handle bias Vector9 zeta = currentEstimate(); + cout << "zeta: " << zeta << endl; + 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; + cout << "zeta: " << zeta << endl; + cout << "tij: " << deltaTij_ << endl; + cout << "gt: " << gt.transpose() << endl; + cout << "gt^2/2: " << 0.5 * deltaTij_ * gt.transpose() << endl; return state_i.expmap(zeta); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 36627ac30..ae024ecc1 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -40,27 +40,29 @@ class GaussianBayesNet; * See ImuFactor.lyx for the relevant math. */ class PreintegratedMeasurements2 { + public: + typedef ImuFactor::PreintegratedMeasurements::Params Params; + private: - SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; + const boost::shared_ptr p_; + const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; const imuBias::ConstantBias estimatedBias_; - size_t k_; ///< index/count of measurements integrated - + size_t k_; ///< index/count of measurements integrated + double deltaTij_; ///< sum of time increments /// posterior on current iterate, as a conditional P(zeta|bias_delta): boost::shared_ptr posterior_k_; public: - typedef ImuFactor::PreintegratedMeasurements::Params Params; - - Matrix9 preintMeasCov() const { return Matrix9::Zero(); } - PreintegratedMeasurements2( const boost::shared_ptr& p, const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias()) - : accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), + : p_(p), + accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), estimatedBias_(estimatedBias), - k_(0) {} + k_(0), + deltaTij_(0.0) {} /** * Add a single IMU measurement to the preintegration. @@ -76,6 +78,8 @@ class PreintegratedMeasurements2 { OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; + Matrix9 preintMeasCov() const { return Matrix9::Zero(); } + private: // initialize posterior with first (corrected) IMU measurement void initPosterior(const Vector3& correctedAcc, const Vector3& correctedOmega, diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 139967699..351161674 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -56,14 +56,12 @@ TEST(ScenarioRunner, Spin) { // 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); @@ -76,120 +74,115 @@ TEST(ScenarioRunner, Forward) { // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, ForwardWithBias) { -// using namespace forward; -// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); -// const double T = 0.1; // seconds -// -// auto pim = runner.integrate(T, kNonZeroBias); -// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); -// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -//} -// -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, Circle) { -// // Forward velocity 2m/s, angular velocity 6 kDegree/sec -// const double v = 2, w = 6 * kDegree; -// ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); -// -// 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(), 0.1)); -// -// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); -// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); -//} -// -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, Loop) { -// // Forward velocity 2m/s -// // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) -// const double v = 2, w = 6 * kDegree; -// ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); -// -// 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(), 0.1)); -// -// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); -// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); -//} -// -///* ************************************************************************* -///*/ -// namespace initial { -//// Set up body pointing towards y axis, and start at 10,20,0 with velocity -//// going in X. The body itself has Z axis pointing down -// const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -// const Point3 P0(10, 20, 0); -// const Vector3 V0(50, 0, 0); -//} -// -///* ************************************************************************* -///*/ -// namespace accelerating { -// using namespace initial; -// const double a = 0.2; // m/s^2 -// const Vector3 A(0, a, 0); -// const AcceleratingScenario scenario(nRb, P0, V0, A); -// -// const double T = 3; // seconds -//} -// -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, Accelerating) { +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + // using namespace forward; + // ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); + // const double T = 0.1; // seconds + // + // auto pim = runner.integrate(T, kNonZeroBias); + // Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, + // kNonZeroBias); + // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Circle) { + // Forward velocity 2m/s, angular velocity 6 kDegree/sec + const double v = 2, w = 6 * kDegree; + ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); + + 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(), 0.1)); + + // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + 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(), 0.1)); + + // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); +} + +/* ************************************************************************* +/*/ +namespace initial { +// Set up body pointing towards y axis, and start at 10,20,0 with velocity +// going in X. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} + +/* ************************************************************************* +/*/ +namespace accelerating { +using namespace initial; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + 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), 0.1)); +} + +/* ************************************************************************* */ +// TODO(frank):Fails ! +// TEST(ScenarioRunner, AcceleratingWithBias) { // using namespace accelerating; -// ScenarioRunner runner(&scenario, defaultParams(), T / 10); +// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, +// kNonZeroBias); // -// 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), 0.1)); -//} -// -///* ************************************************************************* -///*/ -//// TODO(frank):Fails ! -//// TEST(ScenarioRunner, AcceleratingWithBias) { -//// using namespace accelerating; -//// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, -//// kNonZeroBias); -//// -//// auto pim = runner.integrate(T, -//// kNonZeroBias); -//// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, -//// kNonZeroBias); -//// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); -////} -// -///* ************************************************************************* -///*/ -// TEST(ScenarioRunner, AcceleratingAndRotating) { -// using namespace initial; -// const double a = 0.2; // m/s^2 -// const Vector3 A(0, a, 0), W(0, 0.1, 0); -// const AcceleratingScenario scenario(nRb, P0, V0, A, W); -// -// const double T = 3; // seconds -// ScenarioRunner runner(&scenario, defaultParams(), T / 10); -// -// auto pim = runner.integrate(T); -// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); -// -// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); +// auto pim = runner.integrate(T, +// kNonZeroBias); +// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, +// kNonZeroBias); // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); //} +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + 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), 0.1)); +} + /* ************************************************************************* */ int main() { TestResult tr;