NoiseModelFactorN - fixed-number of variables >6

gtsam/mainpage.dox

@@ -16,7 +16,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto
 
 -# The number of variables your factor involves is <b>unknown</b> at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError()
    - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel).
--# The number of variables your factor involves is <b>known</b> at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement <b>\c evaluateError()</b>
+-# The number of variables your factor involves is <b>known</b> at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement <b>\c evaluateError()</b>.  If the number of variables is greater than 6, derive from NoiseModelFactorN.
    - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor.
 -# Derive from NonlinearFactor
    - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor.

gtsam/nonlinear/NonlinearFactor.h

@@ -28,6 +28,7 @@
 
 #include <boost/serialization/base_object.hpp>
 #include <boost/assign/list_of.hpp>
+#include <boost/mp11/integer_sequence.hpp>
 
 namespace gtsam {
 
@@ -770,5 +771,110 @@ private:
 }; // \class NoiseModelFactor6
 
 /* ************************************************************************* */
+/** A convenient base class for creating your own NoiseModelFactor with N
+ * variables.  To derive from this class, implement evaluateError(). */
+template<class... VALUES>
+class NoiseModelFactorN: public NoiseModelFactor {
+
+protected:
+
+  typedef NoiseModelFactor Base;
+  typedef NoiseModelFactorN<VALUES...> This;
+
+  /* "Dummy templated" alias is used to expand fixed-type parameter packs with
+   * same length as VALUES.  This ignores the template parameter. */
+  template <typename T>
+  using optional_matrix_type = boost::optional<Matrix&>;
+
+  /* "Dummy templated" alias is used to expand fixed-type parameter packs with
+   * same length as VALUES.  This ignores the template parameter. */
+  template <typename T>
+  using key_type = Key;
+
+public:
+
+  /**
+   * Default Constructor for I/O
+   */
+  NoiseModelFactorN() {}
+
+  /**
+   * Constructor.
+   * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN)
+   * @param noiseModel shared pointer to noise model
+   * @param keys... keys for the variables in this factor
+   */
+  NoiseModelFactorN(const SharedNoiseModel& noiseModel,
+                    key_type<VALUES>... keys)
+      : Base(noiseModel, std::array<Key, sizeof...(VALUES)>{keys...}) {}
+
+  /**
+   * Constructor.
+   * @param noiseModel shared pointer to noise model
+   * @param keys a container of keys for the variables in this factor
+   */
+  template <typename CONTAINER>
+  NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
+      : Base(noiseModel, keys) {
+    assert(std::size(keys) == sizeof...(VALUES));
+  }
+
+  ~NoiseModelFactorN() override {}
+
+  /** Method to retrieve keys */
+  template <int N>
+  inline Key key() const {  return keys_[N];  }
+
+  /** Calls the n-key specific version of evaluateError, which is pure virtual
+   * so must be implemented in the derived class. */
+  Vector unwhitenedError(
+      const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const override {
+    return unwhitenedError(boost::mp11::index_sequence_for<VALUES...>{}, x, H);
+  }
+
+  /**
+   *  Override this method to finish implementing an n-way factor.
+   *  If any of the optional Matrix reference arguments are specified, it should
+   * compute both the function evaluation and its derivative(s) in the requested
+   * variables.
+   */
+  virtual Vector evaluateError(
+      const VALUES& ... x,
+      optional_matrix_type<VALUES> ... H) const = 0;
+
+  /** No-jacobians requested function overload (since parameter packs can't have
+   * default args) */
+  Vector evaluateError(const VALUES&... x) const {
+    return evaluateError(x..., optional_matrix_type<VALUES>()...);
+  }
+
+ private:
+
+  /** Pack expansion with index_sequence template pattern */
+  template <std::size_t... Inds>
+  Vector unwhitenedError(
+      boost::mp11::index_sequence<Inds...>,  //
+      const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    if (this->active(x)) {
+      if (H) {
+        return evaluateError(x.at<VALUES>(keys_[Inds])..., (*H)[Inds]...);
+      } else {
+        return evaluateError(x.at<VALUES>(keys_[Inds])...);
+      }
+    } else {
+      return Vector::Zero(this->dim());
+    }
+  }
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor",
+        boost::serialization::base_object<Base>(*this));
+  }
+}; // \class NoiseModelFactorN
 
 } // \namespace gtsam

tests/testNonlinearFactor.cpp

@@ -406,6 +406,50 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
 
 }
 
+/* ************************************************************************* */
+class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
+public:
+  typedef NoiseModelFactorN<double, double, double, double> Base;
+  TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
+
+  Vector
+    evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none) const override {
+    if (H1) {
+      *H1 = (Matrix(1, 1) << 1.0).finished();
+      *H2 = (Matrix(1, 1) << 2.0).finished();
+      *H3 = (Matrix(1, 1) << 3.0).finished();
+      *H4 = (Matrix(1, 1) << 4.0).finished();
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NoiseModelFactorN) {
+  TestFactorN tf;
+  Values tv;
+  tv.insert(X(1), double((1.0)));
+  tv.insert(X(2), double((2.0)));
+  tv.insert(X(3), double((3.0)));
+  tv.insert(X(4), double((4.0)));
+  EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
+  LONGS_EQUAL((long)X(1), (long)jf.keys()[0]);
+  LONGS_EQUAL((long)X(2), (long)jf.keys()[1]);
+  LONGS_EQUAL((long)X(3), (long)jf.keys()[2]);
+  LONGS_EQUAL((long)X(4), (long)jf.keys()[3]);
+  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin())));
+  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb()));
+}
+
 /* ************************************************************************* */
 TEST( NonlinearFactor, clone_rekey )
 {