diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox
index ee7bd9924..e07f45f07 100644
--- a/gtsam/mainpage.dox
+++ b/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 unknown 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 known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError()
+-# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError(). 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.
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 7fafd95df..503ae7d58 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -28,6 +28,7 @@
#include
#include
+#include
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 NoiseModelFactorN: public NoiseModelFactor {
+
+protected:
+
+ typedef NoiseModelFactor Base;
+ typedef NoiseModelFactorN This;
+
+ /* "Dummy templated" alias is used to expand fixed-type parameter packs with
+ * same length as VALUES. This ignores the template parameter. */
+ template
+ using optional_matrix_type = boost::optional;
+
+ /* "Dummy templated" alias is used to expand fixed-type parameter packs with
+ * same length as VALUES. This ignores the template parameter. */
+ template
+ 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... keys)
+ : Base(noiseModel, std::array{keys...}) {}
+
+ /**
+ * Constructor.
+ * @param noiseModel shared pointer to noise model
+ * @param keys a container of keys for the variables in this factor
+ */
+ template
+ NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
+ : Base(noiseModel, keys) {
+ assert(std::size(keys) == sizeof...(VALUES));
+ }
+
+ ~NoiseModelFactorN() override {}
+
+ /** Method to retrieve keys */
+ template
+ 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&> H = boost::none) const override {
+ return unwhitenedError(boost::mp11::index_sequence_for{}, 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 ... 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()...);
+ }
+
+ private:
+
+ /** Pack expansion with index_sequence template pattern */
+ template
+ Vector unwhitenedError(
+ boost::mp11::index_sequence, //
+ const Values& x,
+ boost::optional&> H = boost::none) const {
+ if (this->active(x)) {
+ if (H) {
+ return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...);
+ } else {
+ return evaluateError(x.at(keys_[Inds])...);
+ }
+ } else {
+ return Vector::Zero(this->dim());
+ }
+ }
+
+ /** Serialization function */
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+ ar & boost::serialization::make_nvp("NoiseModelFactor",
+ boost::serialization::base_object(*this));
+ }
+}; // \class NoiseModelFactorN
} // \namespace gtsam
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 84bba850b..eb35bf55d 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -406,6 +406,50 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
}
+/* ************************************************************************* */
+class TestFactorN : public NoiseModelFactorN {
+public:
+ typedef NoiseModelFactorN 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 H1 = boost::none,
+ boost::optional H2 = boost::none,
+ boost::optional H3 = boost::none,
+ boost::optional 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(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 )
{