diff --git a/gtsam.h b/gtsam.h index a658c6376..276a272a6 100644 --- a/gtsam.h +++ b/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 +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& 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& constrainedKeys); + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& 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 //************************************************************************* diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 8c470f1aa..16a3ca7b5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -20,6 +20,7 @@ using namespace boost::assign; #include #include +#include #include #include @@ -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) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 0d540a729..ab94e60f0 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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(optimizationParams).print("optimizationParams: "); + else if(optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print("optimizationParams: "); + else + std::cout << "optimizationParams: " << "{unknown type}" << "\n"; + if(relinearizeThreshold.type() == typeid(double)) + std::cout << "relinearizeThreshold: " << boost::get(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 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; }; }; /**