ConcurrentBatchFilter.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 ConcurrentBatchFilter : public ConcurrentFilter {

public:
  typedef std::shared_ptr<ConcurrentBatchFilter> shared_ptr;
  typedef ConcurrentFilter Base; 

  struct Result {
    size_t iterations; 
    size_t lambdas; 
    size_t nonlinearVariables; 
    size_t linearVariables; 

    std::vector<size_t> newFactorsIndices;

    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; }
  };

  ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}

  ~ConcurrentBatchFilter() override = default;

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

  bool equals(const ConcurrentFilter& 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<FastList<Key> >& keysToMove = {}, const std::optional< std::vector<size_t> >& removeFactorIndices = {});

  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:

  LevenbergMarquardtParams parameters_;  
  NonlinearFactorGraph factors_;  
  Values theta_;  
  Ordering ordering_; 
  VectorValues delta_; 
  std::queue<size_t> availableSlots_; 
  Values separatorValues_; 
  std::vector<size_t> separatorSummarizationSlots_;  

  // Storage for information from the Smoother
  NonlinearFactorGraph smootherSummarization_; 
  NonlinearFactorGraph smootherShortcut_; 

  // Storage for information to be sent to the smoother
  NonlinearFactorGraph filterSummarization_; 
  NonlinearFactorGraph smootherFactors_;  
  Values smootherValues_; 

private:

  std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);

  void removeFactors(const std::vector<size_t>& slots);

  void reorder(const std::optional<FastList<Key> >& keysToMove = {});

  void moveSeparator(const FastList<Key>& keysToMove);

  static void optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
       VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters,
       Result& result);

  static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
      const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
      const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
      const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
      const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static void PrintLinearFactorGraph(const GaussianFactorGraph& factors,
      const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  template<class Container>
  static void PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter = DefaultKeyFormatter);

}; // ConcurrentBatchFilter

template<class Container>
void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
  std::cout << indent << title;
  for(Key key: keys) {
    std::cout << " " << keyFormatter(key);
  }
  std::cout << std::endl;
}

typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult;

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

} // \ namespace gtsam