IncrementalFixedLagSmoother.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_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/ISAM2.h>

namespace gtsam {

class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {

public:

  typedef std::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;

  IncrementalFixedLagSmoother(double smootherLag = 0.0,
      const ISAM2Params& parameters = DefaultISAM2Params()) :
      FixedLagSmoother(smootherLag), isam_(parameters) {
  }

  ~IncrementalFixedLagSmoother() override {
  }

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

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

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

  Values calculateEstimate() const override {
    return isam_.calculateEstimate();
  }

  template<class VALUE>
  VALUE calculateEstimate(Key key) const {
    return isam_.calculateEstimate<VALUE>(key);
  }

  const ISAM2Params& params() const {
    return isam_.params();
  }

  const NonlinearFactorGraph& getFactors() const {
    return isam_.getFactorsUnsafe();
  }

  const Values& getLinearizationPoint() const {
    return isam_.getLinearizationPoint();
  }

  const VectorValues& getDelta() const {
    return isam_.getDelta();
  }

  Matrix marginalCovariance(Key key) const {
    return isam_.marginalCovariance(key);
  }

  const ISAM2Result& getISAM2Result() const{ return isamResult_; }

  const ISAM2& getISAM2() const { return isam_; }

protected:

  static ISAM2Params DefaultISAM2Params() {
    ISAM2Params params;
    params.findUnusedFactorSlots = true;
    return params;
  }

  ISAM2 isam_;

  ISAM2Result isamResult_;

  void eraseKeysBefore(double timestamp);

  void createOrderingConstraints(const KeyVector& marginalizableKeys,
      std::optional<FastMap<Key, int> >& constrainedKeys) const;

private:
  static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
      "Keys:");
  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
  static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
      const std::string& label = "Factor Graph:");
  static void PrintSymbolicTree(const gtsam::ISAM2& isam,
      const std::string& label = "Bayes Tree:");
  static void PrintSymbolicTreeHelper(
      const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
          "");

};
// IncrementalFixedLagSmoother

}