int main(int argc, char** argv) {

  // Define the smoother lag (in seconds)
  double lag = 2.0;

  // Create a Concurrent Filter and Smoother
  ConcurrentBatchFilter concurrentFilter;
  ConcurrentBatchSmoother concurrentSmoother;

  // And a fixed lag smoother with a short lag
  BatchFixedLagSmoother fixedlagSmoother(lag);

  // And a fixed lag smoother with very long lag (i.e. a full batch smoother)
  BatchFixedLagSmoother batchSmoother(1000.0);


  // Create containers to store the factors and linearization points that
  // will be sent to the smoothers
  NonlinearFactorGraph newFactors;
  Values newValues;
  FixedLagSmoother::KeyTimestampMap newTimestamps;

  // Create a prior on the first pose, placing it at the origin
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  Key priorKey = 0;
  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
  newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
  newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

  // Now, loop through several time steps, creating factors from different "sensors"
  // and adding them to the fixed-lag smoothers
  double deltaT = 0.25;
  for(double time = 0.0+deltaT; time <= 5.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Unlike the fixed-lag versions, the concurrent filter implementation
    // requires the user to supply the specify which keys to move to the smoother
    FastList<Key> oldKeys;
    if(time >= lag+deltaT) {
      oldKeys.push_back(1000 * (time-lag-deltaT));
    }

    // Update the various inference engines
    concurrentFilter.update(newFactors, newValues, oldKeys);
    fixedlagSmoother.update(newFactors, newValues, newTimestamps);
    batchSmoother.update(newFactors, newValues, newTimestamps);

    // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
    if(fmod(time, 1.0) < 0.01) {
      // Synchronize the Filter and Smoother
      concurrentSmoother.update();
      synchronize(concurrentFilter, concurrentSmoother);
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
    fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate:  ");
    batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:      ");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }
  cout << "******************************************************************" << endl;
  cout << "All three versions should be identical." << endl;
  cout << "Adding a loop closure factor to the Batch version only." << endl;
  cout << "******************************************************************" << endl;
  cout << endl;

  // At the moment, all three versions produce the same output.
  // Now lets create a "loop closure" factor between the first pose and the current pose
  Key loopKey1(1000 * (0.0));
  Key loopKey2(1000 * (5.0));
  Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
  noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25));
  NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));

  // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
  // This measurement can never be added to the fixed-lag smoother, as one of the poses has been permanently marginalized out
  // This measurement can be incorporated into the full batch version though
  newFactors.push_back(loopFactor);
  batchSmoother.update(newFactors, Values(), FixedLagSmoother::KeyTimestampMap());
  newFactors.resize(0);



  // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
  for(double time = 5.0+deltaT; time <= 8.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Unlike the fixed-lag versions, the concurrent filter implementation
    // requires the user to supply the specify which keys to marginalize
    FastList<Key> oldKeys;
    if(time >= lag+deltaT) {
      oldKeys.push_back(1000 * (time-lag-deltaT));
    }

    // Update the various inference engines
    concurrentFilter.update(newFactors, newValues, oldKeys);
    fixedlagSmoother.update(newFactors, newValues, newTimestamps);
    batchSmoother.update(newFactors, newValues, newTimestamps);

    // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
    if(fmod(time, 1.0) < 0.01) {
      // Synchronize the Filter and Smoother
      concurrentSmoother.update();
      synchronize(concurrentFilter, concurrentSmoother);
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
    fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate:  ");
    batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:      ");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }
  cout << "******************************************************************" << endl;
  cout << "The Concurrent system and the Fixed-Lag Smoother should be " << endl;
  cout << "the same, but the Batch version has a loop closure." << endl;
  cout << "Adding the loop closure factor to the Concurrent version." << endl;
  cout << "This will not update the Concurrent Filter until the next " << endl;
  cout << "synchronization, but the Concurrent solution should be identical " << endl;
  cout << "to the Batch solution afterwards." << endl;
  cout << "******************************************************************" << endl;
  cout << endl;

  // The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
  newFactors.push_back(loopFactor);
  concurrentSmoother.update(newFactors, Values());
  newFactors.resize(0);


  // Now run for a few more seconds so the concurrent smoother and filter have to to re-sync
  // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
  for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Unlike the fixed-lag versions, the concurrent filter implementation
    // requires the user to supply the specify which keys to marginalize
    FastList<Key> oldKeys;
    if(time >= lag+deltaT) {
      oldKeys.push_back(1000 * (time-lag-deltaT));
    }

    // Update the various inference engines
    concurrentFilter.update(newFactors, newValues, oldKeys);
    fixedlagSmoother.update(newFactors, newValues, newTimestamps);
    batchSmoother.update(newFactors, newValues, newTimestamps);

    // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
    if(fmod(time, 1.0) < 0.01) {
      // Synchronize the Filter and Smoother
      concurrentSmoother.update();
      synchronize(concurrentFilter, concurrentSmoother);
      cout << "******************************************************************" << endl;
      cout << "Syncing Concurrent Filter and Smoother." << endl;
      cout << "******************************************************************" << endl;
      cout << endl;
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
    fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate:  ");
    batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:      ");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }


  // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
  cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
  cout << "  Concurrent Filter Keys: " << endl;
  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) {
    cout << setprecision(5) << "    Key: " << key_value.key << endl;
  }
  cout << "  Concurrent Smoother Keys: " << endl;
  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentSmoother.getLinearizationPoint()) {
    cout << setprecision(5) << "    Key: " << key_value.key << endl;
  }
  cout << "  Fixed-Lag Smoother Keys: " << endl;
  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, fixedlagSmoother.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << endl;
  }
  cout << "  Batch Smoother Keys: " << endl;
  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, batchSmoother.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << endl;
  }

  return 0;
}
void StateEstimator::optimizationLoop()
{
  ISAM2Params parameters;
  // parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
  // parameters.relinearizeSkip = 1; // Relinearize every time
  gtsam::IncrementalFixedLagSmoother graph(optLag_, parameters);

  double startTime;
  sensor_msgs::ImuConstPtr lastImu;
  double lastImuT;
  int imuKey = 1;
  int gpsKey = 1;

  // first we will initialize the graph with appropriate priors
  NonlinearFactorGraph newFactors;
  Values newVariables;
  FixedLagSmoother::KeyTimestampMap newTimestamps;

  sensor_msgs::NavSatFixConstPtr fix = gpsQ_.pop();
  startTime = ROS_TIME(fix);
  enu_.Reset(fix->latitude, fix->longitude, fix->altitude);

  sensor_msgs::ImuConstPtr imu = imuQ_.pop();
  lastImu = imu;
  lastImuT = ROS_TIME(imu) - 1 / imuFreq_;
  Rot3 initialOrientation =
      Rot3::Quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);

  // we set out initial position to the origin and assume we are stationary
  Pose3 x0(initialOrientation, Point3(0, 0, 0));
  PriorFactor<Pose3> priorPose(X(0), x0,
                               noiseModel::Diagonal::Sigmas((Vector(6) << priorOSigma_, priorOSigma_, priorOSigma_,
                                                             priorPSigma_, priorPSigma_, priorPSigma_)
                                                                .finished()));
  newFactors.add(priorPose);

  Vector3 v0 = Vector3(0, 0, 0);
  PriorFactor<Vector3> priorVel(
      V(0), v0, noiseModel::Diagonal::Sigmas((Vector(3) << priorVSigma_, priorVSigma_, priorVSigma_).finished()));
  newFactors.add(priorVel);

  imuBias::ConstantBias b0((Vector(6) << 0, 0, 0, 0, 0, 0).finished());
  PriorFactor<imuBias::ConstantBias> priorBias(
      B(0), b0,
      noiseModel::Diagonal::Sigmas(
          (Vector(6) << priorABias_, priorABias_, priorABias_, priorGBias_, priorGBias_, priorGBias_).finished()));
  newFactors.add(priorBias);

  noiseModel::Diagonal::shared_ptr imuToGpsFactorNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_).finished());
  newFactors.add(BetweenFactor<Pose3>(X(0), G(0), imuToGps_, imuToGpsFactorNoise));

  newVariables.insert(X(0), x0);
  newVariables.insert(V(0), v0);
  newVariables.insert(B(0), b0);
  newVariables.insert(G(0), x0.compose(imuToGps_));

  newTimestamps[X(0)] = 0;
  newTimestamps[G(0)] = 0;
  newTimestamps[V(0)] = 0;
  newTimestamps[B(0)] = 0;

  graph.update(newFactors, newVariables);  //, newTimestamps);

  Pose3 prevPose = prevPose_ = x0;
  Vector3 prevVel = prevVel_ = v0;
  imuBias::ConstantBias prevBias = prevBias_ = b0;

  // remove old imu messages
  while (!imuQ_.empty() && ROS_TIME(imuQ_.front()) < ROS_TIME(fix))
  {
    lastImuT = ROS_TIME(lastImu);
    lastImu = imuQ_.pop();
  }

  // setting up the IMU integration
  boost::shared_ptr<gtsam::PreintegrationParams> preintegrationParams =
      PreintegrationParams::MakeSharedU(gravityMagnitude_);
  preintegrationParams->accelerometerCovariance = accelSigma_ * I_3x3;
  preintegrationParams->gyroscopeCovariance = gyroSigma_ * I_3x3;
  preintegrationParams->integrationCovariance = imuIntSigma_ * I_3x3;

  PreintegratedImuMeasurements imuIntegrator(preintegrationParams, prevBias);

  Vector noiseModelBetweenBias =
      (Vector(6) << accelBSigma_, accelBSigma_, accelBSigma_, gyroBSigma_, gyroBSigma_, gyroBSigma_).finished();
  SharedDiagonal gpsNoise = noiseModel::Diagonal::Sigmas(Vector3(gpsSigma_, gpsSigma_, 3 * gpsSigma_));

  newFactors.resize(0);
  newVariables.clear();
  newTimestamps.clear();

  // now we loop and let use the queues to grab messages
  while (ros::ok())
  {
    bool optimize = false;

    // integrate imu messages
    while (!imuQ_.empty() && ROS_TIME(imuQ_.back()) > (startTime + 0.1 * imuKey) && !optimize)
    {
      double curTime = startTime + 0.1 * imuKey;
      // we reset the integrator, then integrate
      imuIntegrator.resetIntegrationAndSetBias(prevBias);

      while (ROS_TIME(lastImu) < curTime)
      {
        double dt = ROS_TIME(lastImu) - lastImuT;
        imuIntegrator.integrateMeasurement(
            Vector3(lastImu->linear_acceleration.x, lastImu->linear_acceleration.y, lastImu->linear_acceleration.z),
            Vector3(lastImu->angular_velocity.x, lastImu->angular_velocity.y, lastImu->angular_velocity.z), dt);
        lastImuT = ROS_TIME(lastImu);
        lastImu = imuQ_.pop();
      }

      // now put this into the graph
      ImuFactor imuf(X(imuKey - 1), V(imuKey - 1), X(imuKey), V(imuKey), B(imuKey - 1), imuIntegrator);
      newFactors.add(imuf);
      newFactors.add(BetweenFactor<imuBias::ConstantBias>(
          B(imuKey - 1), B(imuKey), imuBias::ConstantBias(),
          noiseModel::Diagonal::Sigmas(sqrt(imuIntegrator.deltaTij()) * noiseModelBetweenBias)));

      Rot3 orientation = Rot3::Quaternion(lastImu->orientation.w, lastImu->orientation.x, lastImu->orientation.y,
                                          lastImu->orientation.z);
      // std::cout << "adding orientation: " << orientation.xyz() << std::endl;
      newFactors.add(UnaryRotationFactor(X(imuKey), orientation.yaw(),
                                         noiseModel::Diagonal::Sigmas((Vector(1) << yawSigma_).finished())));

      NavState cur(prevPose, prevVel);
      NavState next = imuIntegrator.predict(cur, prevBias);
      prevPose = next.pose();
      prevVel = next.v();
      newVariables.insert(X(imuKey), prevPose);
      newVariables.insert(G(imuKey), prevPose.compose(imuToGps_));
      newVariables.insert(V(imuKey), prevVel);
      newVariables.insert(B(imuKey), prevBias);

      Pose3 temp = prevPose.compose(imuToGps_);
      std::cout << "imu(" << imuKey << "): " << temp.x() << " " << temp.y() << " " << temp.z() << std::endl;

      // for marginalizing out past the time window
      newTimestamps[X(imuKey)] = 0.1 * imuKey;
      newTimestamps[G(imuKey)] = 0.1 * imuKey;
      newTimestamps[V(imuKey)] = 0.1 * imuKey;
      newTimestamps[B(imuKey)] = 0.1 * imuKey;
      ++imuKey;
      optimize = true;
    }

    while (!gpsQ_.empty() && gpsKey < imuKey && optimize && ROS_TIME(gpsQ_.back()) > (startTime + gpsKey * 0.1))
    {
      fix = gpsQ_.pop();
      // we don't want all gps messages, just ones that are very close to the factors (10 hz)
      if (std::abs(ROS_TIME(fix) - (startTime + gpsKey * 0.1)) > 1e-2)
        continue;

      double E, N, U;
      enu_.Forward(fix->latitude, fix->longitude, fix->altitude, E, N, U);
      // we should maybe do a check on the GPS to make sure it's valid
      newFactors.add(GPSFactor(G(gpsKey), Point3(E, N, U), gpsNoise));
      newFactors.add(BetweenFactor<Pose3>(X(gpsKey), G(gpsKey), imuToGps_, imuToGpsFactorNoise));
      std::cout << "gps(" << gpsKey << "): " << E << " " << N << " " << U << std::endl;
      ++gpsKey;
    }

    if (!optimize)
      continue;

    try
    {
      graph.update(newFactors, newVariables);  //, newTimestamps);

      prevPose = graph.calculateEstimate<Pose3>(X(imuKey - 1));
      prevVel = graph.calculateEstimate<Vector3>(V(imuKey - 1));
      prevBias = graph.calculateEstimate<imuBias::ConstantBias>(B(imuKey - 1));

      // pass this to the other thread
      {
        std::lock_guard<std::mutex> lock(mutex_);
        prevPose_ = prevPose;
        prevVel_ = prevVel;
        prevBias_ = prevBias;
        currentTime_ = (imuKey - 1) * 0.1 + startTime;
        doneFirstOpt_ = true;
      }
    }
    catch (IndeterminantLinearSystemException ex)
    {
      // optimization blew up, not much to do just warn user
      ROS_ERROR("Indeterminant linear system error");
    }

    newFactors.resize(0);
    newVariables.clear();
    newTimestamps.clear();
  }
}
int main(int argc, char** argv) {

  // Define the smoother lag (in seconds)
  double lag = 2.0;

  // Create a fixed lag smoother
  // The Batch version uses Levenberg-Marquardt to perform the nonlinear optimization
  BatchFixedLagSmoother smootherBatch(lag);
  // The Incremental version uses iSAM2 to perform the nonlinear optimization
  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
  parameters.relinearizeSkip = 1; // Relinearize every time
  IncrementalFixedLagSmoother smootherISAM2(lag, parameters);

  // Create containers to store the factors and linearization points that
  // will be sent to the smoothers
  NonlinearFactorGraph newFactors;
  Values newValues;
  FixedLagSmoother::KeyTimestampMap newTimestamps;

  // Create a prior on the first pose, placing it at the origin
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  Key priorKey = 0;
  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
  newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
  newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

  // Now, loop through several time steps, creating factors from different "sensors"
  // and adding them to the fixed-lag smoothers
  double deltaT = 0.25;
  for(double time = deltaT; time <= 3.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Update the smoothers with the new factors
    smootherBatch.update(newFactors, newValues, newTimestamps);
    smootherISAM2.update(newFactors, newValues, newTimestamps);
    for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
      smootherISAM2.update();
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
    smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }

  // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
  cout << "After 3.0 seconds, " << endl;
  cout << "  Batch Smoother Keys: " << endl;
  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
  }
  cout << "  iSAM2 Smoother Keys: " << endl;
  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
  }

  return 0;
}