NonlinearISAM.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/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianISAM.h>
 
namespace gtsam {
class GTSAM_EXPORT NonlinearISAM {
protected:
 
  gtsam::GaussianISAM isam_;
 
  Values linPoint_;
 
  NonlinearFactorGraph factors_;
 
  int reorderInterval_;
  int reorderCounter_;
 
  GaussianFactorGraph::Eliminate eliminationFunction_;
 
public:
 
 
  NonlinearISAM(int reorderInterval = 1,
    const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
  reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
 
 
  Values estimate() const;
 
  Matrix marginalCovariance(Key key) const;
 
  // access
 
  const GaussianISAM& bayesTree() const { return isam_; }
 
  const Values& getLinearizationPoint() const { return linPoint_; }
 
  const NonlinearFactorGraph& 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 NonlinearFactorGraph& newFactors, const Values& initialValues);
 
  void reorder_relinearize();
 
 
};
 
} // \namespace gtsam