Removed debug code

gtsam/navigation/AggregateImuReadings.cpp

@@ -103,7 +103,6 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
   const Matrix3& aCov = p_->accelerometerCovariance;
   const Matrix3& wCov = p_->gyroscopeCovariance;
 
-  // TODO(frank): use Eigen-tricks for symmetric matrices
   cov_ = A * cov_ * A.transpose();
   cov_.noalias() += B * (aCov / dt) * B.transpose();
   cov_.noalias() += C * (wCov / dt) * C.transpose();
@@ -118,20 +117,17 @@ NavState AggregateImuReadings::predict(const NavState& state_i,
   using namespace sugar;
   Vector9 zeta = zeta_;
 
-// Correct for initial velocity and gravity
-#if 1
+  // Correct for initial velocity and gravity
   Rot3 Ri = state_i.attitude();
   Matrix3 Rit = Ri.transpose();
   Vector3 gt = deltaTij_ * p_->n_gravity;
   dP(zeta) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
   dV(zeta) += Rit * gt;
-#endif
 
   return state_i.retract(zeta);
 }
 
 SharedGaussian AggregateImuReadings::noiseModel() const {
-#ifndef LOCALCOORDINATES_ONLY
   // Correct for application of retract, by calculating the retract derivative H
   // From NavState::retract:
   Matrix3 D_R_theta;
@@ -144,9 +140,6 @@ SharedGaussian AggregateImuReadings::noiseModel() const {
   // TODO(frank): theta() itself is noisy, so should we not correct for that?
   Matrix9 HcH = H * cov_ * H.transpose();
   return noiseModel::Gaussian::Covariance(HcH, false);
-#else
-  return noiseModel::Gaussian::Covariance(cov_, false);
-#endif
 }
 
 Matrix9 AggregateImuReadings::preintMeasCov() const {

gtsam/navigation/AggregateImuReadings.h

@@ -20,8 +20,6 @@
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 
-#define LOCALCOORDINATES_ONLY
-
 namespace gtsam {
 
 /**

gtsam/navigation/ScenarioRunner.cpp

@@ -65,12 +65,8 @@ 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);
-#ifndef LOCALCOORDINATES_ONLY
     NavState sampled = predict(pim);
     Vector9 xi = sampled.localCoordinates(prediction);
-#else
-    Vector9 xi = pim.zeta();
-#endif
     samples.col(i) = xi;
     sum += xi;
   }