HybridNonlinearISAM.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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
 
#include <optional>
 
namespace gtsam {
class GTSAM_EXPORT HybridNonlinearISAM {
 protected:
  gtsam::HybridGaussianISAM isam_;
 
  Values linPoint_;
 
  DiscreteValues assignment_;
 
  HybridNonlinearFactorGraph factors_;
 
  int reorderInterval_;
  int reorderCounter_;
 
  HybridGaussianFactorGraph::Eliminate eliminationFunction_;
 
 public:
 
  HybridNonlinearISAM(
      int reorderInterval = 1,
      const HybridGaussianFactorGraph::Eliminate& eliminationFunction =
          HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
      : reorderInterval_(reorderInterval),
        reorderCounter_(0),
        eliminationFunction_(eliminationFunction) {}
 
 
  Values estimate();
 
  // /** find the marginal covariance for a single variable */
  // Matrix marginalCovariance(Key key) const;
 
  // access
 
  const HybridGaussianISAM& bayesTree() const { return isam_; }
 
  void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); }
 
  const Values& getLinearizationPoint() const { return linPoint_; }
 
  const DiscreteValues& assignment() const { return assignment_; }
 
  const HybridNonlinearFactorGraph& getFactorsUnsafe() const {
    return factors_;
  }
 
  int reorderInterval() const { return reorderInterval_; }  
  int reorderCounter() const { return reorderCounter_; }    
 
  void print(const std::string& s = "",
             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
  void printStats() const;
 
  void saveGraph(const std::string& s,
                 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
 
  void update(const HybridNonlinearFactorGraph& newFactors,
              const Values& initialValues,
              const std::optional<size_t>& maxNrLeaves = {},
              const std::optional<Ordering>& ordering = {});
 
  void reorderRelinearize();
 
};
 
}  // namespace gtsam