more Monte Carlo

gtsam/navigation/tests/testImuFactor.cpp

@@ -117,14 +117,26 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
 Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
     const NavState& state, const imuBias::ConstantBias& bias, double dt,
     const Pose3& body_P_sensor, const Vector3& measuredAcc,
-    const Vector3& measuredOmega, size_t N = 10000) {
+    const Vector3& measuredOmega, size_t N = 10, size_t M = 1) {
   // Get mean prediction from "ground truth" measurements
   PreintegratedImuMeasurements pim1 = pim;
-  for (size_t k=0;k<10;k++)
+  for (size_t k=0;k<M;k++)
     pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
-  NavState mean = pim1.predict(state, bias);
+  NavState prediction = pim1.predict(state, bias);
+//  NavState prediction = pim1.deltaXij();
+  GTSAM_PRINT(prediction);
   cout << "pim1.preintMeasCov():" << endl;
-  cout << pim1.preintMeasCov() << endl;
+  cout << 1000000*pim1.preintMeasCov() << endl;
+
+  // Below we will call xi = sampled.localCoordinates(prediction) to get a vector, which is
+  //   h = between(g); // derivatives inlined below
+  //   xi = Local(h, D_v_h);
+  // with derivatives
+  //   D_xi_sampled = - D_v_h * h.inverse().AdjointMap();
+  //   D_xi_predict = D_v_h;
+  // But with h near identity they are
+  //   D_xi_sampled = - I_3x3;
+  //   D_xi_predict = I_3x3;
 
   // Do a Monte Carlo analysis to determine empirical density on the predicted state
   Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar/dt)));
@@ -133,14 +145,18 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
   Vector9 sum = Vector9::Zero();
   for (size_t i = 0; i < N; i++) {
     PreintegratedImuMeasurements pim2 = pim;
-    for (size_t k=0;k<10;k++) {
+    for (size_t k=0;k<M;k++) {
       Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
       Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
       pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
     }
-    NavState prediction = pim2.predict(state, bias);
+    NavState sampled = pim2.predict(state, bias);
-    samples.col(i) = mean.localCoordinates(prediction);
+//    NavState sampled = pim2.deltaXij();
-    sum += samples.col(i);
+    Vector9 xi = sampled.localCoordinates(prediction);
+    GTSAM_PRINT(sampled);
+    cout << xi.transpose() << endl;
+    samples.col(i) = xi;
+    sum += xi;
   }
   Vector9 sampleMean = sum / N;
   cout << ":" << endl;
@@ -149,13 +165,14 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
   Q.setZero();
   for (size_t i = 0; i < N; i++) {
     Vector9 xi = samples.col(i);
+#define SUBTRACT_SAMPLE_MEAN
 #ifdef SUBTRACT_SAMPLE_MEAN
     xi -= sampleMean;
 #endif
     Q += xi * xi.transpose() / (N - 1);
   }
   cout << "Q:" << endl;
-  cout << Q << endl;
+  cout << 1000000*Q << endl;
   return Q;
 }
 
@@ -163,7 +180,7 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
 TEST(ImuFactor, StraightLine) {
   // Set up IMU measurements
   static const double g = 10; // make gravity 10 to make this easy to check
-  static const double v = 5.0, a = 0.2, dt = 3.0;
+  static const double v = 50.0, a = 0.2, dt = 3.0;
   const double dt22 = dt * dt / 2;
 
   // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
@@ -183,12 +200,26 @@ TEST(ImuFactor, StraightLine) {
   p->accelerometerCovariance = kMeasuredAccCovariance;
   p->gyroscopeCovariance = kMeasuredOmegaCovariance;
 
+  // Check G1 and G2 derivatives of pim.update
+
   // Now, preintegrate for 3 seconds, in 10 steps
   PreintegratedImuMeasurements pim(p, kZeroBiasHat);
   for (size_t i = 0; i < 10; i++)
     pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
 
-  // Get mean prediction from "ground truth" measurements
+  Matrix93 aG1,aG2;
+  boost::function<NavState(const Vector3&, const Vector3&)> f =
+      boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10,
+          boost::none, boost::none, boost::none);
+  pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2);
+  EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
+  EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
+  cout << "aG1" << endl;
+  cout << aG1 << endl;
+  cout << "aG2:" << endl;
+  cout << aG2 << endl;
+
+  // Do Monte-Carlo analysis
   PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance,
       p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise
   Matrix9 Q = MonteCarlo(pimMC, state1, kZeroBias, dt/10, Pose3(), measuredAcc,