From c81f91999df337e5a17215d2466695f5dd0b9165 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:17:25 -0800 Subject: [PATCH] Trying to avoid mallocs --- gtsam/navigation/AggregateImuReadings.cpp | 53 +++++++++---------- gtsam/navigation/AggregateImuReadings.h | 10 ++-- .../tests/testAggregateImuReadings.cpp | 26 +++++---- 3 files changed, 45 insertions(+), 44 deletions(-) diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp index b32fcbdb1..cb6533ca9 100644 --- a/gtsam/navigation/AggregateImuReadings.cpp +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -33,30 +33,22 @@ AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr& p, // Tangent space sugar. namespace sugar { - static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } - -typedef const Vector9 constV9; -static Eigen::Block dR(constV9& v) { return v.segment<3>(0); } -static Eigen::Block dP(constV9& v) { return v.segment<3>(3); } -static Eigen::Block dV(constV9& v) { return v.segment<3>(6); } - } // namespace sugar // See extensive discussion in ImuFactor.lyx -Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta, - const Vector3& a_body, - const Vector3& w_body, double dt, - OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, - OptionalJacobian<9, 3> C) { +void AggregateImuReadings::UpdateEstimate(const Vector3& a_body, + const Vector3& w_body, double dt, + Vector9* zeta, + OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { using namespace sugar; - const auto theta = dR(zeta); - const auto p = dP(zeta); - const auto v = dV(zeta); + const Vector3 theta = dR(*zeta); + const Vector3 v = dV(*zeta); // Calculate exact mean propagation Matrix3 H; @@ -65,10 +57,9 @@ Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta, const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; - Vector9 zeta_plus; - dR(zeta_plus) = theta + invH * w_body * dt; // theta - dP(zeta_plus) = p + v * dt + a_nav * dt22; // position - dV(zeta_plus) = v + a_nav * dt; // velocity + dR(*zeta) += invH * w_body * dt; // theta + dP(*zeta) += v * dt + a_nav * dt22; // position + dV(*zeta) += a_nav * dt; // velocity if (A) { // First order (small angle) approximation of derivative of invH*w: @@ -78,15 +69,21 @@ Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta, const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H; A->setIdentity(); - A->block<3, 3>(0, 0) += invHw_H_theta * dt; + A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; A->block<3, 3>(3, 6) = I_3x3 * dt; A->block<3, 3>(6, 0) = a_nav_H_theta * dt; } - if (B) *B << Z_3x3, R* dt22, R* dt; - if (C) *C << invH* dt, Z_3x3, Z_3x3; - - return zeta_plus; + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } } void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, @@ -99,7 +96,7 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, // Do exact mean propagation Matrix9 A; Matrix93 B, C; - zeta_ = UpdateEstimate(zeta_, a_body, w_body, dt, A, B, C); + UpdateEstimate(a_body, w_body, dt, &zeta_, A, B, C); // propagate uncertainty // TODO(frank): use noiseModel routine so we can have arbitrary noise models. @@ -108,8 +105,8 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, // TODO(frank): use Eigen-tricks for symmetric matrices cov_ = A * cov_ * A.transpose(); - cov_ += B * (aCov / dt) * B.transpose(); - cov_ += C * (wCov / dt) * C.transpose(); + cov_.noalias() += B * (aCov / dt) * B.transpose(); + cov_.noalias() += C * (wCov / dt) * C.transpose(); deltaTij_ += dt; } diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h index 38263542b..82c6cbdf0 100644 --- a/gtsam/navigation/AggregateImuReadings.h +++ b/gtsam/navigation/AggregateImuReadings.h @@ -76,11 +76,11 @@ class AggregateImuReadings { // Update integrated vector on tangent manifold zeta with acceleration // readings a_body and gyro readings w_body, bias-corrected in body frame. - static Vector9 UpdateEstimate(const Vector9& zeta, const Vector3& a_body, - const Vector3& w_body, double dt, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); + static void UpdateEstimate(const Vector3& a_body, const Vector3& w_body, + double dt, Vector9* zeta, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp index fc0c21760..86acf93ff 100644 --- a/gtsam/navigation/tests/testAggregateImuReadings.cpp +++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp @@ -38,19 +38,22 @@ static boost::shared_ptr defaultParams() { return p; } -boost::function f = - boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, - boost::none, boost::none, boost::none); +Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { + Vector9 zeta_plus = zeta; + AggregateImuReadings::UpdateEstimate(a, w, kDt, &zeta_plus); + return zeta_plus; +} /* ************************************************************************* */ TEST(AggregateImuReadings, UpdateEstimate1) { AggregateImuReadings pim(defaultParams()); - Matrix9 aH1; - Matrix93 aH2, aH3; + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); + Vector9 zeta2 = zeta; + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3); EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); @@ -59,12 +62,13 @@ TEST(AggregateImuReadings, UpdateEstimate1) { /* ************************************************************************* */ TEST(AggregateImuReadings, UpdateEstimate2) { AggregateImuReadings pim(defaultParams()); - Matrix9 aH1; - Matrix93 aH2, aH3; + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); + Vector9 zeta2 = zeta; + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));