diff --git a/gtsam.h b/gtsam.h index 42a419230..dc2f8363a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1247,6 +1247,26 @@ class ISAM2DoglegParams { void setVerbose(bool verbose); }; +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + class ISAM2Params { ISAM2Params(); @@ -1256,8 +1276,7 @@ class ISAM2Params { void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); void setRelinearizeThreshold(double relinearizeThreshold); - // TODO: wrap this - //void setRelinearizeThreshold(const FastMap& relinearizeThreshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); int getRelinearizeSkip() const; void setRelinearizeSkip(int relinearizeSkip); bool isEnableRelinearization() const; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index ab94e60f0..52e5bccef 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -98,6 +98,8 @@ struct ISAM2DoglegParams { * @addtogroup ISAM2 * Parameters for the ISAM2 algorithm. Default parameter values are listed below. */ +typedef FastMap ISAM2ThresholdMap; +typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; struct ISAM2Params { typedef boost::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams typedef boost::variant > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds @@ -189,7 +191,12 @@ struct ISAM2Params { if(relinearizeThreshold.type() == typeid(double)) std::cout << "relinearizeThreshold: " << boost::get(relinearizeThreshold) << "\n"; else + { std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; + BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get(relinearizeThreshold)) { + std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; + } + } std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; std::cout << "enableRelinearization: " << enableRelinearization << "\n"; std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n";