ConcurrentBatchSmoother.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/LevenbergMarquardtOptimizer.h>
#include <queue>
 
namespace gtsam {
 
class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
 
public:
  typedef std::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
  typedef ConcurrentSmoother Base; 
 
  struct Result {
    size_t iterations; 
    size_t lambdas; 
    size_t nonlinearVariables; 
    size_t linearVariables; 
    double error; 
 
    Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}
 
    size_t getIterations() const { return iterations; }
    size_t getLambdas() const { return lambdas; }
    size_t getNonlinearVariables() const { return nonlinearVariables; }
    size_t getLinearVariables() const { return linearVariables; }
    double getError() const { return error; }
  };
 
  ConcurrentBatchSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}
 
  ~ConcurrentBatchSmoother() override {}
 
  void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
  bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
 
  const NonlinearFactorGraph& getFactors() const {
    return factors_;
  }
 
  const Values& getLinearizationPoint() const {
    return theta_;
  }
 
  const Ordering& getOrdering() const {
    return ordering_;
  }
 
  const VectorValues& getDelta() const {
    return delta_;
  }
 
  Values calculateEstimate() const {
    return theta_.retract(delta_);
  }
 
  template<class VALUE>
  VALUE calculateEstimate(Key key) const {
    const Vector delta = delta_.at(key);
    return theta_.at<VALUE>(key).retract(delta);
  }
 
  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
      const std::optional< std::vector<size_t> >& removeFactorIndices = {});
 
  void presync() override;
 
  void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
 
  void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
 
  void postsync() override;
 
protected:
 
  LevenbergMarquardtParams parameters_;  
  NonlinearFactorGraph factors_;  
  Values theta_;  
  Ordering ordering_; 
  VectorValues delta_; 
  VariableIndex variableIndex_; 
  std::queue<size_t> availableSlots_; 
  Values separatorValues_; 
  std::vector<size_t> filterSummarizationSlots_;  
 
  // Storage for information to be sent to the filter
  NonlinearFactorGraph smootherSummarization_; 
 
private:
 
  std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
 
  void removeFactors(const std::vector<size_t>& slots);
 
  void reorder();
 
  Result optimize();
 
  void updateSmootherSummarization();
 
  static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
      const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
  static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
      const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
}; // ConcurrentBatchSmoother
 
typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult;
 
template<>
struct traits<ConcurrentBatchSmoother> : public Testable<ConcurrentBatchSmoother> {
};
 
} //\ namespace gtsam