ConcurrentIncrementalSmoother.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/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/ISAM2.h>

namespace gtsam {

class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalSmoother : public virtual ConcurrentSmoother {

public:
  typedef boost::shared_ptr<ConcurrentIncrementalSmoother> shared_ptr;
  typedef ConcurrentSmoother Base; 

  struct Result {
    size_t iterations; 
    size_t nonlinearVariables; 
    size_t linearVariables; 
    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; }
  };

  ConcurrentIncrementalSmoother(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};

  virtual ~ConcurrentIncrementalSmoother() {};

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

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

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

  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<FactorIndices>& removeFactorIndices = boost::none);

  virtual void presync();

  virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);

  virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);

  virtual void postsync();

protected:

  ISAM2 isam2_; 

  // Storage for information received from the filter during the last synchronization
  NonlinearFactorGraph smootherFactors_; 
  Values smootherValues_; 
  NonlinearFactorGraph filterSummarizationFactors_; 
  Values separatorValues_; 
  FactorIndices filterSummarizationSlots_;  
  bool synchronizationUpdatesAvailable_; 

  // Storage for information to be sent to the filter
  NonlinearFactorGraph smootherSummarization_; 

private:

  void updateSmootherSummarization();

}; // ConcurrentBatchSmoother

typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult;

template<>
struct traits<ConcurrentIncrementalSmoother> : public Testable<ConcurrentIncrementalSmoother> {
};

} // \ namespace gtsam