Wrapped iSAM2

gtsam.h

@@ -1025,6 +1025,7 @@ class InvertedOrdering {
 class NonlinearFactorGraph {
 	NonlinearFactorGraph();
   void print(string s) const;
+  size_t size() const;
 	double error(const gtsam::Values& c) const;
 	double probPrime(const gtsam::Values& c) const;
 	gtsam::NonlinearFactor* at(size_t i) const;
@@ -1208,6 +1209,95 @@ virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
 	double lambda() const;
 };
 
+#include <gtsam/nonlinear/ISAM2.h>
+class ISAM2GaussNewtonParams {
+  ISAM2GaussNewtonParams();
+
+  void print(string str) const;
+
+  /** Getters and Setters for all properties */
+  double getWildfireThreshold() const;
+  void setWildfireThreshold(double wildfireThreshold);
+};
+
+class ISAM2DoglegParams {
+  ISAM2DoglegParams();
+
+  void print(string str) const;
+
+  /** Getters and Setters for all properties */
+  double getWildfireThreshold() const;
+  void setWildfireThreshold(double wildfireThreshold);
+  double getInitialDelta() const;
+  void setInitialDelta(double initialDelta);
+  string getAdaptationMode() const;
+  void setAdaptationMode(string adaptationMode);
+  bool isVerbose() const;
+  void setVerbose(bool verbose);
+};
+
+class ISAM2Params {
+  ISAM2Params();
+
+  void print(string str) const;
+
+  /** Getters and Setters for all properties */
+  void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params);
+  void setOptimizationParams(const gtsam::ISAM2DoglegParams& params);
+  void setRelinearizeThreshold(double relinearizeThreshold);
+  // TODO: wrap this
+  //void setRelinearizeThreshold(const FastMap<char,Vector>& relinearizeThreshold);
+  int getRelinearizeSkip() const;
+  void setRelinearizeSkip(int relinearizeSkip);
+  bool isEnableRelinearization() const;
+  void setEnableRelinearization(bool enableRelinearization);
+  bool isEvaluateNonlinearError() const;
+  void setEvaluateNonlinearError(bool evaluateNonlinearError);
+  string getFactorization() const;
+  void setFactorization(string factorization);
+  bool isCacheLinearizedFactors() const;
+  void setCacheLinearizedFactors(bool cacheLinearizedFactors);
+  bool isEnableDetailedResults() const;
+  void setEnableDetailedResults(bool enableDetailedResults);
+  bool isEnablePartialRelinearizationCheck() const;
+  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck);
+};
+
+class ISAM2Result {
+  ISAM2Result();
+
+  void print(string str) const;
+
+  /** Getters and Setters for all properties */
+  size_t getVariablesRelinearized() const;
+  size_t getVariablesReeliminated() const;
+  size_t getCliques() const;
+};
+
+class ISAM2 {
+  ISAM2();
+  ISAM2(const gtsam::ISAM2Params& params);
+
+  bool equals(const gtsam::ISAM2& other, double tol) const;
+  void print(string s) const;
+
+  gtsam::ISAM2Result update();
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices);
+  // TODO: wrap the full version of update
+  //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
+  //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
+
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Values calculateEstimate() const;
+  gtsam::Values calculateBestEstimate() const;
+  gtsam::VectorValues getDelta() const;
+  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
+  gtsam::Ordering getOrdering() const;
+  gtsam::VariableIndex getVariableIndex() const;
+  gtsam::ISAM2Params params() const;
+};
+
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************

gtsam/nonlinear/ISAM2.cpp

@@ -20,6 +20,7 @@
 using namespace boost::assign;
 #include <boost/range/adaptors.hpp>
 #include <boost/range/algorithm.hpp>
+#include <boost/algorithm/string.hpp>
 
 #include <gtsam/base/timing.h>
 #include <gtsam/base/debug.h>
@@ -39,6 +40,48 @@ using namespace std;
 static const bool disableReordering = false;
 static const double batchThreshold = 0.65;
 
+/* ************************************************************************* */
+std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const {
+  std::string s;
+  switch (adaptationMode) {
+  case DoglegOptimizerImpl::SEARCH_EACH_ITERATION:   s = "SEARCH_EACH_ITERATION";  break;
+  case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION:  s = "ONE_STEP_PER_ITERATION"; break;
+  default:                                           s = "UNDEFINED";              break;
+  }
+  return s;
+}
+
+/* ************************************************************************* */
+DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const {
+  std::string s = adaptationMode;  boost::algorithm::to_upper(s);
+  if (s == "SEARCH_EACH_ITERATION")  return DoglegOptimizerImpl::SEARCH_EACH_ITERATION;
+  if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION;
+
+  /* default is SEARCH_EACH_ITERATION */
+  return DoglegOptimizerImpl::SEARCH_EACH_ITERATION;
+}
+
+/* ************************************************************************* */
+ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const {
+  std::string s = str;  boost::algorithm::to_upper(s);
+  if (s == "QR") return ISAM2Params::QR;
+  if (s == "CHOLESKY") return ISAM2Params::CHOLESKY;
+
+  /* default is CHOLESKY */
+  return ISAM2Params::CHOLESKY;
+}
+
+/* ************************************************************************* */
+std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const {
+  std::string s;
+  switch (value) {
+  case ISAM2Params::QR:         s = "QR"; break;
+  case ISAM2Params::CHOLESKY:   s = "CHOLESKY"; break;
+  default:                      s = "UNDEFINED"; break;
+  }
+  return s;
+}
+
 /* ************************************************************************* */
 ISAM2::ISAM2(const ISAM2Params& params):
     deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {

gtsam/nonlinear/ISAM2.h

@@ -40,6 +40,15 @@ struct ISAM2GaussNewtonParams {
   ISAM2GaussNewtonParams(
       double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold
   ) : wildfireThreshold(_wildfireThreshold) {}
+
+  void print(const std::string str = "") const {
+    std::cout << str << "type:              ISAM2GaussNewtonParams\n";
+    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
+    std::cout.flush();
+  }
+
+  double getWildfireThreshold() const { return wildfireThreshold; }
+  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
 };
 
 /**
@@ -62,6 +71,27 @@ struct ISAM2DoglegParams {
       bool _verbose = false ///< see ISAM2DoglegParams::verbose
   ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold),
   adaptationMode(_adaptationMode), verbose(_verbose) {}
+
+  void print(const std::string str = "") const {
+    std::cout << str << "type:              ISAM2DoglegParams\n";
+    std::cout << str << "initialDelta:      " << initialDelta << "\n";
+    std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
+    std::cout << str << "adaptationMode:    " << adaptationModeTranslator(adaptationMode) << "\n";
+    std::cout.flush();
+  }
+
+  double getInitialDelta() const { return initialDelta; }
+  double getWildfireThreshold() const { return wildfireThreshold; }
+  std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); };
+  bool isVerbose() const { return verbose; };
+
+  void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; }
+  void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; }
+  void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); }
+  void setVerbose(bool verbose) { this->verbose = verbose; };
+
+  std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const;
+  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const;
 };
 
 /**
@@ -147,8 +177,57 @@ struct ISAM2Params {
       evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
       cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
       enableDetailedResults(false), enablePartialRelinearizationCheck(false) {}
+
+  void print(const std::string& str = "") const {
+    std::cout << str << "\n";
+    if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
+      boost::get<ISAM2GaussNewtonParams>(optimizationParams).print("optimizationParams:                ");
+    else if(optimizationParams.type() == typeid(ISAM2DoglegParams))
+      boost::get<ISAM2DoglegParams>(optimizationParams).print("optimizationParams:                ");
+    else
+      std::cout << "optimizationParams:                " << "{unknown type}" << "\n";
+    if(relinearizeThreshold.type() == typeid(double))
+      std::cout << "relinearizeThreshold:              " << boost::get<double>(relinearizeThreshold) << "\n";
+    else
+      std::cout << "relinearizeThreshold:              " << "{mapped}" << "\n";
+    std::cout << "relinearizeSkip:                   " << relinearizeSkip << "\n";
+    std::cout << "enableRelinearization:             " << enableRelinearization << "\n";
+    std::cout << "evaluateNonlinearError:            " << evaluateNonlinearError << "\n";
+    std::cout << "factorization:                     " << factorizationTranslator(factorization) << "\n";
+    std::cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors << "\n";
+    std::cout << "enableDetailedResults:             " << enableDetailedResults << "\n";
+    std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n";
+    std::cout.flush();
+  }
+
+   /** Getters and Setters for all properties */
+  OptimizationParams getOptimizationParams() const { return this->optimizationParams; }
+  RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; }
+  int getRelinearizeSkip() const { return relinearizeSkip; }
+  bool isEnableRelinearization() const { return enableRelinearization; }
+  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
+  std::string getFactorization() const { return factorizationTranslator(factorization); }
+  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
+  KeyFormatter getKeyFormatter() const { return keyFormatter; }
+  bool isEnableDetailedResults() const { return enableDetailedResults; }
+  bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; }
+
+  void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; }
+  void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; }
+  void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; }
+  void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; }
+  void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; }
+  void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); }
+  void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; }
+  void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; }
+  void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; }
+  void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; }
+
+  Factorization factorizationTranslator(const std::string& str) const;
+  std::string factorizationTranslator(const Factorization& value) const;
 };
 
+
 /**
  * @addtogroup ISAM2
  * This struct is returned from ISAM2::update() and contains information about
@@ -237,6 +316,16 @@ struct ISAM2Result {
   /** Detailed results, if enabled by ISAM2Params::enableDetailedResults.  See
    * Detail for information about the results data stored here. */
   boost::optional<DetailedResults> detail;
+
+
+  void print(const std::string str = "") const {
+    std::cout << str << "  Reelimintated: " << variablesReeliminated << "  Relinearized: " << variablesRelinearized << "  Cliques: " << cliques << std::endl;
+  }
+
+  /** Getters and Setters */
+  size_t getVariablesRelinearized() const { return variablesRelinearized; };
+  size_t getVariablesReeliminated() const { return variablesReeliminated; };
+  size_t getCliques() const { return cliques; };
 };
 
 /**