diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp index 655d061c4..1d0fbb34b 100644 --- a/gtsam/navigation/AggregateImuReadings.cpp +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include @@ -49,9 +48,8 @@ static Eigen::Block dV(constV9& v) { return v.segment<3>(6); } // See extensive discussion in ImuFactor.lyx Vector9 AggregateImuReadings::UpdateEstimate( const Vector9& zeta, const Vector3& correctedAcc, - const Vector3& correctedOmega, double dt, bool useExactDexpDerivative, - OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, - OptionalJacobian<9, 3> C) { + const Vector3& correctedOmega, double dt, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { using namespace sugar; const Vector3 a_dt = correctedAcc * dt; @@ -62,20 +60,6 @@ Vector9 AggregateImuReadings::UpdateEstimate( const auto theta = dR(zeta); const Matrix3 R = Rot3::Expmap(theta, H).matrix(); - Matrix3 D_invHwdt_theta = Z_3x3; - if (A) { - if (useExactDexpDerivative) { - boost::function f = - [w_dt](const Vector3& theta) { - return Rot3::ExpmapDerivative(theta).inverse() * w_dt; - }; - D_invHwdt_theta = numericalDerivative11(f, theta); - } else { - // First order (small angle) approximation of derivative of invH*w*dt: - D_invHwdt_theta = skewSymmetric(-0.5 * w_dt); - } - } - Vector9 zeta_plus; const double dt2 = 0.5 * dt; const Vector3 Radt = R * a_dt; @@ -89,7 +73,8 @@ Vector9 AggregateImuReadings::UpdateEstimate( const Matrix3 D_Radt_theta = R * skewSymmetric(-a_dt) * H; A->setIdentity(); - A->block<3, 3>(0, 0) += D_invHwdt_theta; + // First order (small angle) approximation of derivative of invH*w*dt: + A->block<3, 3>(0, 0) -= skewSymmetric(0.5 * w_dt); A->block<3, 3>(3, 0) = D_Radt_theta * dt2; A->block<3, 3>(3, 6) = I_3x3 * dt; A->block<3, 3>(6, 0) = D_Radt_theta; @@ -102,17 +87,15 @@ Vector9 AggregateImuReadings::UpdateEstimate( void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, - double dt, - bool useExactDexpDerivative) { + double dt) { // Correct measurements const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); // Do exact mean propagation Matrix9 A; - Matrix93 Ba, Bw; - zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt, - useExactDexpDerivative, A, Ba, Bw); + Matrix93 B, C; + zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt, A, B, C); // propagate uncertainty // TODO(frank): use noiseModel routine so we can have arbitrary noise models. @@ -120,8 +103,8 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, const Matrix3& a = p_->accelerometerCovariance; // TODO(frank): use Eigen-tricks for symmetric matrices cov_ = A * cov_ * A.transpose(); - cov_ += Bw * (w / dt) * Bw.transpose(); - cov_ += Ba * (a / dt) * Ba.transpose(); + cov_ += B * (a / dt) * B.transpose(); + cov_ += C * (w / dt) * C.transpose(); // increment counter and time k_ += 1; diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h index fd3b4d10e..691a162bc 100644 --- a/gtsam/navigation/AggregateImuReadings.h +++ b/gtsam/navigation/AggregateImuReadings.h @@ -60,8 +60,7 @@ class AggregateImuReadings { * TODO(frank): put useExactDexpDerivative in params */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt, - bool useExactDexpDerivative = false); + const Vector3& measuredOmega, double dt); /// Predict state at time j NavState predict(const NavState& state_i, const Bias& estimatedBias_i, @@ -79,7 +78,6 @@ class AggregateImuReadings { static Vector9 UpdateEstimate(const Vector9& zeta, const Vector3& correctedAcc, const Vector3& correctedOmega, double dt, - bool useExactDexpDerivative = false, OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> C = boost::none); diff --git a/gtsam/navigation/functors.h b/gtsam/navigation/functors.h deleted file mode 100644 index 36d87ac7b..000000000 --- a/gtsam/navigation/functors.h +++ /dev/null @@ -1,154 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file functors.h - * @brief Functors for use in Navigation factors - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -namespace gtsam { - -// Implement Rot3::ExpmapDerivative(omega) * theta, with derivatives -static Vector3 correctWithExpmapDerivative( - const Vector3& omega, const Vector3& theta, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) { - using std::sin; - const double angle2 = omega.dot(omega); // rotation angle, squared - if (angle2 <= std::numeric_limits::epsilon()) { - if (H1) *H1 = 0.5 * skewSymmetric(theta); - if (H2) *H2 = I_3x3; - return theta; - } - - const double angle = std::sqrt(angle2); // rotation angle - const double s1 = sin(angle) / angle; - const double s2 = sin(angle / 2.0); - const double a = 2.0 * s2 * s2 / angle2; - const double b = (1.0 - s1) / angle2; - - const Vector3 omega_x_theta = omega.cross(theta); - const Vector3 yt = a * omega_x_theta; - - const Matrix3 W = skewSymmetric(omega); - const Vector3 omega_x_omega_x_theta = omega.cross(omega_x_theta); - const Vector3 yyt = b * omega_x_omega_x_theta; - - if (H1) { - Matrix13 omega_t = omega.transpose(); - const Matrix3 T = skewSymmetric(theta); - const double Da = (s1 - 2.0 * a) / angle2; - const double Db = (3.0 * s1 - cos(angle) - 2.0) / angle2 / angle2; - *H1 = (-Da * omega_x_theta + Db * omega_x_omega_x_theta) * omega_t + a * T - - b * skewSymmetric(omega_x_theta) - b * W * T; - } - if (H2) *H2 = I_3x3 - a* W + b* W* W; - - return theta - yt + yyt; -} - -// theta(k+1) = theta(k) + inverse(H)*omega dt -// omega = (H/dt_)*(theta(k+1) - H*theta(k)) -// TODO(frank): make linear expression -class PredictAngularVelocity { - private: - double dt_; - - public: - typedef Vector3 result_type; - - PredictAngularVelocity(double dt) : dt_(dt) {} - - Vector3 operator()(const Vector3& theta, const Vector3& theta_plus, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - // TODO(frank): take into account derivative of ExpmapDerivative - const Vector3 predicted = (theta_plus - theta) / dt_; - Matrix3 D_c_t, D_c_p; - const Vector3 corrected = - correctWithExpmapDerivative(theta, predicted, D_c_t, D_c_p); - if (H1) *H1 = D_c_t - D_c_p / dt_; - if (H2) *H2 = D_c_p / dt_; - return corrected; - } -}; - -// TODO(frank): make linear expression -static Vector3 averageVelocity(const Vector3& vel, const Vector3& vel_plus, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) { - // TODO(frank): take into account derivative of ExpmapDerivative - if (H1) *H1 = 0.5 * I_3x3; - if (H2) *H2 = 0.5 * I_3x3; - return 0.5 * (vel + vel_plus); -} - -// pos(k+1) = pos(k) + average_velocity * dt -// TODO(frank): make linear expression -class PositionDefect { - private: - double dt_; - - public: - typedef Vector3 result_type; - - PositionDefect(double dt) : dt_(dt) {} - - Vector3 operator()(const Vector3& pos, const Vector3& pos_plus, - const Vector3& average_velocity, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 3> H3 = boost::none) const { - // TODO(frank): take into account derivative of ExpmapDerivative - if (H1) *H1 = I_3x3; - if (H2) *H2 = -I_3x3; - if (H3) *H3 = I_3x3* dt_; - return (pos + average_velocity * dt_) - pos_plus; - } -}; - -// vel(k+1) = vel(k) + Rk * acc * dt -// acc = Rkt * [vel(k+1) - vel(k)]/dt -// TODO(frank): take in Rot3 -class PredictAcceleration { - private: - double dt_; - - public: - typedef Vector3 result_type; - - PredictAcceleration(double dt) : dt_(dt) {} - - Vector3 operator()(const Vector3& vel, const Vector3& vel_plus, - const Vector3& theta, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 3> H3 = boost::none) const { - Matrix3 D_R_theta; - // TODO(frank): D_R_theta is ExpmapDerivative (computed again) - Rot3 nRb = Rot3::Expmap(theta, D_R_theta); - Vector3 n_acc = (vel_plus - vel) / dt_; - Matrix3 D_b_R, D_b_n; - Vector3 b_acc = nRb.unrotate(n_acc, D_b_R, D_b_n); - if (H1) *H1 = -D_b_n / dt_; - if (H2) *H2 = D_b_n / dt_; - if (H3) *H3 = D_b_R* D_R_theta; - return b_acc; - } -}; - -} // namespace gtsam diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp index 388d0164b..7638a88be 100644 --- a/gtsam/navigation/tests/testAggregateImuReadings.cpp +++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert */ -#include #include #include @@ -40,176 +39,22 @@ static boost::shared_ptr defaultParams() { } /* ************************************************************************* */ -TEST(AggregateImuReadings, CorrectWithExpmapDerivative1) { - Matrix aH1, aH2; - boost::function f = boost::bind( - correctWithExpmapDerivative, _1, _2, boost::none, boost::none); - for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { - for (Vector3 theta : - {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { - Matrix3 H = Rot3::ExpmapDerivative(omega); - Vector3 expected = H * theta; - EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); - EXPECT(assert_equal(expected, - correctWithExpmapDerivative(omega, theta, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); - EXPECT(assert_equal(H, aH2)); - } - } -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, CorrectWithExpmapDerivative2) { - Matrix aH1, aH2; - boost::function f = boost::bind( - correctWithExpmapDerivative, _1, _2, boost::none, boost::none); - const Vector3 omega(0, 0, 0); - for (Vector3 theta : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { - Matrix3 H = Rot3::ExpmapDerivative(omega); - Vector3 expected = H * theta; - EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); - EXPECT(assert_equal(expected, - correctWithExpmapDerivative(omega, theta, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); - EXPECT(assert_equal(H, aH2)); - } -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, CorrectWithExpmapDerivative3) { - Matrix aH1, aH2; - boost::function f = boost::bind( - correctWithExpmapDerivative, _1, _2, boost::none, boost::none); - const Vector3 omega(0.1, 0.2, 0.3), theta(0.4, 0.3, 0.2); - Matrix3 H = Rot3::ExpmapDerivative(omega); - Vector3 expected = H * theta; - EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); - EXPECT(assert_equal(expected, - correctWithExpmapDerivative(omega, theta, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); - EXPECT(assert_equal(H, aH2)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, PredictAngularVelocity1) { - Matrix aH1, aH2; - PredictAngularVelocity functor(kDt); - boost::function f = - boost::bind(functor, _1, _2, boost::none, boost::none); - const Vector3 theta(0, 0, 0), theta_plus(0.4, 0.3, 0.2); - EXPECT(assert_equal(Vector3(4, 3, 2), functor(theta, theta_plus, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, PredictAngularVelocity2) { - Matrix aH1, aH2; - PredictAngularVelocity functor(kDt); - boost::function f = - boost::bind(functor, _1, _2, boost::none, boost::none); - const Vector3 theta(0.1, 0.2, 0.3), theta_plus(0.4, 0.3, 0.2); - EXPECT( - assert_equal(Vector3(Rot3::ExpmapDerivative(theta) * Vector3(3, 1, -1)), - functor(theta, theta_plus, aH1, aH2), 1e-5)); - EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, AverageVelocity) { - Matrix aH1, aH2; - boost::function f = - boost::bind(averageVelocity, _1, _2, boost::none, boost::none); - const Vector3 v(1, 2, 3), v_plus(3, 2, 1); - EXPECT(assert_equal(Vector3(2, 2, 2), averageVelocity(v, v_plus, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, v, v_plus), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, v, v_plus), aH2)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, PositionDefect) { - Matrix aH1, aH2, aH3; - PositionDefect functor(kDt); - boost::function f = - boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); - const Vector3 pos(1, 2, 3), pos_plus(2, 4, 6); - const Vector3 avg(10, 20, 30); - EXPECT(assert_equal(Vector3(0, 0, 0), - functor(pos, pos_plus, avg, aH1, aH2, aH3))); - EXPECT(assert_equal(numericalDerivative31(f, pos, pos_plus, avg), aH1)); - EXPECT(assert_equal(numericalDerivative32(f, pos, pos_plus, avg), aH2)); - EXPECT(assert_equal(numericalDerivative33(f, pos, pos_plus, avg), aH3)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, PredictAcceleration1) { - Matrix aH1, aH2, aH3; - PredictAcceleration functor(kDt); - boost::function f = - boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); - const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6); - const Vector3 theta(0, 0, 0); - EXPECT(assert_equal(Vector3(10, 20, 30), - functor(vel, vel_plus, theta, aH1, aH2, aH3))); - EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1)); - EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2)); - EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, PredictAcceleration2) { - Matrix aH1, aH2, aH3; - PredictAcceleration functor(kDt); - boost::function f = - boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); - const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6); - const Vector3 theta(0.1, 0.2, 0.3); - EXPECT(assert_equal(Vector3(10, 20, 30), - functor(vel, vel_plus, theta, aH1, aH2, aH3))); - EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1)); - EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2)); - EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3)); -} - -/* ************************************************************************* */ -TEST(AggregateImuReadings, UpdateEstimate1) { +TEST(AggregateImuReadings, UpdateEstimate) { AggregateImuReadings pim(defaultParams()); Matrix9 aH1; Matrix93 aH2, aH3; boost::function f = - boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, false, + boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, boost::none, boost::none, boost::none); 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, false, aH1, aH2, aH3); + pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-5)); EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-5)); } -/* ************************************************************************* */ -TEST(AggregateImuReadings, UpdateEstimate2) { - // Using exact dexp derivatives is more expensive but much more accurate - AggregateImuReadings pim(defaultParams()); - Matrix9 aH1; - Matrix93 aH2, aH3; - boost::function f = - boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, true, - boost::none, boost::none, boost::none); - 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, true, aH1, aH2, aH3); - EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-8)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); - EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr;