Wrapped iSAM2
Stephen Williams
parent
c865485342
commit
4b3edb0889
90
gtsam.h
90
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
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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; };
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue