ConcurrentIncrementalFilter.h

/* ----------------------------------------------------------------------------
 
 * 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/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/ISAM2.h>
 
namespace gtsam {
 
class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual ConcurrentFilter {
 
public:
 
  typedef boost::shared_ptr<ConcurrentIncrementalFilter> shared_ptr;
  typedef ConcurrentFilter Base; 
 
  struct Result {
    size_t iterations; 
    size_t nonlinearVariables; 
    size_t linearVariables; 
    size_t variablesReeliminated;
    size_t variablesRelinearized;
 
    FactorIndices newFactorsIndices;
 
    double error; 
 
    Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
 
    size_t getIterations() const { return iterations; }
    size_t getNonlinearVariables() const { return nonlinearVariables; }
    size_t getLinearVariables() const { return linearVariables; }
    double getError() const { return error; }
  };
 
  ConcurrentIncrementalFilter(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};
 
  ~ConcurrentIncrementalFilter() override {};
 
  void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
  bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
 
  const NonlinearFactorGraph& getFactors() const {
    return isam2_.getFactorsUnsafe();
  }
 
  const ISAM2& getISAM2() const {
    return isam2_;
  }
 
  const Values& getLinearizationPoint() const {
    return isam2_.getLinearizationPoint();
  }
 
  const VectorValues& getDelta() const {
    return isam2_.getDelta();
  }
 
  Values calculateEstimate() const {
    return isam2_.calculateEstimate();
  }
 
  template<class VALUE>
  VALUE calculateEstimate(Key key) const {
    return isam2_.calculateEstimate<VALUE>(key);
  }
 
  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
      const boost::optional<FastList<Key> >& keysToMove = boost::none,
      const boost::optional< FactorIndices >& removeFactorIndices = boost::none);
 
  void presync() override;
 
  void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
 
  void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
 
  void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
 
  void postsync() override;
 
protected:
 
  ISAM2 isam2_; 
 
  // ???
  NonlinearFactorGraph previousSmootherSummarization_; 
  FactorIndices currentSmootherSummarizationSlots_;  
  NonlinearFactorGraph smootherShortcut_; 
 
  // Storage for information to be sent to the smoother
  NonlinearFactorGraph smootherFactors_;  
  Values smootherValues_; 
 
private:
 
  static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
 
  static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
 
  // TODO: Make this a static function
  void updateShortcut(const NonlinearFactorGraph& removedFactors);
 
  // TODO: Make this a static function
  NonlinearFactorGraph calculateFilterSummarization() const;
 
}; // ConcurrentBatchFilter
 
typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult;
 
template<>
struct traits<ConcurrentIncrementalFilter> : public Testable<ConcurrentIncrementalFilter> {
};
 
} //\ namespace gtsam