FixedLagSmoother.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */

 
// \callgraph
#pragma once
 
#include <gtsam/dllexport.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
 
#include <map>
#include <vector>
 
namespace gtsam {
 
class GTSAM_EXPORT FixedLagSmoother {
 
public:

  typedef std::shared_ptr<FixedLagSmoother> shared_ptr;

  typedef std::map<Key, double> KeyTimestampMap;
  typedef std::multimap<double, Key> TimestampKeyMap;

  // TODO: Think of some more things to put here
  struct Result {
    size_t iterations; 
    size_t intermediateSteps; 
    size_t nonlinearVariables; 
    size_t linearVariables; 
    double error; 
    FactorIndices marginalFactorIndices; 
    FactorIndices deletedFactorIndices; 
    KeySet keysOfDeletedNodes; 
    KeySet expiredPendingKeys;  
    Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}

    size_t getIterations() const { return iterations; }
    size_t getIntermediateSteps() const { return intermediateSteps; }
    size_t getNonlinearVariables() const { return nonlinearVariables; }
    size_t getLinearVariables() const { return linearVariables; }
    double getError() const { return error; }
    FactorIndices getMarginalFactorIndices() const { return marginalFactorIndices; }
    FactorIndices getDeletedFactorIndices() const { return deletedFactorIndices; }
    KeySet getKeysOfDeletedNodes() const { return keysOfDeletedNodes; }
    KeySet getExpiredPendingKeys() const { return expiredPendingKeys; }
    GTSAM_EXPORT void print() const;
  };

  FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }

  virtual ~FixedLagSmoother() { }

  virtual void print(
      const std::string& s = "FixedLagSmoother:\n",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;

  double smootherLag() const {
    return smootherLag_;
  }

  double& smootherLag() {
    return smootherLag_;
  }

  void setSmootherLag(double smootherLag) {
    smootherLag_ = smootherLag;
  }

  const KeyTimestampMap& timestamps() const {
    return keyTimestampMap_;
  }

  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
                        const Values& newTheta = Values(),
                        const KeyTimestampMap& timestamps = KeyTimestampMap(),
                        const FactorIndices& factorsToRemove = FactorIndices()) = 0;

  virtual Values calculateEstimate() const  = 0;

  virtual Values calculateEstimate(const KeyVector& keys) const {
    return calculateEstimate().extract(keys);
  }
 
 
protected:

  double smootherLag_;

  TimestampKeyMap timestampKeyMap_;
  KeyTimestampMap keyTimestampMap_;

  void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);

  void eraseKeyTimestampMap(const KeyVector& keys);

  double getCurrentTimestamp() const;

  KeyVector findKeysBefore(double timestamp) const;

  KeyVector findKeysAfter(double timestamp) const;
 
}; // FixedLagSmoother

typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap;
typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
typedef FixedLagSmoother::Result FixedLagSmootherResult;
 
}