Merge pull request #947 from borglab/feature/NoiseModelFactorN

NoiseModelFactorN - fixed-number of variables >6

gtsam/base/utilities.h

@@ -27,3 +27,42 @@ struct RedirectCout {
 };
 
 }
+
+// boost::index_sequence was introduced in 1.66, so we'll manually define an
+// implementation if user has 1.65.  boost::index_sequence is used to get array
+// indices that align with a parameter pack.
+#include <boost/version.hpp>
+#if BOOST_VERSION >= 106600
+#include <boost/mp11/integer_sequence.hpp>
+#else
+namespace boost {
+namespace mp11 {
+// Adapted from https://stackoverflow.com/a/32223343/9151520
+template <size_t... Ints>
+struct index_sequence {
+  using type = index_sequence;
+  using value_type = size_t;
+  static constexpr std::size_t size() noexcept { return sizeof...(Ints); }
+};
+namespace detail {
+template <class Sequence1, class Sequence2>
+struct _merge_and_renumber;
+
+template <size_t... I1, size_t... I2>
+struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...> >
+    : index_sequence<I1..., (sizeof...(I1) + I2)...> {};
+}  // namespace detail
+template <size_t N>
+struct make_index_sequence
+    : detail::_merge_and_renumber<
+          typename make_index_sequence<N / 2>::type,
+          typename make_index_sequence<N - N / 2>::type> {};
+template <>
+struct make_index_sequence<0> : index_sequence<> {};
+template <>
+struct make_index_sequence<1> : index_sequence<0> {};
+template <class... T>
+using index_sequence_for = make_index_sequence<sizeof...(T)>;
+}  // namespace mp11
+}  // namespace boost
+#endif

gtsam/mainpage.dox

@@ -22,7 +22,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, derive from NoiseModelFactorN<T1, T2, ...> (where T1, T2, ... are the types of the variables, e.g. double, Vector, Pose3) and implement <b>\c evaluateError()</b>.
    - 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.