Added per-variable-type and per-vector-element relinearization thresholds in iSAM2

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -64,7 +64,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Index> CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold);
+  static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
 
   /**
    * Recursively search this clique and its children for marked keys appearing
@@ -175,14 +175,29 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
 
 /* ************************************************************************* */
 template<class CONDITIONAL, class VALUES, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold) {
+FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
   FastSet<Index> relinKeys;
+
+  if(relinearizeThreshold.type() == typeid(double)) {
+    double threshold = boost::get<double>(relinearizeThreshold);
     for(Index var=0; var<delta.size(); ++var) {
       double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
-    if(maxDelta >= relinearizeThreshold) {
+      if(maxDelta >= threshold) {
         relinKeys.insert(var);
       }
     }
+  } else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
+    const FastMap<char,Vector>& thresholds = boost::get<FastMap<char,Vector> >(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;
 }
 

gtsam/nonlinear/ISAM2-inl.h

@@ -469,8 +469,8 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
     tic(4,"gather relinearize keys");
     vector<bool> markedRelinMask(ordering_.nVars(), false);
     // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
-    FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, params_.relinearizeThreshold);
+    FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
-    if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, 0.0); // This is used for debugging
+    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; }

gtsam/nonlinear/ISAM2.h

@@ -72,6 +72,8 @@ struct ISAM2DoglegParams {
  */
 struct ISAM2Params {
   typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams
+  typedef boost::variant<double, FastMap<char,Vector> > 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<char,Vector> 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<char,Vector> 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