diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 0c3136e9d..fc48720f9 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -64,7 +64,7 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearization(Permuted& delta, double relinearizeThreshold); + static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** * Recursively search this clique and its children for marked keys appearing @@ -175,14 +175,29 @@ FastSet ISAM2::Impl::IndicesFromFactors(const O /* ************************************************************************* */ template -FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { +FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { FastSet relinKeys; - for(Index var=0; var(); - if(maxDelta >= relinearizeThreshold) { - relinKeys.insert(var); + + if(relinearizeThreshold.type() == typeid(double)) { + double threshold = boost::get(relinearizeThreshold); + for(Index var=0; var(); + if(maxDelta >= threshold) { + relinKeys.insert(var); + } + } + } else if(relinearizeThreshold.type() == typeid(FastMap)) { + const FastMap& thresholds = boost::get >(relinearizeThreshold); + BOOST_FOREACH(const Ordering::value_type& key_index, ordering) { + const Vector& threshold = thresholds.find(key_index.first.chr())->second; + Index j = key_index.second; + if(threshold.rows() != delta[j].rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); + if((delta[j].array().abs() > threshold.array()).any()) + relinKeys.insert(j); } } + return relinKeys; } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index cdac57b7f..338427b2c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -469,8 +469,8 @@ ISAM2Result ISAM2::update( tic(4,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - FastSet relinKeys = Impl::CheckRelinearization(delta_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, 0.0); // This is used for debugging + FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index fdb42cfc8..7e2a51ffa 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -72,6 +72,8 @@ struct ISAM2DoglegParams { */ 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 + /** Optimization parameters, this both selects the nonlinear optimization * method and specifies its parameters, either ISAM2GaussNewtonParams or * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used @@ -79,15 +81,34 @@ struct ISAM2Params { * algorithm will be used with the specified parameters. */ OptimizationParams optimizationParams; - double relinearizeThreshold; ///< Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1) + + /** Only relinearize variables whose linear delta magnitude is greater than + * this threshold (default: 0.1). If this is a FastMap instead + * of a double, then the threshold is specified for each dimension of each + * variable type. This parameter then maps from a character indicating the + * variable type to a Vector of thresholds for each dimension of that + * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, + * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate + * entries would be added with: + * \code + FastMap thresholds; + thresholds[PoseKey::chr()] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds[PointKey::chr()] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + params.relinearizeThreshold = thresholds; + \endcode + */ + RelinearizationThreshold relinearizeThreshold; + int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true) + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams - double _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold + RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError