Exact dexp derivative flag

gtsam/navigation/AggregateImuReadings.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/navigation/AggregateImuReadings.h>
+#include <gtsam/navigation/functors.h>
 #include <cmath>
 
 using namespace std;
@@ -52,8 +53,9 @@ static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
 
 Vector9 AggregateImuReadings::UpdateEstimate(
     const Vector9& zeta, const Vector3& correctedAcc,
-    const Vector3& correctedOmega, double dt, OptionalJacobian<9, 9> A,
+    const Vector3& correctedOmega, double dt, bool useExactDexpDerivative,
-    OptionalJacobian<9, 3> Ba, OptionalJacobian<9, 3> Bw) {
+    OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> Ba,
+    OptionalJacobian<9, 3> Bw) {
   using namespace sugar;
 
   const Vector3 a_dt = correctedAcc * dt;
@@ -61,25 +63,36 @@ Vector9 AggregateImuReadings::UpdateEstimate(
 
   // Calculate exact mean propagation
   Matrix3 D_R_theta;
-  const Rot3 R = Rot3::Expmap(dR(zeta), D_R_theta).matrix();
+  const auto theta = dR(zeta);
-  const Matrix3 invH = D_R_theta.inverse();
+  const Rot3 R = Rot3::Expmap(theta, D_R_theta).matrix();
+
+  Matrix3 D_invHwdt_theta, D_invHwdt_wdt;
+  Vector3 invHwdt;
+  if (useExactDexpDerivative) {
+    invHwdt = correctWithExpmapDerivative(
+        -theta, w_dt, A ? &D_invHwdt_theta : 0, A ? &D_invHwdt_wdt : 0);
+    if (A) D_invHwdt_theta *= -1;
+  } else {
+    const Matrix3 invH = D_R_theta.inverse();
+    invHwdt = invH * w_dt;
+    // First order (small angle) approximation of derivative of invH*w*dt:
+    if (A) D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
+    if (A) D_invHwdt_wdt = invH;
+  }
 
   Matrix3 D_Radt_R, D_Radt_adt;
   const Vector3 Radt = R.rotate(a_dt, A ? &D_Radt_R : 0, A ? &D_Radt_adt : 0);
 
   Vector9 zeta_plus;
   const double dt2 = 0.5 * dt;
-  dR(zeta_plus) = dR(zeta) + invH * w_dt;
+  dR(zeta_plus) = dR(zeta) + invHwdt;                     // theta
-  dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2;
+  dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2;  // position
-  dV(zeta_plus) = dV(zeta) + Radt;
+  dV(zeta_plus) = dV(zeta) + Radt;                        // velocity
 
   if (A) {
     // Exact derivative of R*a*dt with respect to theta:
     const Matrix3 D_Radt_theta = D_Radt_R * D_R_theta;
 
-    // First order (small angle) approximation of derivative of invH*w*dt:
-    const Matrix3 D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
-
     A->setIdentity();
     A->block<3, 3>(0, 0) += D_invHwdt_theta;
     A->block<3, 3>(3, 0) = D_Radt_theta * dt2;
@@ -87,14 +100,15 @@ Vector9 AggregateImuReadings::UpdateEstimate(
     A->block<3, 3>(6, 0) = D_Radt_theta;
   }
   if (Ba) *Ba << Z_3x3, D_Radt_adt* dt* dt2, D_Radt_adt* dt;
-  if (Bw) *Bw << invH* dt, Z_3x3, Z_3x3;
+  if (Bw) *Bw << D_invHwdt_wdt* dt, Z_3x3, Z_3x3;
 
   return zeta_plus;
 }
 
 void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
                                                 const Vector3& measuredOmega,
-                                                double dt) {
+                                                double dt,
+                                                bool useExactDexpDerivative) {
   // Correct measurements
   const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer();
   const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
@@ -102,10 +116,11 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
   // Do exact mean propagation
   Matrix9 A;
   Matrix93 Ba, Bw;
-  zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt, A, Ba, Bw);
+  zeta_ = UpdateEstimate(zeta_, correctedAcc, correctedOmega, dt,
+                         useExactDexpDerivative, A, Ba, Bw);
 
   // propagate uncertainty
-  // TODO(frank): specialize to diagonal and upper triangular views
+  // TODO(frank): use noiseModel power: covariance is very expensive !
   const Matrix3 w = gyroscopeNoiseModel_->covariance() / dt;
   const Matrix3 a = accelerometerNoiseModel_->covariance() / dt;
   cov_ = A * cov_ * A.transpose() + Bw * w * Bw.transpose() +
@@ -137,7 +152,6 @@ NavState AggregateImuReadings::predict(const NavState& state_i,
 
 SharedGaussian AggregateImuReadings::noiseModel() const {
   // Correct for application of retract, by calculating the retract derivative H
-  // We have inv(Rp'Rp) = H inv(Rz'Rz) H' => Rp = Rz * inv(H)
   // From NavState::retract:
   Matrix3 D_R_theta;
   const Matrix3 iRj = Rot3::Expmap(theta(), D_R_theta).matrix();
@@ -146,33 +160,10 @@ SharedGaussian AggregateImuReadings::noiseModel() const {
       Z_3x3, iRj.transpose(), Z_3x3,  //
       Z_3x3, Z_3x3, iRj.transpose();
 
+  // TODO(frank): theta() itself is noisy, so should we not correct for that?
+
   Matrix9 HcH = H * cov_ * H.transpose();
-  return noiseModel::Gaussian::Covariance(cov_, false);
+  return noiseModel::Gaussian::Covariance(HcH, false);
-
-  //  // Get covariance on zeta from Bayes Net, which stores P(zeta|bias) as a
-  //  // quadratic |R*zeta + S*bias -d|^2
-  //  Matrix RS;
-  //  Vector d;
-  //  boost::tie(RS, d) = posterior_k_->matrix();
-  //  // NOTEfrank): R'*R = inv(zetaCov)
-  //
-  //  Matrix9 R = RS.block<9, 9>(0, 0);
-  //  cout << "R'R" << endl;
-  //  cout << (R.transpose() * R).inverse() << endl;
-  //  cout << "cov" << endl;
-  //  cout << cov << endl;
-
-  //  // Rp = R * H.inverse(), implemented blockwise in-place below
-  //  // TODO(frank): yet another application of expmap and expmap derivative
-  //  // NOTE(frank): makes sense: a change in the j-frame has to be converted
-  //  // to a change in the i-frame, byy rotating with iRj. Similarly, a change
-  //  // in rotation nRj is mapped to a change in theta via the inverse dexp.
-  //  R.block<9, 3>(0, 0) *= D_R_theta.inverse();
-  //  R.block<9, 3>(0, 3) *= iRj;
-  //  R.block<9, 3>(0, 6) *= iRj;
-  //
-  //  // TODO(frank): think of a faster way - implement in noiseModel
-  //  return noiseModel::Gaussian::SqrtInformation(R, false);
 }
 
 Matrix9 AggregateImuReadings::preintMeasCov() const {

gtsam/navigation/AggregateImuReadings.h

@@ -56,9 +56,11 @@ class AggregateImuReadings {
    * @param measuredAcc Measured acceleration (in body frame)
    * @param measuredOmega Measured angular velocity (in body frame)
    * @param dt Time interval between this and the last IMU measurement
+   * TODO(frank): put useExactDexpDerivative in params
    */
   void integrateMeasurement(const Vector3& measuredAcc,
-                            const Vector3& measuredOmega, double dt);
+                            const Vector3& measuredOmega, double dt,
+                            bool useExactDexpDerivative = false);
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const Bias& estimatedBias_i,
@@ -72,9 +74,11 @@ class AggregateImuReadings {
   Matrix9 preintMeasCov() const;
 
   Vector3 theta() const { return zeta_.head<3>(); }
+
   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> Ba = boost::none,
                                 OptionalJacobian<9, 3> Bw = boost::none);

gtsam/navigation/ScenarioRunner.cpp

@@ -65,7 +65,7 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
   Vector9 sum = Vector9::Zero();
   for (size_t i = 0; i < N; i++) {
     auto pim = integrate(T, estimatedBias, true);
-#if 0
+#if 1
     NavState sampled = predict(pim);
     Vector9 xi = sampled.localCoordinates(prediction);
 #else

gtsam/navigation/tests/testAggregateImuReadings.cpp

@@ -170,22 +170,40 @@ TEST(AggregateImuReadings, PredictAcceleration2) {
 }
 
 /* ************************************************************************* */
-TEST(AggregateImuReadings, UpdateEstimate) {
+TEST(AggregateImuReadings, UpdateEstimate1) {
   AggregateImuReadings pim(defaultParams());
   Matrix9 aH1;
   Matrix93 aH2, aH3;
   boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f =
-      boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
+      boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, false,
                   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, aH1, aH2, aH3);
+  pim.UpdateEstimate(zeta, acc, omega, kDt, false, 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<Vector9(const Vector9&, const Vector3&, const Vector3&)> 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;