BatchFixedLagSmoother.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/LevenbergMarquardtOptimizer.h>
#include <queue>
 
namespace gtsam {
 
class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
 
public:
 
  typedef boost::shared_ptr<BatchFixedLagSmoother> shared_ptr;
 
  BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
    FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { };
 
  ~BatchFixedLagSmoother() override { };
 
  void print(const std::string& s = "BatchFixedLagSmoother:\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 theta_.retract(delta_);
  }
 
  template<class VALUE>
  VALUE calculateEstimate(Key key) const {
    const Vector delta = delta_.at(key);
    return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
  }
 
  const LevenbergMarquardtParams& params() const {
    return parameters_;
  }
 
  LevenbergMarquardtParams& params() {
    return parameters_;
  }
 
  const NonlinearFactorGraph& getFactors() const {
    return factors_;
  }
 
  const Values& getLinearizationPoint() const {
    return theta_;
  }
 
  const Ordering& getOrdering() const {
    return ordering_;
  }
 
  const VectorValues& getDelta() const {
    return delta_;
  }
 
  Matrix marginalCovariance(Key key) const;
 
  static GaussianFactorGraph CalculateMarginalFactors(
      const GaussianFactorGraph& graph, const KeyVector& keys,
      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
 
  static NonlinearFactorGraph CalculateMarginalFactors(
      const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
 
protected:
 
  typedef std::map<Key, std::set<Key> > FactorIndex;
 
  LevenbergMarquardtParams parameters_;
 
  bool enforceConsistency_;
 
  NonlinearFactorGraph factors_;
 
  Values theta_;
 
  Values linearKeys_;
 
  Ordering ordering_;
 
  VectorValues delta_;
 
  std::queue<size_t> availableSlots_;
 
  FactorIndex factorIndex_;
 
  void insertFactors(const NonlinearFactorGraph& newFactors);
 
  void removeFactors(const std::set<size_t>& deleteFactors);
 
  void eraseKeys(const KeyVector& keys);
 
  void reorder(const KeyVector& marginalizeKeys = KeyVector());
 
  Result optimize();
 
  void marginalize(const KeyVector& marginalizableKeys);
 
private:
  static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
  static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
  static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
  static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
  static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
}; // BatchFixedLagSmoother
 
}