Merge branch 'develop' into hybrid/tests

examples/CameraResectioning.cpp

@@ -30,8 +30,8 @@ using symbol_shorthand::X;
  * Unary factor on the unknown pose, resulting from meauring the projection of
  * a known 3D point in the image
  */
-class ResectioningFactor: public NoiseModelFactor1<Pose3> {
+class ResectioningFactor: public NoiseModelFactorN<Pose3> {
-  typedef NoiseModelFactor1<Pose3> Base;
+  typedef NoiseModelFactorN<Pose3> Base;
 
   Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
   Point3 P_;              ///< 3D point on the calibration rig

examples/LocalizationExample.cpp

@@ -62,10 +62,10 @@ using namespace gtsam;
 //
 // The factor will be a unary factor, affect only a single system variable. It will
 // also use a standard Gaussian noise model. Hence, we will derive our new factor from
-// the NoiseModelFactor1.
+// the NoiseModelFactorN.
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-class UnaryFactor: public NoiseModelFactor1<Pose2> {
+class UnaryFactor: public NoiseModelFactorN<Pose2> {
   // The factor will hold a measurement consisting of an (X,Y) location
   // We could this with a Point2 but here we just use two doubles
   double mx_, my_;
@@ -76,11 +76,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
 
   // The constructor requires the variable key, the (X, Y) measurement value, and the noise model
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
-    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
+    NoiseModelFactorN<Pose2>(model, j), mx_(x), my_(y) {}
 
   ~UnaryFactor() override {}
 
-  // Using the NoiseModelFactor1 base class there are two functions that must be overridden.
+  // Using the NoiseModelFactorN base class there are two functions that must be overridden.
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.

examples/Pose3SLAMExample_changeKeys.cpp

@@ -71,11 +71,11 @@ int main(const int argc, const char *argv[]) {
       if (pose3Between){
         Key key1, key2;
         if(add){
-          key1 = pose3Between->key1() + firstKey;
+          key1 = pose3Between->key<1>() + firstKey;
-          key2 = pose3Between->key2() + firstKey;
+          key2 = pose3Between->key<2>() + firstKey;
         }else{
-          key1 = pose3Between->key1() - firstKey;
+          key1 = pose3Between->key<1>() - firstKey;
-          key2 = pose3Between->key2() - firstKey;
+          key2 = pose3Between->key<2>() - firstKey;
         }
         NonlinearFactor::shared_ptr simpleFactor(
             new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));

examples/SolverComparer.cpp

@@ -69,8 +69,8 @@ namespace br = boost::range;
 
 typedef Pose2 Pose;
 
-typedef NoiseModelFactor1<Pose> NM1;
+typedef NoiseModelFactorN<Pose> NM1;
-typedef NoiseModelFactor2<Pose,Pose> NM2;
+typedef NoiseModelFactorN<Pose,Pose> NM2;
 typedef BearingRangeFactor<Pose,Point2> BR;
 
 double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
@@ -261,7 +261,7 @@ void runIncremental()
     if(BetweenFactor<Pose>::shared_ptr factor =
       boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
     {
-      Key key1 = factor->key1(), key2 = factor->key2();
+      Key key1 = factor->key<1>(), key2 = factor->key<2>();
       if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
         // We found an odometry starting at firstStep
         firstPose = std::min(key1, key2);
@@ -313,11 +313,11 @@ void runIncremental()
         boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
       {
         // Stop collecting measurements that are for future steps
-        if(factor->key1() > step || factor->key2() > step)
+        if(factor->key<1>() > step || factor->key<2>() > step)
           break;
 
         // Require that one of the nodes is the current one
-        if(factor->key1() != step && factor->key2() != step)
+        if(factor->key<1>() != step && factor->key<2>() != step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add a new factor
@@ -325,22 +325,22 @@ void runIncremental()
         const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(factor->key1() > factor->key2()) {
+        if(factor->key<1>() > factor->key<2>()) {
-          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(factor->key1(), measured.inverse());
+              newVariables.insert(factor->key<1>(), measured.inverse());
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
-              newVariables.insert(factor->key1(), prevPose * measured.inverse());
+              newVariables.insert(factor->key<1>(), prevPose * measured.inverse());
             }
           }
         } else {
-          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(factor->key2(), measured);
+              newVariables.insert(factor->key<2>(), measured);
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
-              newVariables.insert(factor->key2(), prevPose * measured);
+              newVariables.insert(factor->key<2>(), prevPose * measured);
             }
           }
         }

gtsam/base/Testable.h

@@ -23,7 +23,7 @@
  *     void print(const std::string& name) const = 0;
  *
  * equality up to tolerance
- * tricky to implement, see NoiseModelFactor1 for an example
+ * tricky to implement, see PriorFactor for an example
  * equals is not supposed to print out *anything*, just return true|false
  *     bool equals(const Derived& expected, double tol) const = 0;
  *

gtsam/base/types.h

@@ -46,18 +46,49 @@
 #include <omp.h>
 #endif
 
+/* Define macros for ignoring compiler warnings.
+ * Usage Example:
+ * ```
+ *  CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
+ *  GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
+ *  MSVC_DIAGNOSTIC_PUSH_IGNORE(4996)
+ *  // ... code you want to suppress deprecation warnings for ...
+ *  DIAGNOSTIC_POP()
+ * ```
+ */
+#define DO_PRAGMA(x) _Pragma (#x)
 #ifdef __clang__
 #  define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \
   _Pragma("clang diagnostic push") \
-  _Pragma("clang diagnostic ignored \"" diag "\"")
+  DO_PRAGMA(clang diagnostic ignored diag)
 #else
 #  define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
 #endif
 
-#ifdef __clang__
+#ifdef __GNUC__
-#  define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
+#  define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \
+  _Pragma("GCC diagnostic push") \
+  DO_PRAGMA(GCC diagnostic ignored diag)
 #else
-#  define CLANG_DIAGNOSTIC_POP()
+#  define GCC_DIAGNOSTIC_PUSH_IGNORE(diag)
+#endif
+
+#ifdef _MSC_VER
+#  define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \
+  _Pragma("warning ( push )") \
+  DO_PRAGMA(warning ( disable : code ))
+#else
+#  define MSVC_DIAGNOSTIC_PUSH_IGNORE(code)
+#endif
+
+#if defined(__clang__)
+#  define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
+#elif defined(__GNUC__)
+#  define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop")
+#elif defined(_MSC_VER)
+#  define DIAGNOSTIC_POP() _Pragma("warning ( pop )")
+#else
+#  define DIAGNOSTIC_POP()
 #endif
 
 namespace gtsam {

gtsam/base/utilities.h

@@ -27,3 +27,42 @@ private:
 };
 
 }
+
+// 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/hybrid/GaussianMixture.cpp

@@ -85,8 +85,8 @@ size_t GaussianMixture::nrComponents() const {
 
 /* *******************************************************************************/
 GaussianConditional::shared_ptr GaussianMixture::operator()(
-    const DiscreteValues &discreteVals) const {
+    const DiscreteValues &discreteValues) const {
-  auto &ptr = conditionals_(discreteVals);
+  auto &ptr = conditionals_(discreteValues);
   if (!ptr) return nullptr;
   auto conditional = boost::dynamic_pointer_cast<GaussianConditional>(ptr);
   if (conditional)
@@ -209,14 +209,15 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> GaussianMixture::error(
-    const VectorValues &continuousVals) const {
+    const VectorValues &continuousValues) const {
-  // functor to convert from GaussianConditional to double error value.
+  // functor to calculate to double error value from GaussianConditional.
   auto errorFunc =
-      [continuousVals](const GaussianConditional::shared_ptr &conditional) {
+      [continuousValues](const GaussianConditional::shared_ptr &conditional) {
         if (conditional) {
-          return conditional->error(continuousVals);
+          return conditional->error(continuousValues);
         } else {
-          // return arbitrarily large error
+          // Return arbitrarily large error if conditional is null
+          // Conditional is null if it is pruned out.
           return 1e50;
         }
       };
@@ -225,10 +226,11 @@ AlgebraicDecisionTree<Key> GaussianMixture::error(
 }
 
 /* *******************************************************************************/
-double GaussianMixture::error(const VectorValues &continuousVals,
+double GaussianMixture::error(const VectorValues &continuousValues,
                               const DiscreteValues &discreteValues) const {
+  // Directly index to get the conditional, no need to build the whole tree.
   auto conditional = conditionals_(discreteValues);
-  return conditional->error(continuousVals);
+  return conditional->error(continuousValues);
 }
 
 }  // namespace gtsam

gtsam/hybrid/GaussianMixture.h

@@ -122,7 +122,7 @@ class GTSAM_EXPORT GaussianMixture
   /// @{
 
   GaussianConditional::shared_ptr operator()(
-      const DiscreteValues &discreteVals) const;
+      const DiscreteValues &discreteValues) const;
 
   /// Returns the total number of continuous components
   size_t nrComponents() const;
@@ -147,21 +147,21 @@ class GTSAM_EXPORT GaussianMixture
   /**
    * @brief Compute error of the GaussianMixture as a tree.
    *
-   * @param continuousVals The continuous VectorValues.
+   * @param continuousValues The continuous VectorValues.
-   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * @return AlgebraicDecisionTree<Key> A decision tree with the same keys
-   * as the factor but leaf values as the error.
+   * as the conditionals, and leaf values as the error.
    */
-  AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
+  AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
 
   /**
    * @brief Compute the error of this Gaussian Mixture given the continuous
    * values and a discrete assignment.
    *
-   * @param continuousVals The continuous values at which to compute the error.
+   * @param continuousValues Continuous values at which to compute the error.
    * @param discreteValues The discrete assignment for a specific mode sequence.
    * @return double
    */
-  double error(const VectorValues &continuousVals,
+  double error(const VectorValues &continuousValues,
                const DiscreteValues &discreteValues) const;
 
   /**

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -98,21 +98,23 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> GaussianMixtureFactor::error(
-    const VectorValues &continuousVals) const {
+    const VectorValues &continuousValues) const {
   // functor to convert from sharedFactor to double error value.
-  auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) {
+  auto errorFunc =
-    return factor->error(continuousVals);
+      [continuousValues](const GaussianFactor::shared_ptr &factor) {
-  };
+        return factor->error(continuousValues);
+      };
   DecisionTree<Key, double> errorTree(factors_, errorFunc);
   return errorTree;
 }
 
 /* *******************************************************************************/
 double GaussianMixtureFactor::error(
-    const VectorValues &continuousVals,
+    const VectorValues &continuousValues,
     const DiscreteValues &discreteValues) const {
+  // Directly index to get the conditional, no need to build the whole tree.
   auto factor = factors_(discreteValues);
-  return factor->error(continuousVals);
+  return factor->error(continuousValues);
 }
 
 }  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.h

@@ -133,21 +133,21 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
   /**
    * @brief Compute error of the GaussianMixtureFactor as a tree.
    *
-   * @param continuousVals The continuous VectorValues.
+   * @param continuousValues The continuous VectorValues.
-   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * @return AlgebraicDecisionTree<Key> A decision tree with the same keys
-   * as the factor but leaf values as the error.
+   * as the factors involved, and leaf values as the error.
    */
-  AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
+  AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
 
   /**
    * @brief Compute the error of this Gaussian Mixture given the continuous
    * values and a discrete assignment.
    *
-   * @param continuousVals The continuous values at which to compute the error.
+   * @param continuousValues Continuous values at which to compute the error.
    * @param discreteValues The discrete assignment for a specific mode sequence.
    * @return double
    */
-  double error(const VectorValues &continuousVals,
+  double error(const VectorValues &continuousValues,
                const DiscreteValues &discreteValues) const;
 
   /// Add MixtureFactor to a Sum, syntactic sugar.

gtsam/hybrid/HybridBayesNet.cpp

@@ -250,13 +250,16 @@ AlgebraicDecisionTree<Key> HybridBayesNet::error(
     const VectorValues &continuousValues) const {
   AlgebraicDecisionTree<Key> error_tree;
 
+  // Iterate over each factor.
   for (size_t idx = 0; idx < size(); idx++) {
     AlgebraicDecisionTree<Key> conditional_error;
+
     if (factors_.at(idx)->isHybrid()) {
-      // If factor is hybrid, select based on assignment.
+      // If factor is hybrid, select based on assignment and compute error.
       GaussianMixture::shared_ptr gm = this->atMixture(idx);
       conditional_error = gm->error(continuousValues);
 
+      // Assign for the first index, add error for subsequent ones.
       if (idx == 0) {
         error_tree = conditional_error;
       } else {
@@ -267,6 +270,7 @@ AlgebraicDecisionTree<Key> HybridBayesNet::error(
       // If continuous only, get the (double) error
       // and add it to the error_tree
       double error = this->atGaussian(idx)->error(continuousValues);
+      // Add the computed error to every leaf of the error tree.
       error_tree = error_tree.apply(
           [error](double leaf_value) { return leaf_value + error; });
 
@@ -279,6 +283,7 @@ AlgebraicDecisionTree<Key> HybridBayesNet::error(
   return error_tree;
 }
 
+/* ************************************************************************* */
 AlgebraicDecisionTree<Key> HybridBayesNet::probPrime(
     const VectorValues &continuousValues) const {
   AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues);

gtsam/hybrid/HybridBayesNet.h

@@ -145,8 +145,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
 
   /**
-   * @brief Compute unnormalized probability for each discrete assignment,
+   * @brief Compute unnormalized probability q(μ|M),
-   * and return as a tree.
+   * for each discrete assignment, and return as a tree.
+   * q(μ|M) is the unnormalized probability at the MLE point μ,
+   * conditioned on the discrete variables.
    *
    * @param continuousValues Continuous values at which to compute the
    * probability.

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -448,6 +448,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
     const VectorValues &continuousValues) const {
   AlgebraicDecisionTree<Key> error_tree(0.0);
 
+  // Iterate over each factor.
   for (size_t idx = 0; idx < size(); idx++) {
     AlgebraicDecisionTree<Key> factor_error;
 
@@ -455,8 +456,10 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
       // If factor is hybrid, select based on assignment.
       GaussianMixtureFactor::shared_ptr gaussianMixture =
           boost::static_pointer_cast<GaussianMixtureFactor>(factors_.at(idx));
+      // Compute factor error.
       factor_error = gaussianMixture->error(continuousValues);
 
+      // If first factor, assign error, else add it.
       if (idx == 0) {
         error_tree = factor_error;
       } else {
@@ -470,7 +473,9 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
           boost::static_pointer_cast<HybridGaussianFactor>(factors_.at(idx));
       GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner();
 
+      // Compute the error of the gaussian factor.
       double error = gaussian->error(continuousValues);
+      // Add the gaussian factor error to every leaf of the error tree.
       error_tree = error_tree.apply(
           [error](double leaf_value) { return leaf_value + error; });
 

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -100,11 +100,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
 
   using Values = gtsam::Values;  ///< backwards compatibility
-  using Indices = KeyVector;     ///> map from keys to values
+  using Indices = KeyVector;     ///< map from keys to values
 
   /// @name Constructors
   /// @{
 
+  /// @brief Default constructor.
   HybridGaussianFactorGraph() = default;
 
   /**
@@ -175,6 +176,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
    * @brief Compute error for each discrete assignment,
    * and return as a tree.
    *
+   * Error \f$ e = \Vert x - \mu \Vert_{\Sigma} \f$.
+   *
    * @param continuousValues Continuous values at which to compute the error.
    * @return AlgebraicDecisionTree<Key>
    */
@@ -194,8 +197,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
                const DiscreteValues& discreteValues) const;
 
   /**
-   * @brief Compute unnormalized probability for each discrete assignment,
+   * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$
-   * and return as a tree.
+   * for each discrete assignment, and return as a tree.
    *
    * @param continuousValues Continuous values at which to compute the
    * probability.

gtsam/hybrid/MixtureFactor.h

@@ -23,6 +23,7 @@
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Symbol.h>
 
 #include <algorithm>
@@ -86,11 +87,11 @@ class MixtureFactor : public HybridFactor {
    * elements based on the number of discrete keys and the cardinality of the
    * keys, so that the decision tree is constructed appropriately.
    *
-   * @tparam FACTOR The type of the factor shared pointers being passed in. Will
+   * @tparam FACTOR The type of the factor shared pointers being passed in.
-   * be typecast to NonlinearFactor shared pointers.
+   * Will be typecast to NonlinearFactor shared pointers.
    * @param keys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
-   * @param factors Vector of shared pointers to factors.
+   * @param factors Vector of nonlinear factors.
    * @param normalized Flag indicating if the factor error is already
    * normalized.
    */
@@ -128,14 +129,15 @@ class MixtureFactor : public HybridFactor {
   /**
    * @brief Compute error of the MixtureFactor as a tree.
    *
-   * @param continuousVals The continuous values for which to compute the error.
+   * @param continuousValues The continuous values for which to compute the
-   * @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
+   * error.
-   * as the factor but leaf values as the error.
+   * @return AlgebraicDecisionTree<Key> A decision tree with the same keys
+   * as the factor, and leaf values as the error.
    */
-  AlgebraicDecisionTree<Key> error(const Values& continuousVals) const {
+  AlgebraicDecisionTree<Key> error(const Values& continuousValues) const {
     // functor to convert from sharedFactor to double error value.
-    auto errorFunc = [continuousVals](const sharedFactor& factor) {
+    auto errorFunc = [continuousValues](const sharedFactor& factor) {
-      return factor->error(continuousVals);
+      return factor->error(continuousValues);
     };
     DecisionTree<Key, double> errorTree(factors_, errorFunc);
     return errorTree;
@@ -144,19 +146,19 @@ class MixtureFactor : public HybridFactor {
   /**
    * @brief Compute error of factor given both continuous and discrete values.
    *
-   * @param continuousVals The continuous Values.
+   * @param continuousValues The continuous Values.
-   * @param discreteVals The discrete Values.
+   * @param discreteValues The discrete Values.
    * @return double The error of this factor.
    */
-  double error(const Values& continuousVals,
+  double error(const Values& continuousValues,
-               const DiscreteValues& discreteVals) const {
+               const DiscreteValues& discreteValues) const {
-    // Retrieve the factor corresponding to the assignment in discreteVals.
+    // Retrieve the factor corresponding to the assignment in discreteValues.
-    auto factor = factors_(discreteVals);
+    auto factor = factors_(discreteValues);
     // Compute the error for the selected factor
-    const double factorError = factor->error(continuousVals);
+    const double factorError = factor->error(continuousValues);
     if (normalized_) return factorError;
-    return factorError +
+    return factorError + this->nonlinearFactorLogNormalizingConstant(
-           this->nonlinearFactorLogNormalizingConstant(factor, continuousVals);
+                             factor, continuousValues);
   }
 
   size_t dim() const {
@@ -212,17 +214,18 @@ class MixtureFactor : public HybridFactor {
   /// Linearize specific nonlinear factors based on the assignment in
   /// discreteValues.
   GaussianFactor::shared_ptr linearize(
-      const Values& continuousVals, const DiscreteValues& discreteVals) const {
+      const Values& continuousValues,
-    auto factor = factors_(discreteVals);
+      const DiscreteValues& discreteValues) const {
-    return factor->linearize(continuousVals);
+    auto factor = factors_(discreteValues);
+    return factor->linearize(continuousValues);
   }
 
   /// Linearize all the continuous factors to get a GaussianMixtureFactor.
   boost::shared_ptr<GaussianMixtureFactor> linearize(
-      const Values& continuousVals) const {
+      const Values& continuousValues) const {
     // functional to linearize each factor in the decision tree
-    auto linearizeDT = [continuousVals](const sharedFactor& factor) {
+    auto linearizeDT = [continuousValues](const sharedFactor& factor) {
-      return factor->linearize(continuousVals);
+      return factor->linearize(continuousValues);
     };
 
     DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(

gtsam/hybrid/hybrid.i

@@ -196,22 +196,24 @@ class HybridNonlinearFactorGraph {
 
 #include <gtsam/hybrid/MixtureFactor.h>
 class MixtureFactor : gtsam::HybridFactor {
-  MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+  MixtureFactor(
-                const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors, bool normalized = false);
+      const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::DecisionTree<gtsam::Key, gtsam::NonlinearFactor*>& factors,
+      bool normalized = false);
 
   template <FACTOR = {gtsam::NonlinearFactor}>
   MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
                 const std::vector<FACTOR*>& factors,
                 bool normalized = false);
 
-  double error(const gtsam::Values& continuousVals,
+  double error(const gtsam::Values& continuousValues,
-               const gtsam::DiscreteValues& discreteVals) const;
+               const gtsam::DiscreteValues& discreteValues) const;
 
   double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor,
                                                const gtsam::Values& values) const;
 
   GaussianMixtureFactor* linearize(
-      const gtsam::Values& continuousVals) const;
+      const gtsam::Values& continuousValues) const;
 
   void print(string s = "MixtureFactor\n",
              const gtsam::KeyFormatter& keyFormatter =

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -104,7 +104,7 @@ TEST(GaussianMixture, Error) {
                                                               X(2), S2, model);
 
   // Create decision tree
-  DiscreteKey m1(1, 2);
+  DiscreteKey m1(M(1), 2);
   GaussianMixture::Conditionals conditionals(
       {m1},
       vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
@@ -115,12 +115,19 @@ TEST(GaussianMixture, Error) {
   values.insert(X(2), Vector2::Zero());
   auto error_tree = mixture.error(values);
 
+  // regression
   std::vector<DiscreteKey> discrete_keys = {m1};
   std::vector<double> leaves = {0.5, 4.3252595};
   AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
 
-  // regression
   EXPECT(assert_equal(expected_error, error_tree, 1e-6));
+
+  // Regression for non-tree version.
+  DiscreteValues assignment;
+  assignment[M(1)] = 0;
+  EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8);
+  assignment[M(1)] = 1;
+  EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testGaussianMixtureFactor.cpp

@@ -170,24 +170,25 @@ TEST(GaussianMixtureFactor, Error) {
 
   GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
 
-  VectorValues continuousVals;
+  VectorValues continuousValues;
-  continuousVals.insert(X(1), Vector2(0, 0));
+  continuousValues.insert(X(1), Vector2(0, 0));
-  continuousVals.insert(X(2), Vector2(1, 1));
+  continuousValues.insert(X(2), Vector2(1, 1));
 
   // error should return a tree of errors, with nodes for each discrete value.
-  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
+  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousValues);
 
   std::vector<DiscreteKey> discrete_keys = {m1};
+  // Error values for regression test
   std::vector<double> errors = {1, 4};
   AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
 
   EXPECT(assert_equal(expected_error, error_tree));
 
   // Test for single leaf given discrete assignment P(X|M,Z).
-  DiscreteValues discreteVals;
+  DiscreteValues discreteValues;
-  discreteVals[m1.first] = 1;
+  discreteValues[m1.first] = 1;
-  EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals),
+  EXPECT_DOUBLES_EQUAL(
-                       1e-9);
+      4.0, mixtureFactor.error(continuousValues, discreteValues), 1e-9);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -197,8 +197,7 @@ TEST(HybridBayesNet, Error) {
 
   // Verify error computation and check for specific error value
   DiscreteValues discrete_values;
-  discrete_values[M(0)] = 1;
+  boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1);
-  discrete_values[M(1)] = 1;
 
   double total_error = 0;
   for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {

gtsam/hybrid/tests/testMixtureFactor.cpp

@@ -41,7 +41,8 @@ TEST(MixtureFactor, Constructor) {
   CHECK(it == factor.end());
 }
 
-
+/* ************************************************************************* */
+// Test .print() output.
 TEST(MixtureFactor, Printing) {
   DiscreteKey m1(1, 2);
   double between0 = 0.0;
@@ -87,11 +88,11 @@ TEST(MixtureFactor, Error) {
 
   MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
 
-  Values continuousVals;
+  Values continuousValues;
-  continuousVals.insert<double>(X(1), 0);
+  continuousValues.insert<double>(X(1), 0);
-  continuousVals.insert<double>(X(2), 1);
+  continuousValues.insert<double>(X(2), 1);
 
-  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
+  AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousValues);
 
   std::vector<DiscreteKey> discrete_keys = {m1};
   std::vector<double> errors = {0.5, 0};

gtsam/inference/graph-inl.h

@@ -200,8 +200,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
     boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
     if (!factor) continue;
 
-    KEY key1 = factor->key1();
+    KEY key1 = factor->template key<1>();
-    KEY key2 = factor->key2();
+    KEY key2 = factor->template key<2>();
 
     PoseVertex v1 = key2vertex.find(key1)->second;
     PoseVertex v2 = key2vertex.find(key2)->second;
@@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
         FACTOR2>(factor);
     if (!factor2) continue;
 
-    KEY key1 = factor2->key1();
+    KEY key1 = factor2->template key<1>();
-    KEY key2 = factor2->key2();
+    KEY key2 = factor2->template key<2>();
     // if the tree contains the key
     if ((tree.find(key1) != tree.end() &&
        tree.find(key1)->second.compare(key2) == 0) ||

gtsam/linear/GaussianBayesNet.cpp

@@ -217,14 +217,7 @@ namespace gtsam {
   double GaussianBayesNet::logDeterminant() const {
     double logDet = 0.0;
     for (const sharedConditional& cg : *this) {
-      if (cg->get_model()) {
+      logDet += cg->logDeterminant();
-        Vector diag = cg->R().diagonal();
-        cg->get_model()->whitenInPlace(diag);
-        logDet += diag.unaryExpr([](double x) { return log(x); }).sum();
-      } else {
-        logDet +=
-            cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
-      }
     }
     return logDet;
   }

gtsam/linear/GaussianBayesTree-inl.h

@@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
   double result = 0.0;
 
   // this clique
-  result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
+  result += clique->conditional()->logDeterminant();
 
   // sum of children
   for(const typename BAYESTREE::sharedClique& child: clique->children_)

gtsam/linear/GaussianBayesTree.cpp

@@ -50,15 +50,7 @@ namespace gtsam {
       const GaussianBayesTreeClique::shared_ptr& clique,
       LogDeterminantData& parentSum) {
     auto cg = clique->conditional();
-    double logDet;
+    double logDet = cg->logDeterminant();
-    if (cg->get_model()) {
-      Vector diag = cg->R().diagonal();
-      cg->get_model()->whitenInPlace(diag);
-      logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
-    } else {
-      logDet =
-          cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
-    }
     // Add the current clique's log-determinant to the overall sum
     (*parentSum.logDet) += logDet;
     return parentSum;

gtsam/linear/GaussianConditional.cpp

@@ -155,6 +155,20 @@ namespace gtsam {
     }
   }
 
+/* ************************************************************************* */
+double GaussianConditional::logDeterminant() const {
+  double logDet;
+  if (this->get_model()) {
+    Vector diag = this->R().diagonal();
+    this->get_model()->whitenInPlace(diag);
+    logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
+  } else {
+    logDet =
+        this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
+  }
+  return logDet;
+}
+
   /* ************************************************************************* */
   VectorValues GaussianConditional::solve(const VectorValues& x) const {
     // Concatenate all vector values that correspond to parent variables

gtsam/linear/GaussianConditional.h

@@ -133,6 +133,28 @@ namespace gtsam {
     /** Get a view of the r.h.s. vector d */
     const constBVector d() const { return BaseFactor::getb(); }
 
+    /**
+     * @brief Compute the log determinant of the Gaussian conditional.
+     * The determinant is computed using the R matrix, which is upper
+     * triangular.
+     * For numerical stability, the determinant is computed in log
+     * form, so it is a summation rather than a multiplication.
+     *
+     * @return double
+     */
+    double logDeterminant() const;
+
+    /**
+     * @brief Compute the determinant of the conditional from the
+     * upper-triangular R matrix.
+     *
+     * The determinant is computed in log form (hence summation) for numerical
+     * stability and then exponentiated.
+     *
+     * @return double
+     */
+    double determinant() const { return exp(this->logDeterminant()); }
+
     /**
     * Solves a conditional Gaussian and writes the solution into the entries of
     * \c x for each frontal variable of the conditional.  The parents are

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.

gtsam/navigation/AHRSFactor.cpp

@@ -105,8 +105,8 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
 //------------------------------------------------------------------------------
 void AHRSFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
-  cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
+  cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << ","
-      << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
+      << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ",";
   _PIM_.print("  preintegrated measurements:");
   noiseModel_->print("  noise model: ");
 }

gtsam/navigation/AHRSFactor.h

@@ -128,10 +128,10 @@ private:
   }
 };
 
-class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
+class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
   typedef AHRSFactor This;
-  typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
+  typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
 
   PreintegratedAhrsMeasurements _PIM_;
 
@@ -212,6 +212,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor3",
             boost::serialization::base_object<Base>(*this));

gtsam/navigation/AttitudeFactor.h

@@ -76,9 +76,9 @@ public:
  * Version of AttitudeFactor for Rot3
  * @ingroup navigation
  */
-class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor {
+class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
 
-  typedef NoiseModelFactor1<Rot3> Base;
+  typedef NoiseModelFactorN<Rot3> Base;
 
 public:
 
@@ -132,6 +132,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & boost::serialization::make_nvp("AttitudeFactor",
@@ -149,10 +150,10 @@ template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFacto
  * Version of AttitudeFactor for Pose3
  * @ingroup navigation
  */
-class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
+class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
     public AttitudeFactor {
 
-  typedef NoiseModelFactor1<Pose3> Base;
+  typedef NoiseModelFactorN<Pose3> Base;
 
 public:
 
@@ -212,6 +213,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & boost::serialization::make_nvp("AttitudeFactor",

gtsam/navigation/BarometricFactor.cpp

@@ -26,8 +26,8 @@ namespace gtsam {
 void BarometricFactor::print(const string& s,
                              const KeyFormatter& keyFormatter) const {
     cout << (s.empty() ? "" : s + " ") << "Barometric Factor on "
-         << keyFormatter(key1()) << "Barometric Bias on "
+         << keyFormatter(key<1>()) << "Barometric Bias on "
-         << keyFormatter(key2()) << "\n";
+         << keyFormatter(key<2>()) << "\n";
 
     cout << "  Baro measurement: " << nT_ << "\n";
     noiseModel_->print("  noise model: ");

gtsam/navigation/BarometricFactor.h

@@ -31,9 +31,9 @@ namespace gtsam {
  * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
  * @ingroup navigation
  */
-class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
+class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
    private:
-    typedef NoiseModelFactor2<Pose3, double> Base;
+    typedef NoiseModelFactorN<Pose3, double> Base;
 
     double nT_;  ///< Height Measurement based on a standard atmosphere
 
@@ -99,6 +99,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
     friend class boost::serialization::access;
     template <class ARCHIVE>
     void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+        // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
         ar& boost::serialization::make_nvp(
             "NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));

gtsam/navigation/CombinedImuFactor.cpp

@@ -204,9 +204,9 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
 void CombinedImuFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
   cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
-       << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
+       << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
-       << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
+       << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << ","
-       << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
+       << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>())
        << ")\n";
   _PIM_.print("  preintegrated measurements:");
   this->noiseModel_->print("  noise model: ");

gtsam/navigation/CombinedImuFactor.h

@@ -255,14 +255,14 @@ public:
  *
  * @ingroup navigation
  */
-class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
+class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
     Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
 public:
 
 private:
 
   typedef CombinedImuFactor This;
-  typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
+  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
       imuBias::ConstantBias, imuBias::ConstantBias> Base;
 
   PreintegratedCombinedMeasurements _PIM_;
@@ -334,7 +334,9 @@ public:
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
+    // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
+    ar& boost::serialization::make_nvp(
+        "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
     ar& BOOST_SERIALIZATION_NVP(_PIM_);
   }
 

gtsam/navigation/ConstantVelocityFactor.h

@@ -26,15 +26,15 @@ namespace gtsam {
  * Binary factor for applying a constant velocity model to a moving body represented as a NavState.
  * The only measurement is dt, the time delta between the states.
  */
-class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
+class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
     double dt_;
 
    public:
-    using Base = NoiseModelFactor2<NavState, NavState>;
+    using Base = NoiseModelFactorN<NavState, NavState>;
 
    public:
     ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
-        : NoiseModelFactor2<NavState, NavState>(model, i, j), dt_(dt) {}
+        : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
     ~ConstantVelocityFactor() override{};
 
     /**

gtsam/navigation/GPSFactor.h

@@ -32,11 +32,11 @@ namespace gtsam {
  * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html
  * @ingroup navigation
  */
-class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1<Pose3> {
+class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
 
 private:
 
-  typedef NoiseModelFactor1<Pose3> Base;
+  typedef NoiseModelFactorN<Pose3> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
 
@@ -99,6 +99,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
@@ -110,11 +111,11 @@ private:
  * Version of GPSFactor for NavState
  * @ingroup navigation
  */
-class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1<NavState> {
+class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
 
 private:
 
-  typedef NoiseModelFactor1<NavState> Base;
+  typedef NoiseModelFactorN<NavState> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
 
@@ -163,6 +164,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));

gtsam/navigation/ImuFactor.cpp

@@ -133,9 +133,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
 
 //------------------------------------------------------------------------------
 void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
-  cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
+  cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>())
-       << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
+       << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
-       << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
+       << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>())
        << ")\n";
   cout << *this << endl;
 }
@@ -184,22 +184,22 @@ PreintegratedImuMeasurements ImuFactor::Merge(
 ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
     const shared_ptr& f12) {
   // IMU bias keys must be the same.
-  if (f01->key5() != f12->key5())
+  if (f01->key<5>() != f12->key<5>())
   throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
 
   // expect intermediate pose, velocity keys to matchup.
-  if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
+  if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>())
   throw std::domain_error(
       "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
 
   // return new factor
   auto pim02 =
   Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
-  return boost::make_shared<ImuFactor>(f01->key1(),  // P0
+  return boost::make_shared<ImuFactor>(f01->key<1>(),  // P0
-      f01->key2(),  // V0
+      f01->key<2>(),  // V0
-      f12->key3(),  // P2
+      f12->key<3>(),  // P2
-      f12->key4(),  // V2
+      f12->key<4>(),  // V2
-      f01->key5(),  // B
+      f01->key<5>(),  // B
       pim02);
 }
 #endif
@@ -230,8 +230,8 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
 void ImuFactor2::print(const string& s,
     const KeyFormatter& keyFormatter) const {
   cout << (s.empty() ? s : s + "\n") << "ImuFactor2("
-       << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
+       << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
-       << keyFormatter(this->key3()) << ")\n";
+       << keyFormatter(this->key<3>()) << ")\n";
   cout << *this << endl;
 }
 

gtsam/navigation/ImuFactor.h

@@ -168,12 +168,12 @@ public:
  *
  * @ingroup navigation
  */
-class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
+class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
     imuBias::ConstantBias> {
 private:
 
   typedef ImuFactor This;
-  typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
+  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
       imuBias::ConstantBias> Base;
 
   PreintegratedImuMeasurements _PIM_;
@@ -248,6 +248,7 @@ public:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor5",
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
@@ -259,11 +260,11 @@ public:
  * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
  * @ingroup navigation
  */
-class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
+class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
 private:
 
   typedef ImuFactor2 This;
-  typedef NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> Base;
+  typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
 
   PreintegratedImuMeasurements _PIM_;
 
@@ -316,6 +317,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor3",
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);

gtsam/navigation/MagFactor.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * and assumes scale, direction, and the bias are given.
  * Rotation is around negative Z axis, i.e. positive is yaw to right!
  */
-class MagFactor: public NoiseModelFactor1<Rot2> {
+class MagFactor: public NoiseModelFactorN<Rot2> {
 
   const Point3 measured_; ///< The measured magnetometer values
   const Point3 nM_; ///< Local magnetic field (mag output units)
@@ -50,7 +50,7 @@ public:
   MagFactor(Key key, const Point3& measured, double scale,
       const Unit3& direction, const Point3& bias,
       const SharedNoiseModel& model) :
-      NoiseModelFactor1<Rot2>(model, key), //
+      NoiseModelFactorN<Rot2>(model, key), //
       measured_(measured), nM_(scale * direction), bias_(bias) {
   }
 
@@ -87,7 +87,7 @@ public:
  * This version uses model measured bM = scale * bRn * direction + bias
  * and assumes scale, direction, and the bias are given
  */
-class MagFactor1: public NoiseModelFactor1<Rot3> {
+class MagFactor1: public NoiseModelFactorN<Rot3> {
 
   const Point3 measured_; ///< The measured magnetometer values
   const Point3 nM_; ///< Local magnetic field (mag output units)
@@ -99,7 +99,7 @@ public:
   MagFactor1(Key key, const Point3& measured, double scale,
       const Unit3& direction, const Point3& bias,
       const SharedNoiseModel& model) :
-      NoiseModelFactor1<Rot3>(model, key), //
+      NoiseModelFactorN<Rot3>(model, key), //
       measured_(measured), nM_(scale * direction), bias_(bias) {
   }
 
@@ -125,7 +125,7 @@ public:
  * This version uses model measured bM = bRn * nM + bias
  * and optimizes for both nM and the bias, where nM is in units defined by magnetometer
  */
-class MagFactor2: public NoiseModelFactor2<Point3, Point3> {
+class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
 
   const Point3 measured_; ///< The measured magnetometer values
   const Rot3 bRn_; ///< The assumed known rotation from nav to body
@@ -135,7 +135,7 @@ public:
   /** Constructor */
   MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
       const SharedNoiseModel& model) :
-      NoiseModelFactor2<Point3, Point3>(model, key1, key2), //
+      NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
       measured_(measured), bRn_(nRb.inverse()) {
   }
 
@@ -166,7 +166,7 @@ public:
  * This version uses model measured bM = scale * bRn * direction + bias
  * and optimizes for both scale, direction, and the bias.
  */
-class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
+class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
 
   const Point3 measured_; ///< The measured magnetometer values
   const Rot3 bRn_; ///< The assumed known rotation from nav to body
@@ -176,7 +176,7 @@ public:
   /** Constructor */
   MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
       const Rot3& nRb, const SharedNoiseModel& model) :
-      NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
+      NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
       measured_(measured), bRn_(nRb.inverse()) {
   }
 

gtsam/navigation/MagPoseFactor.h

@@ -25,10 +25,10 @@ namespace gtsam {
  * expressed in the sensor frame.
  */
 template <class POSE>
-class MagPoseFactor: public NoiseModelFactor1<POSE> {
+class MagPoseFactor: public NoiseModelFactorN<POSE> {
  private:
   using This = MagPoseFactor<POSE>;
-  using Base = NoiseModelFactor1<POSE>;
+  using Base = NoiseModelFactorN<POSE>;
   using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
   using Rot = typename POSE::Rotation;
 
@@ -129,6 +129,7 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/nonlinear/ExtendedKalmanFilter.h

@@ -53,8 +53,8 @@ class ExtendedKalmanFilter {
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
-  typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
+  typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
-  typedef NoiseModelFactor1<VALUE> MeasurementFactor;
+  typedef NoiseModelFactorN<VALUE> MeasurementFactor;
 #endif
 
  protected:

gtsam/nonlinear/FunctorizedFactor.h

@@ -56,9 +56,9 @@ namespace gtsam {
  *     MultiplyFunctor(multiplier));
  */
 template <typename R, typename T>
-class FunctorizedFactor : public NoiseModelFactor1<T> {
+class FunctorizedFactor : public NoiseModelFactorN<T> {
  private:
-  using Base = NoiseModelFactor1<T>;
+  using Base = NoiseModelFactorN<T>;
 
   R measured_;  ///< value that is compared with functor return value
   SharedNoiseModel noiseModel_;                          ///< noise model
@@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
-              << keyFormatter(this->key()) << ")" << std::endl;
+              << keyFormatter(this->template key<1>()) << ")" << std::endl;
     traits<R>::Print(measured_, "  measurement: ");
     std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
               << std::endl;
@@ -120,6 +120,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar &boost::serialization::make_nvp(
         "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
     ar &BOOST_SERIALIZATION_NVP(measured_);
@@ -155,9 +156,9 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
  * @param T2: The second argument type for the functor.
  */
 template <typename R, typename T1, typename T2>
-class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
+class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
  private:
-  using Base = NoiseModelFactor2<T1, T2>;
+  using Base = NoiseModelFactorN<T1, T2>;
 
   R measured_;  ///< value that is compared with functor return value
   SharedNoiseModel noiseModel_;  ///< noise model
@@ -207,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
-              << keyFormatter(this->key1()) << ", "
+              << keyFormatter(this->template key<1>()) << ", "
-              << keyFormatter(this->key2()) << ")" << std::endl;
+              << keyFormatter(this->template key<2>()) << ")" << std::endl;
     traits<R>::Print(measured_, "  measurement: ");
     std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
               << std::endl;
@@ -227,6 +228,7 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar &boost::serialization::make_nvp(
         "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
     ar &BOOST_SERIALIZATION_NVP(measured_);

gtsam/nonlinear/NonlinearEquality.h

@@ -42,7 +42,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<class VALUE>
-class NonlinearEquality: public NoiseModelFactor1<VALUE> {
+class NonlinearEquality: public NoiseModelFactorN<VALUE> {
 
 public:
   typedef VALUE T;
@@ -62,7 +62,7 @@ private:
   using This = NonlinearEquality<VALUE>;
 
   // typedef to base class
-  using Base = NoiseModelFactor1<VALUE>;
+  using Base = NoiseModelFactorN<VALUE>;
 
 public:
 
@@ -184,6 +184,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
@@ -203,13 +204,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
  * Simple unary equality constraint - fixes a value for a variable
  */
 template<class VALUE>
-class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
+class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
 
 public:
   typedef VALUE X;
 
 protected:
-  typedef NoiseModelFactor1<VALUE> Base;
+  typedef NoiseModelFactorN<VALUE> Base;
   typedef NonlinearEquality1<VALUE> This;
 
   /// Default constructor to allow for serialization
@@ -272,6 +273,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
@@ -290,9 +292,9 @@ struct traits<NonlinearEquality1<VALUE> >
  * be the same.
  */
 template <class T>
-class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
+class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
  protected:
-  using Base = NoiseModelFactor2<T, T>;
+  using Base = NoiseModelFactorN<T, T>;
   using This = NonlinearEquality2<T>;
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(T)
@@ -337,6 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar& boost::serialization::make_nvp(
         "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
   }

gtsam/nonlinear/PriorFactor.h

@@ -27,14 +27,14 @@ namespace gtsam {
    * @ingroup nonlinear
    */
   template<class VALUE>
-  class PriorFactor: public NoiseModelFactor1<VALUE> {
+  class PriorFactor: public NoiseModelFactorN<VALUE> {
 
   public:
     typedef VALUE T;
 
   private:
 
-    typedef NoiseModelFactor1<VALUE> Base;
+    typedef NoiseModelFactorN<VALUE> Base;
 
     VALUE prior_; /** The measurement */
 
@@ -105,6 +105,7 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
       ar & boost::serialization::make_nvp("NoiseModelFactor1",
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);

gtsam/nonlinear/tests/priorFactor.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<!DOCTYPE boost_serialization>
+<boost_serialization signature="serialization::archive" version="15">
+<data class_id="0" tracking_level="1" version="0" object_id="_0">
+	<NoiseModelFactor1 class_id="1" tracking_level="0" version="0">
+		<NoiseModelFactor class_id="2" tracking_level="0" version="0">
+			<NonlinearFactor class_id="3" tracking_level="0" version="0">
+				<keys_>
+					<count>1</count>
+					<item_version>0</item_version>
+					<item>12345</item>
+				</keys_>
+			</NonlinearFactor>
+			<noiseModel_ class_id="5" tracking_level="0" version="1">
+				<px class_id="6" class_name="gtsam_noiseModel_Unit" tracking_level="1" version="0" object_id="_1">
+					<Isotropic class_id="7" tracking_level="1" version="0" object_id="_2">
+						<Diagonal class_id="8" tracking_level="1" version="0" object_id="_3">
+							<Gaussian class_id="9" tracking_level="0" version="0">
+								<Base class_id="10" tracking_level="0" version="0">
+									<dim_>6</dim_>
+								</Base>
+								<sqrt_information_ class_id="11" tracking_level="0" version="1">
+									<initialized>0</initialized>
+								</sqrt_information_>
+							</Gaussian>
+							<sigmas_ class_id="12" tracking_level="0" version="0">
+								<size>6</size>
+								<data>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+								</data>
+							</sigmas_>
+							<invsigmas_>
+								<size>6</size>
+								<data>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+									<item>1.00000000000000000e+00</item>
+								</data>
+							</invsigmas_>
+						</Diagonal>
+						<sigma_>1.00000000000000000e+00</sigma_>
+						<invsigma_>1.00000000000000000e+00</invsigma_>
+					</Isotropic>
+				</px>
+			</noiseModel_>
+		</NoiseModelFactor>
+	</NoiseModelFactor1>
+	<prior_ class_id="13" tracking_level="0" version="0">
+		<R_ class_id="14" tracking_level="0" version="0">
+			<rot11>4.11982245665682978e-01</rot11>
+			<rot12>-8.33737651774156818e-01</rot12>
+			<rot13>-3.67630462924899259e-01</rot13>
+			<rot21>-5.87266449276209815e-02</rot21>
+			<rot22>-4.26917621276207360e-01</rot22>
+			<rot23>9.02381585483330806e-01</rot23>
+			<rot31>-9.09297426825681709e-01</rot31>
+			<rot32>-3.50175488374014632e-01</rot32>
+			<rot33>-2.24845095366152908e-01</rot33>
+		</R_>
+		<t_ class_id="15" tracking_level="0" version="0">
+			<data>
+				<item>4.00000000000000000e+00</item>
+				<item>5.00000000000000000e+00</item>
+				<item>6.00000000000000000e+00</item>
+			</data>
+		</t_>
+	</prior_>
+</data>
+</boost_serialization>

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -107,8 +107,60 @@ TEST (Serialization, TemplatedValues) {
   EXPECT(equalsBinary(values));
 }
 
-TEST(Serialization, ISAM2) {
+/**
+ * Test deserializing from a known serialization generated by code from commit
+ * 0af17f438f62f4788f3a572ecd36d06d224fd1e1 (>4.2a7)
+ * We only test that deserialization matches since
+ *  (1) that's the main backward compatibility requirement and
+ *  (2) serialized string depends on boost version
+ * Also note: we don't run this test when quaternions or TBB are enabled since
+ * serialization structures are different and the serialized strings/xml are
+ * hard-coded in this test.
+ */
+TEST(Serialization, NoiseModelFactor1_backwards_compatibility) {
+  PriorFactor<Pose3> factor(
+      12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)),
+      noiseModel::Unit::Create(6));
 
+  // roundtrip (sanity check)
+  PriorFactor<Pose3> factor_deserialized_str_2 = PriorFactor<Pose3>();
+  roundtrip(factor, factor_deserialized_str_2);
+  EXPECT(assert_equal(factor, factor_deserialized_str_2));
+
+#if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB)
+  // Deserialize string
+  std::string serialized_str =
+      "22 serialization::archive 15 1 0\n"
+      "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n"
+      "1 1 0\n"
+      "2 1 0\n"
+      "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 "
+      "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
+      "1.00000000000000000e+00 6 1.00000000000000000e+00 "
+      "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
+      "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 "
+      "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 "
+      "-8.33737651774156818e-01 -3.67630462924899259e-01 "
+      "-5.87266449276209815e-02 -4.26917621276207360e-01 "
+      "9.02381585483330806e-01 -9.09297426825681709e-01 "
+      "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 "
+      "4.00000000000000000e+00 5.00000000000000000e+00 "
+      "6.00000000000000000e+00\n";
+
+  PriorFactor<Pose3> factor_deserialized_str = PriorFactor<Pose3>();
+  deserializeFromString(serialized_str, factor_deserialized_str);
+  EXPECT(assert_equal(factor, factor_deserialized_str));
+
+  // Deserialize XML
+  PriorFactor<Pose3> factor_deserialized_xml = PriorFactor<Pose3>();
+  deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR
+                         "/../../gtsam/nonlinear/tests/priorFactor.xml",
+                         factor_deserialized_xml);
+  EXPECT(assert_equal(factor, factor_deserialized_xml));
+#endif
+}
+
+TEST(Serialization, ISAM2) {
   gtsam::ISAM2Params parameters;
   gtsam::ISAM2 solver(parameters);
   gtsam::NonlinearFactorGraph graph;

gtsam/sfm/ShonanAveraging.cpp

@@ -958,7 +958,7 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
   // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
   // because the tangent space of Pose2 is ordered as (vx, vy, w)
   auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
-  return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
+  return BinaryMeasurement<Rot2>(f->key<1>(), f->key<2>(), f->measured().rotation(),
                                  model);
 }
     
@@ -1006,7 +1006,7 @@ static BinaryMeasurement<Rot3> convert(
   // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
   // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
   auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
-  return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
+  return BinaryMeasurement<Rot3>(f->key<1>(), f->key<2>(), f->measured().rotation(),
                                  model);
 }
 

gtsam/sfm/ShonanFactor.cpp

@@ -35,7 +35,7 @@ template <size_t d>
 ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
                               const SharedNoiseModel &model,
                               const boost::shared_ptr<Matrix> &G)
-    : NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
+    : NoiseModelFactorN<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
       M_(R12.matrix()), // d*d in all cases
       p_(p),            // 4 for SO(4)
       pp_(p * p),       // 16 for SO(4)
@@ -57,8 +57,8 @@ ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
 template <size_t d>
 void ShonanFactor<d>::print(const std::string &s,
                             const KeyFormatter &keyFormatter) const {
-  std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
+  std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << ","
-            << keyFormatter(key2()) << ")\n";
+            << keyFormatter(key<2>()) << ")\n";
   traits<Matrix>::Print(M_, "  M: ");
   noiseModel_->print("  noise model: ");
 }
@@ -68,7 +68,7 @@ template <size_t d>
 bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
                              double tol) const {
   auto e = dynamic_cast<const ShonanFactor *>(&expected);
-  return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
+  return e != nullptr && NoiseModelFactorN<SOn, SOn>::equals(*e, tol) &&
          p_ == e->p_ && M_ == e->M_;
 }
 

gtsam/sfm/ShonanFactor.h

@@ -33,7 +33,7 @@ namespace gtsam {
  * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
  */
 template <size_t d>
-class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> {
+class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
   Matrix M_;                    ///< measured rotation between R1 and R2
   size_t p_, pp_;               ///< dimensionality constants
   boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
@@ -66,7 +66,7 @@ public:
               double tol = 1e-9) const override;
 
   /// @}
-  /// @name NoiseModelFactor2 methods
+  /// @name NoiseModelFactorN methods
   /// @{
 
   /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]

gtsam/sfm/TranslationFactor.h

@@ -39,9 +39,9 @@ namespace gtsam {
  * 
  * 
  */
-class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
+class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
  private:
-  typedef NoiseModelFactor2<Point3, Point3> Base;
+  typedef NoiseModelFactorN<Point3, Point3> Base;
   Point3 measured_w_aZb_;
 
  public:

gtsam/slam/BetweenFactor.h

@@ -37,7 +37,7 @@ namespace gtsam {
    * @ingroup slam
    */
   template<class VALUE>
-  class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {
+  class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
 
     // Check that VALUE type is a testable Lie group
     BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
@@ -50,7 +50,7 @@ namespace gtsam {
   private:
 
     typedef BetweenFactor<VALUE> This;
-    typedef NoiseModelFactor2<VALUE, VALUE> Base;
+    typedef NoiseModelFactorN<VALUE, VALUE> Base;
 
     VALUE measured_; /** The measurement */
 
@@ -88,8 +88,8 @@ namespace gtsam {
         const std::string& s = "",
         const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
-          << keyFormatter(this->key1()) << ","
+          << keyFormatter(this->template key<1>()) << ","
-          << keyFormatter(this->key2()) << ")\n";
+          << keyFormatter(this->template key<2>()) << ")\n";
       traits<T>::Print(measured_, "  measured: ");
       this->noiseModel_->print("  noise model: ");
     }
@@ -101,7 +101,7 @@ namespace gtsam {
     }
 
     /// @}
-    /// @name NoiseModelFactor2 methods 
+    /// @name NoiseModelFactorN methods 
     /// @{
 
     /// evaluate error, returns vector of errors size of tangent space
@@ -136,6 +136,7 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
       ar & boost::serialization::make_nvp("NoiseModelFactor2",
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/slam/BoundingConstraint.h

@@ -30,9 +30,9 @@ namespace gtsam {
  * @ingroup slam
  */
 template<class VALUE>
-struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
+struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
   typedef VALUE X;
-  typedef NoiseModelFactor1<VALUE> Base;
+  typedef NoiseModelFactorN<VALUE> Base;
   typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
 
   double threshold_;
@@ -85,6 +85,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(threshold_);
@@ -97,11 +98,11 @@ private:
  * to implement for specific systems
  */
 template<class VALUE1, class VALUE2>
-struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
+struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
   typedef VALUE1 X1;
   typedef VALUE2 X2;
 
-  typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
+  typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
   typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
 
   double threshold_;
@@ -128,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
   /** active when constraint *NOT* met */
   bool active(const Values& c) const override {
     // note: still active at equality to avoid zigzagging
-    double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
+    double x = value(c.at<X1>(this->template key<1>()), c.at<X2>(this->template key<2>()));
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
   }
 
@@ -158,6 +159,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor2",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(threshold_);

gtsam/slam/EssentialMatrixConstraint.cpp

@@ -27,8 +27,8 @@ namespace gtsam {
 /* ************************************************************************* */
 void EssentialMatrixConstraint::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
-  std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
+  std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>())
-      << "," << keyFormatter(this->key2()) << ")\n";
+      << "," << keyFormatter(this->key<2>()) << ")\n";
   measuredE_.print("  measured: ");
   this->noiseModel_->print("  noise model: ");
 }

gtsam/slam/EssentialMatrixConstraint.h

@@ -27,12 +27,12 @@ namespace gtsam {
  * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
  * @ingroup slam
  */
-class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
+class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
 
 private:
 
   typedef EssentialMatrixConstraint This;
-  typedef NoiseModelFactor2<Pose3, Pose3> Base;
+  typedef NoiseModelFactorN<Pose3, Pose3> Base;
 
   EssentialMatrix measuredE_; /** The measurement is an essential matrix */
 
@@ -93,6 +93,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar
         & boost::serialization::make_nvp("NoiseModelFactor2",
             boost::serialization::base_object<Base>(*this));

gtsam/slam/EssentialMatrixFactor.h

@@ -31,10 +31,10 @@ namespace gtsam {
 /**
  * Factor that evaluates epipolar error p'Ep for given essential matrix
  */
-class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
+class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
   Vector3 vA_, vB_;  ///< Homogeneous versions, in ideal coordinates
 
-  typedef NoiseModelFactor1<EssentialMatrix> Base;
+  typedef NoiseModelFactorN<EssentialMatrix> Base;
   typedef EssentialMatrixFactor This;
 
  public:
@@ -106,12 +106,12 @@ class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
  * in image 2 is perfect, and returns re-projection error in image 1
  */
 class EssentialMatrixFactor2
-    : public NoiseModelFactor2<EssentialMatrix, double> {
+    : public NoiseModelFactorN<EssentialMatrix, double> {
   Point3 dP1_;  ///< 3D point corresponding to measurement in image 1
   Point2 pn_;   ///< Measurement in image 2, in ideal coordinates
   double f_;    ///< approximate conversion factor for error scaling
 
-  typedef NoiseModelFactor2<EssentialMatrix, double> Base;
+  typedef NoiseModelFactorN<EssentialMatrix, double> Base;
   typedef EssentialMatrixFactor2 This;
 
  public:
@@ -321,11 +321,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
  */
 template <class CALIBRATION>
 class EssentialMatrixFactor4
-    : public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
+    : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
  private:
   Point2 pA_, pB_;  ///< points in pixel coordinates
 
-  typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
+  typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
   typedef EssentialMatrixFactor4 This;
 
   static constexpr int DimK = FixedDimension<CALIBRATION>::value;

gtsam/slam/FrobeniusFactor.h

@@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
  * element of SO(3) or SO(4).
  */
 template <class Rot>
-class FrobeniusPrior : public NoiseModelFactor1<Rot> {
+class FrobeniusPrior : public NoiseModelFactorN<Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
   using MatrixNN = typename Rot::MatrixNN;
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
@@ -59,7 +59,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
   /// Constructor
   FrobeniusPrior(Key j, const MatrixNN& M,
                  const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
+      : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
     vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
   }
 
@@ -75,13 +75,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
  * The template argument can be any fixed-size SO<N>.
  */
 template <class Rot>
-class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
+class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
 
  public:
   /// Constructor
   FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
+      : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
                                     j2) {}
 
   /// Error is just Frobenius norm between rotation matrices.
@@ -101,7 +101,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
  * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
  */
 template <class Rot>
-class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
+class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   Rot R12_;  ///< measured rotation between R1 and R2
   Eigen::Matrix<double, Rot::dimension, Rot::dimension>
       R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
@@ -116,7 +116,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   /// Construct from two keys and measured rotation
   FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
                          const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactor2<Rot, Rot>(
+      : NoiseModelFactorN<Rot, Rot>(
             ConvertNoiseModel(model, Dim), j1, j2),
         R12_(R12),
         R2hat_H_R1_(R12.inverse().AdjointMap()) {}
@@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   print(const std::string &s,
         const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
-              << ">(" << keyFormatter(this->key1()) << ","
+              << ">(" << keyFormatter(this->template key<1>()) << ","
-              << keyFormatter(this->key2()) << ")\n";
+              << keyFormatter(this->template key<2>()) << ")\n";
     traits<Rot>::Print(R12_, "  R12: ");
     this->noiseModel_->print("  noise model: ");
   }
@@ -140,12 +140,12 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   bool equals(const NonlinearFactor &expected,
               double tol = 1e-9) const override {
     auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
-    return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
+    return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
            traits<Rot>::Equals(this->R12_, e->R12_, tol);
   }
 
   /// @}
-  /// @name NoiseModelFactor2 methods 
+  /// @name NoiseModelFactorN methods 
   /// @{
 
   /// Error is Frobenius norm between R1*R12 and R2.

gtsam/slam/GeneralSFMFactor.h

@@ -57,7 +57,7 @@ namespace gtsam {
  * @ingroup slam
  */
 template<class CAMERA, class LANDMARK>
-class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
+class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
   GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
@@ -74,7 +74,7 @@ protected:
 public:
 
   typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
-  typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class
+  typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
 
   // shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -140,7 +140,7 @@ public:
     // Only linearize if the factor is active
     if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
 
-    const Key key1 = this->key1(), key2 = this->key2();
+    const Key key1 = this->template key<1>(), key2 = this->template key<2>();
     JacobianC H1;
     JacobianL H2;
     Vector2 b;
@@ -184,6 +184,7 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor2",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);
@@ -200,7 +201,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
  * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
  */
 template<class CALIBRATION>
-class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
+class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
 
   GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
   static const int DimK = FixedDimension<CALIBRATION>::value;
@@ -213,7 +214,7 @@ public:
 
   typedef GeneralSFMFactor2<CALIBRATION> This;
   typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
-  typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
+  typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
 
   // shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -269,8 +270,8 @@ public:
       if (H1) *H1 = Matrix::Zero(2, 6);
       if (H2) *H2 = Matrix::Zero(2, 3);
       if (H3) *H3 = Matrix::Zero(2, DimK);
-      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
+      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>())
-      << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+      << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
     }
     return Z_2x1;
   }
@@ -285,6 +286,7 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor3",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/slam/InitializePose3.cpp

@@ -228,7 +228,7 @@ void InitializePose3::createSymbolicGraph(
       Rot3 Rij = pose3Between->measured().rotation();
       factorId2RotMap->emplace(factorId, Rij);
 
-      Key key1 = pose3Between->key1();
+      Key key1 = pose3Between->key<1>();
       if (adjEdgesMap->find(key1) != adjEdgesMap->end()) {  // key is already in
         adjEdgesMap->at(key1).push_back(factorId);
       } else {
@@ -236,7 +236,7 @@ void InitializePose3::createSymbolicGraph(
         edge_id.push_back(factorId);
         adjEdgesMap->emplace(key1, edge_id);
       }
-      Key key2 = pose3Between->key2();
+      Key key2 = pose3Between->key<2>();
       if (adjEdgesMap->find(key2) != adjEdgesMap->end()) {  // key is already in
         adjEdgesMap->at(key2).push_back(factorId);
       } else {

gtsam/slam/OrientedPlane3Factor.cpp

@@ -15,8 +15,8 @@ namespace gtsam {
 void OrientedPlane3Factor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
   cout << s << (s == "" ? "" : "\n");
-  cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
+  cout << "OrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", "
-       << keyFormatter(key2()) << ")\n";
+       << keyFormatter(key<2>()) << ")\n";
   measured_p_.print("Measured Plane");
   this->noiseModel_->print("  noise model: ");
 }

gtsam/slam/OrientedPlane3Factor.h

@@ -15,10 +15,10 @@ namespace gtsam {
 /**
  * Factor to measure a planar landmark from a given pose
  */
-class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
+class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
  protected:
   OrientedPlane3 measured_p_;
-  typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
+  typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
 
  public:
   /// Constructor
@@ -49,10 +49,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, Oriente
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
-class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> {
+class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
  protected:
   OrientedPlane3 measured_p_;  /// measured plane parameters
-  typedef NoiseModelFactor1<OrientedPlane3> Base;
+  typedef NoiseModelFactorN<OrientedPlane3> Base;
 
  public:
   typedef OrientedPlane3DirectionPrior This;

gtsam/slam/PoseRotationPrior.h

@@ -16,11 +16,11 @@
 namespace gtsam {
 
 template<class POSE>
-class PoseRotationPrior : public NoiseModelFactor1<POSE> {
+class PoseRotationPrior : public NoiseModelFactorN<POSE> {
 public:
 
   typedef PoseRotationPrior<POSE> This;
-  typedef NoiseModelFactor1<POSE> Base;
+  typedef NoiseModelFactorN<POSE> Base;
   typedef POSE Pose;
   typedef typename POSE::Translation Translation;
   typedef typename POSE::Rotation Rotation;
@@ -92,6 +92,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/slam/PoseTranslationPrior.h

@@ -18,10 +18,10 @@ namespace gtsam {
  * A prior on the translation part of a pose
  */
 template<class POSE>
-class PoseTranslationPrior : public NoiseModelFactor1<POSE> {
+class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
 public:
   typedef PoseTranslationPrior<POSE> This;
-  typedef NoiseModelFactor1<POSE> Base;
+  typedef NoiseModelFactorN<POSE> Base;
   typedef POSE Pose;
   typedef typename POSE::Translation Translation;
   typedef typename POSE::Rotation Rotation;
@@ -91,6 +91,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor1",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/slam/ProjectionFactor.h

@@ -37,7 +37,7 @@ namespace gtsam {
    */
   template <class POSE = Pose3, class LANDMARK = Point3,
             class CALIBRATION = Cal3_S2>
-  class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
+  class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
   protected:
 
     // Keep a copy of measurement and calibration for I/O
@@ -52,7 +52,7 @@ namespace gtsam {
   public:
 
     /// shorthand for base class type
-    typedef NoiseModelFactor2<POSE, LANDMARK> Base;
+    typedef NoiseModelFactorN<POSE, LANDMARK> Base;
 
     /// shorthand for this class
     typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
@@ -154,10 +154,10 @@ namespace gtsam {
         if (H1) *H1 = Matrix::Zero(2,6);
         if (H2) *H2 = Matrix::Zero(2,3);
         if (verboseCheirality_)
-          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
-              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+              " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
         if (throwCheirality_)
-          throw CheiralityException(this->key2());
+          throw CheiralityException(this->template key<2>());
       }
       return Vector2::Constant(2.0 * K_->fx());
     }

gtsam/slam/ReferenceFrameFactor.h

@@ -54,13 +54,13 @@ P transform_point(
  * specific classes of landmarks
  */
 template<class POINT, class TRANSFORM>
-class ReferenceFrameFactor : public NoiseModelFactor3<POINT, TRANSFORM, POINT> {
+class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
 protected:
   /** default constructor for serialization only */
   ReferenceFrameFactor() {}
 
 public:
-  typedef NoiseModelFactor3<POINT, TRANSFORM, POINT> Base;
+  typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
   typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
 
   typedef POINT Point;
@@ -107,16 +107,16 @@ public:
   void print(const std::string& s="",
       const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": ReferenceFrameFactor("
-        << "Global: " << keyFormatter(this->key1()) << ","
+        << "Global: " << keyFormatter(this->template key<1>()) << ","
-        << " Transform: " << keyFormatter(this->key2()) << ","
+        << " Transform: " << keyFormatter(this->template key<2>()) << ","
-        << " Local: " << keyFormatter(this->key3()) << ")\n";
+        << " Local: " << keyFormatter(this->template key<3>()) << ")\n";
     this->noiseModel_->print("  noise model");
   }
 
   // access - convenience functions
-  Key global_key() const { return this->key1(); }
+  Key global_key() const { return this->template key<1>(); }
-  Key transform_key() const { return this->key2(); }
+  Key transform_key() const { return this->template key<2>(); }
-  Key local_key() const { return this->key3(); }
+  Key local_key() const { return this->template key<3>(); }
 
 private:
   /** Serialization function */

gtsam/slam/RotateFactor.h

@@ -20,11 +20,11 @@ namespace gtsam {
  * with z and p measured and predicted angular velocities, and hence
  *   p = iRc * z
  */
-class RotateFactor: public NoiseModelFactor1<Rot3> {
+class RotateFactor: public NoiseModelFactorN<Rot3> {
 
   Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
 
-  typedef NoiseModelFactor1<Rot3> Base;
+  typedef NoiseModelFactorN<Rot3> Base;
   typedef RotateFactor This;
 
 public:
@@ -64,11 +64,11 @@ public:
  * Factor on unknown rotation iRc that relates two directions c
  * Directions provide less constraints than a full rotation
  */
-class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
+class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
 
   Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z
 
-  typedef NoiseModelFactor1<Rot3> Base;
+  typedef NoiseModelFactorN<Rot3> Base;
   typedef RotateDirectionsFactor This;
 
 public:

gtsam/slam/StereoFactor.h

@@ -28,7 +28,7 @@ namespace gtsam {
  * @ingroup slam
  */
 template<class POSE, class LANDMARK>
-class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
+class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
 private:
 
   // Keep a copy of measurement and calibration for I/O
@@ -43,7 +43,7 @@ private:
 public:
 
   // shorthand for base class type
-  typedef NoiseModelFactor2<POSE, LANDMARK> Base;             ///< typedef for base class
+  typedef NoiseModelFactorN<POSE, LANDMARK> Base;             ///< typedef for base class
   typedef GenericStereoFactor<POSE, LANDMARK> This;           ///< typedef for this class (with templates)
   typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
   typedef POSE CamPose;                                       ///< typedef for Pose Lie Value type
@@ -141,8 +141,8 @@ public:
       if (H1) *H1 = Matrix::Zero(3,6);
       if (H2) *H2 = Z_3x3;
       if (verboseCheirality_)
-      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
-          " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+          " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
       if (throwCheirality_)
         throw StereoCheiralityException(this->key2());
     }
@@ -170,6 +170,7 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor2",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam/slam/TriangulationFactor.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * @ingroup slam
  */
 template<class CAMERA>
-class TriangulationFactor: public NoiseModelFactor1<Point3> {
+class TriangulationFactor: public NoiseModelFactorN<Point3> {
 
 public:
 
@@ -40,7 +40,7 @@ public:
 protected:
 
   /// shorthand for base class type
-  using Base = NoiseModelFactor1<Point3>;
+  using Base = NoiseModelFactorN<Point3>;
 
   /// shorthand for this class
   using This = TriangulationFactor<CAMERA>;

gtsam/slam/dataset.cpp

@@ -535,7 +535,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
       graph->push_back(*f);
 
       // Insert vertices if pure odometry file
-      Key key1 = (*f)->key1(), key2 = (*f)->key2();
+      Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>();
       if (!initial->exists(key1))
         initial->insert(key1, Pose2());
       if (!initial->exists(key2))
@@ -603,7 +603,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
       continue;
 
     const Pose2 pose = factor->measured().inverse();
-    stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
+    stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " "
            << pose.x() << " " << pose.y() << " " << pose.theta() << " "
            << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
            << " " << RR(0, 2) << " " << RR(1, 2) << endl;
@@ -691,8 +691,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
       }
       Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
       Pose2 pose = factor->measured(); //.inverse();
-      stream << "EDGE_SE2 " << index(factor->key1()) << " "
+      stream << "EDGE_SE2 " << index(factor->key<1>()) << " "
-             << index(factor->key2()) << " " << pose.x() << " " << pose.y()
+             << index(factor->key<2>()) << " " << pose.x() << " " << pose.y()
              << " " << pose.theta();
       for (size_t i = 0; i < 3; i++) {
         for (size_t j = i; j < 3; j++) {
@@ -717,8 +717,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
       const Pose3 pose3D = factor3D->measured();
       const Point3 p = pose3D.translation();
       const auto q = pose3D.rotation().toQuaternion();
-      stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " "
+      stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " "
-             << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " "
+             << index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " "
              << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
              << q.w();
 

gtsam_unstable/dynamics/FullIMUFactor.h

@@ -22,9 +22,9 @@ namespace gtsam {
  * assumed to be PoseRTV
  */
 template<class POSE>
-class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
+class FullIMUFactor : public NoiseModelFactorN<POSE, POSE> {
 public:
-  typedef NoiseModelFactor2<POSE, POSE> Base;
+  typedef NoiseModelFactorN<POSE, POSE> Base;
   typedef FullIMUFactor<POSE> This;
 
 protected:

gtsam_unstable/dynamics/IMUFactor.h

@@ -20,9 +20,9 @@ namespace gtsam {
  * assumed to be PoseRTV
  */
 template<class POSE>
-class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
+class IMUFactor : public NoiseModelFactorN<POSE, POSE> {
 public:
-  typedef NoiseModelFactor2<POSE, POSE> Base;
+  typedef NoiseModelFactorN<POSE, POSE> Base;
   typedef IMUFactor<POSE> This;
 
 protected:

gtsam_unstable/dynamics/Pendulum.h

@@ -20,11 +20,11 @@ namespace gtsam {
  *    - For implicit Euler method:  q_{k+1} = q_k + h*v_{k+1}
  *    - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
  */
-class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
+class PendulumFactor1: public NoiseModelFactorN<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<double, double, double> Base;
+  typedef NoiseModelFactorN<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactor1() {}
@@ -66,11 +66,11 @@ public:
  *    - For implicit Euler method:  v_{k+1} = v_k - h*g/L*sin(q_{k+1})
  *    - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
  */
-class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
+class PendulumFactor2: public NoiseModelFactorN<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<double, double, double> Base;
+  typedef NoiseModelFactorN<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactor2() {}
@@ -113,11 +113,11 @@ public:
  *  \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
  *  \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
  */
-class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
+class PendulumFactorPk: public NoiseModelFactorN<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<double, double, double> Base;
+  typedef NoiseModelFactorN<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactorPk() {}
@@ -169,11 +169,11 @@ public:
  *  \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
  *  \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
  */
-class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
+class PendulumFactorPk1: public NoiseModelFactorN<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<double, double, double> Base;
+  typedef NoiseModelFactorN<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactorPk1() {}

gtsam_unstable/dynamics/SimpleHelicopter.h

@@ -24,10 +24,10 @@ namespace gtsam {
  * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
  *  in sequential update method.
  */
-class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6>  {
+class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6>  {
 
   double h_;  // time step
-  typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
+  typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
 public:
   Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
     Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
@@ -73,7 +73,7 @@ public:
 /**
  * Implement the Discrete Euler-Poincare' equation:
  */
-class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3>  {
+class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector6, Pose3>  {
 
   double h_;  /// time step
   Matrix Inertia_;  /// Inertia tensors Inertia = [ J 0; 0 M ]
@@ -86,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector
   // This might be needed in control or system identification problems.
   // We treat them as constant here, since the control inputs are to specify.
 
-  typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base;
+  typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
 
 public:
   DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,

gtsam_unstable/dynamics/VelocityConstraint.h

@@ -32,9 +32,9 @@ typedef enum {
  * NOTE: this approximation is insufficient for large timesteps, but is accurate
  * if timesteps are small.
  */
-class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> {
+class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
 public:
-  typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
+  typedef gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> Base;
 
 protected:
 

gtsam_unstable/dynamics/VelocityConstraint3.h

@@ -10,11 +10,11 @@
 
 namespace gtsam {
 
-class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
+class VelocityConstraint3 : public NoiseModelFactorN<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<double, double, double> Base;
+  typedef NoiseModelFactorN<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   VelocityConstraint3() {}
@@ -53,6 +53,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor3",
         boost::serialization::base_object<Base>(*this));
   }

gtsam_unstable/slam/BiasedGPSFactor.h

@@ -27,12 +27,12 @@ namespace gtsam {
    * common-mode errors and that can be partially corrected if other sensors are used
    * @ingroup slam
    */
-  class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
+  class BiasedGPSFactor: public NoiseModelFactorN<Pose3, Point3> {
 
   private:
 
     typedef BiasedGPSFactor This;
-    typedef NoiseModelFactor2<Pose3, Point3> Base;
+    typedef NoiseModelFactorN<Pose3, Point3> Base;
 
     Point3 measured_; /** The measurement */
 
@@ -57,8 +57,8 @@ namespace gtsam {
     /** print */
     void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BiasedGPSFactor("
-          << keyFormatter(this->key1()) << ","
+          << keyFormatter(this->key<1>()) << ","
-          << keyFormatter(this->key2()) << ")\n"
+          << keyFormatter(this->key<2>()) << ")\n"
           << "  measured: " << measured_.transpose() << std::endl;
       this->noiseModel_->print("  noise model: ");
     }
@@ -96,6 +96,7 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
       ar & boost::serialization::make_nvp("NoiseModelFactor2",
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h

@@ -88,12 +88,12 @@ namespace gtsam {
  */
 
 template<class POSE, class VELOCITY, class IMUBIAS>
-class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
+class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
 
 private:
 
   typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
-  typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
+  typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
 
   Vector delta_pos_in_t0_;
   Vector delta_vel_in_t0_;
@@ -136,11 +136,11 @@ public:
   /** print */
   void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "("
-        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->template key<1>()) << ","
-        << keyFormatter(this->key2()) << ","
+        << keyFormatter(this->template key<2>()) << ","
-        << keyFormatter(this->key3()) << ","
+        << keyFormatter(this->template key<3>()) << ","
-        << keyFormatter(this->key4()) << ","
+        << keyFormatter(this->template key<4>()) << ","
-        << keyFormatter(this->key5()) << "\n";
+        << keyFormatter(this->template key<5>()) << "\n";
     std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
     std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
     std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h

@@ -87,12 +87,12 @@ namespace gtsam {
  */
 
 template<class POSE, class VELOCITY>
-class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> {
+class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> {
 
 private:
 
   typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
-  typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
+  typedef NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> Base;
 
   Vector delta_pos_in_t0_;
   Vector delta_vel_in_t0_;
@@ -136,10 +136,10 @@ public:
       const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << s << "("
-        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->key<1>()) << ","
-        << keyFormatter(this->key2()) << ","
+        << keyFormatter(this->key<2>()) << ","
-        << keyFormatter(this->key3()) << ","
+        << keyFormatter(this->key<3>()) << ","
-        << keyFormatter(this->key4()) << "\n";
+        << keyFormatter(this->key<4>()) << "\n";
     std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
     std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
     std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

gtsam_unstable/slam/GaussMarkov1stOrderFactor.h

@@ -42,12 +42,12 @@ namespace gtsam {
  * T is the measurement type, by default the same
  */
 template<class VALUE>
-class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
+class GaussMarkov1stOrderFactor: public NoiseModelFactorN<VALUE, VALUE> {
 
 private:
 
   typedef GaussMarkov1stOrderFactor<VALUE> This;
-  typedef NoiseModelFactor2<VALUE, VALUE> Base;
+  typedef NoiseModelFactorN<VALUE, VALUE> Base;
 
   double dt_;
   Vector tau_;
@@ -73,8 +73,8 @@ public:
   /** print */
   void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "GaussMarkov1stOrderFactor("
-        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->template key<1>()) << ","
-        << keyFormatter(this->key2()) << ")\n";
+        << keyFormatter(this->template key<2>()) << ")\n";
     this->noiseModel_->print("  noise model");
   }
 

gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h

@@ -77,12 +77,12 @@ namespace gtsam {
  *            vehicle
  */
 template<class POSE, class VELOCITY, class IMUBIAS>
-class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
+class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
 
 private:
 
   typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
-  typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
+  typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
 
   Vector measurement_acc_;
   Vector measurement_gyro_;
@@ -117,11 +117,11 @@ public:
   /** print */
   void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "("
-        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->template key<1>()) << ","
-        << keyFormatter(this->key2()) << ","
+        << keyFormatter(this->template key<2>()) << ","
-        << keyFormatter(this->key3()) << ","
+        << keyFormatter(this->template key<3>()) << ","
-        << keyFormatter(this->key4()) << ","
+        << keyFormatter(this->template key<4>()) << ","
-        << keyFormatter(this->key5()) << "\n";
+        << keyFormatter(this->template key<5>()) << "\n";
     std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
     std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
     std::cout << "dt: " << this->dt_ << std::endl;

gtsam_unstable/slam/InvDepthFactor3.h

@@ -24,7 +24,7 @@ namespace gtsam {
  * Ternary factor representing a visual measurement that includes inverse depth
  */
 template<class POSE, class LANDMARK, class INVDEPTH>
-class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
+class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -34,7 +34,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
+  typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base;
 
   /// shorthand for this class
   typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@@ -93,8 +93,8 @@ public:
       if (H1) *H1 = Matrix::Zero(2,6);
       if (H2) *H2 = Matrix::Zero(2,5);
       if (H3) *H3 = Matrix::Zero(2,1);
-      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
-          " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+          " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (Vector(1) << 0.0).finished();

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -24,7 +24,7 @@ namespace gtsam {
 /**
  * Binary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
+class InvDepthFactorVariant1: public NoiseModelFactorN<Pose3, Vector6> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -34,7 +34,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, Vector6> Base;
+  typedef NoiseModelFactorN<Pose3, Vector6> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant1 This;
@@ -93,8 +93,8 @@ public:
       return camera.project(world_P_landmark) - measured_;
     } catch( CheiralityException& e) {
       std::cout << e.what()
-          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
+          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
-          << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
+          << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
           << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();
     }

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -25,7 +25,7 @@ namespace gtsam {
 /**
  * Binary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
+class InvDepthFactorVariant2: public NoiseModelFactorN<Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -36,7 +36,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, Vector3> Base;
+  typedef NoiseModelFactorN<Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant2 This;
@@ -96,8 +96,8 @@ public:
       return camera.project(world_P_landmark) - measured_;
     } catch( CheiralityException& e) {
       std::cout << e.what()
-          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
+          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
-          << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
+          << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
           << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();
     }

gtsam_unstable/slam/InvDepthFactorVariant3.h

@@ -24,7 +24,7 @@ namespace gtsam {
 /**
  * Binary factor representing the first visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
+class InvDepthFactorVariant3a: public NoiseModelFactorN<Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -34,7 +34,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, Vector3> Base;
+  typedef NoiseModelFactorN<Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant3a This;
@@ -96,8 +96,8 @@ public:
       return camera.project(world_P_landmark) - measured_;
     } catch( CheiralityException& e) {
       std::cout << e.what()
-          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
+          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
-          << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
+          << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
           << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();
     }
@@ -150,7 +150,7 @@ private:
 /**
  * Ternary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
+class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -160,7 +160,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base;
+  typedef NoiseModelFactorN<Pose3, Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant3b This;
@@ -222,8 +222,8 @@ public:
       return camera.project(world_P_landmark) - measured_;
     } catch( CheiralityException& e) {
       std::cout << e.what()
-          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
+          << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
-          << " moved behind camera " << DefaultKeyFormatter(this->key2())
+          << " moved behind camera " << DefaultKeyFormatter(this->key<2>())
           << std::endl;
       return Vector::Ones(2) * 2.0 * K_->fx();
     }

gtsam_unstable/slam/LocalOrientedPlane3Factor.h

@@ -35,10 +35,10 @@ namespace gtsam {
  * optimized in x1 frame in the optimization.
  */
 class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
-    : public NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> {
+    : public NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> {
  protected:
   OrientedPlane3 measured_p_;
-  typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base;
+  typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
 public:
   /// Constructor
   LocalOrientedPlane3Factor() {}

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -187,8 +187,8 @@ namespace gtsam {
         if (H1) *H1 = Matrix::Zero(2,6);
         if (H2) *H2 = Matrix::Zero(2,3);
         if (verboseCheirality_)
-          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) <<
-              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+              " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl;
         if (throwCheirality_)
           throw e;
       }

gtsam_unstable/slam/PartialPriorFactor.h

@@ -35,7 +35,7 @@ namespace gtsam {
    * @tparam VALUE is the type of variable the prior effects
    */
   template<class VALUE>
-  class PartialPriorFactor: public NoiseModelFactor1<VALUE> {
+  class PartialPriorFactor: public NoiseModelFactorN<VALUE> {
 
   public:
     typedef VALUE T;
@@ -44,7 +44,7 @@ namespace gtsam {
     // Concept checks on the variable type - currently requires Lie
     GTSAM_CONCEPT_LIE_TYPE(VALUE)
 
-    typedef NoiseModelFactor1<VALUE> Base;
+    typedef NoiseModelFactorN<VALUE> Base;
     typedef PartialPriorFactor<VALUE> This;
 
     Vector prior_;                 ///< Measurement on tangent space parameters, in compressed form.
@@ -141,6 +141,7 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
       ar & boost::serialization::make_nvp("NoiseModelFactor1",
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);

gtsam_unstable/slam/PoseBetweenFactor.h

@@ -29,12 +29,12 @@ namespace gtsam {
    * @ingroup slam
    */
   template<class POSE>
-  class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {
+  class PoseBetweenFactor: public NoiseModelFactorN<POSE, POSE> {
 
   private:
 
     typedef PoseBetweenFactor<POSE> This;
-    typedef NoiseModelFactor2<POSE, POSE> Base;
+    typedef NoiseModelFactorN<POSE, POSE> Base;
 
     POSE measured_; /** The measurement */
     boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
@@ -68,8 +68,8 @@ namespace gtsam {
     /** print */
     void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
-          << keyFormatter(this->key1()) << ","
+          << keyFormatter(this->template key<1>()) << ","
-          << keyFormatter(this->key2()) << ")\n";
+          << keyFormatter(this->template key<2>()) << ")\n";
       measured_.print("  measured: ");
       if(this->body_P_sensor_)
         this->body_P_sensor_->print("  sensor pose in body frame: ");

gtsam_unstable/slam/PosePriorFactor.h

@@ -26,12 +26,12 @@ namespace gtsam {
    * @ingroup slam
    */
   template<class POSE>
-  class PosePriorFactor: public NoiseModelFactor1<POSE> {
+  class PosePriorFactor: public NoiseModelFactorN<POSE> {
 
   private:
 
     typedef PosePriorFactor<POSE> This;
-    typedef NoiseModelFactor1<POSE> Base;
+    typedef NoiseModelFactorN<POSE> Base;
 
     POSE prior_; /** The measurement */
     boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame

gtsam_unstable/slam/PoseToPointFactor.h

@@ -21,10 +21,10 @@ namespace gtsam {
  * @ingroup slam
  */
 template<typename POSE = Pose3, typename POINT = Point3>
-class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
+class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
  private:
   typedef PoseToPointFactor This;
-  typedef NoiseModelFactor2<POSE, POINT> Base;
+  typedef NoiseModelFactorN<POSE, POINT> Base;
 
   POINT measured_; /** the point measurement in local coordinates */
 
@@ -47,8 +47,9 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
   /** print */
   void print(const std::string& s, const KeyFormatter& keyFormatter =
                                        DefaultKeyFormatter) const override {
-    std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
+    std::cout << s << "PoseToPointFactor("
-              << keyFormatter(this->key2()) << ")\n"
+              << keyFormatter(this->template key<1>()) << ","
+              << keyFormatter(this->template key<2>()) << ")\n"
               << "  measured: " << measured_.transpose() << std::endl;
     this->noiseModel_->print("  noise model: ");
   }
@@ -91,8 +92,10 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar& boost::serialization::make_nvp(
-        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
+        "NoiseModelFactor2",
+        boost::serialization::base_object<Base>(*this));
     ar& BOOST_SERIALIZATION_NVP(measured_);
   }
 

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -31,7 +31,7 @@ namespace gtsam {
    * @ingroup slam
    */
   template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
-  class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
+  class ProjectionFactorPPP: public NoiseModelFactorN<POSE, POSE, LANDMARK> {
   protected:
 
     // Keep a copy of measurement and calibration for I/O
@@ -45,7 +45,7 @@ namespace gtsam {
   public:
 
     /// shorthand for base class type
-    typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
+    typedef NoiseModelFactorN<POSE, POSE, LANDMARK> Base;
 
     /// shorthand for this class
     typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
@@ -142,8 +142,11 @@ namespace gtsam {
         if (H2) *H2 = Matrix::Zero(2,6);
         if (H3) *H3 = Matrix::Zero(2,3);
         if (verboseCheirality_)
-          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+            std::cout << e.what() << ": Landmark "
-              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+                      << DefaultKeyFormatter(this->template key<2>())
+                      << " moved behind camera "
+                      << DefaultKeyFormatter(this->template key<1>())
+                      << std::endl;
         if (throwCheirality_)
           throw e;
       }

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -34,7 +34,7 @@ namespace gtsam {
    */
 template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
 class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
-    : public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
+    : public NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> {
  protected:
   Point2 measured_;  ///< 2D measurement
 
@@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
 
  public:
   /// shorthand for base class type
-  typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
+  typedef NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> Base;
 
   /// shorthand for this class
   typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
@@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
         if (H3) *H3 = Matrix::Zero(2,3);
         if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
         if (verboseCheirality_)
-          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
-              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+              " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
         if (throwCheirality_)
           throw e;
       }

gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp

@@ -54,9 +54,9 @@ Vector ProjectionFactorRollingShutter::evaluateError(
     if (H3) *H3 = Matrix::Zero(2, 3);
     if (verboseCheirality_)
       std::cout << e.what() << ": Landmark "
-                << DefaultKeyFormatter(this->key2()) << " moved behind camera "
+                << DefaultKeyFormatter(this->key<2>()) << " moved behind camera "
-                << DefaultKeyFormatter(this->key1()) << std::endl;
+                << DefaultKeyFormatter(this->key<1>()) << std::endl;
-    if (throwCheirality_) throw CheiralityException(this->key2());
+    if (throwCheirality_) throw CheiralityException(this->key<2>());
   }
   return Vector2::Constant(2.0 * K_->fx());
 }

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -42,7 +42,7 @@ namespace gtsam {
  */
 
 class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
-    : public NoiseModelFactor3<Pose3, Pose3, Point3> {
+    : public NoiseModelFactorN<Pose3, Pose3, Point3> {
  protected:
   // Keep a copy of measurement and calibration for I/O
   Point2 measured_;  ///< 2D measurement
@@ -60,7 +60,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
 
  public:
   /// shorthand for base class type
-  typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
+  typedef NoiseModelFactorN<Pose3, Pose3, Point3> Base;
 
   /// shorthand for this class
   typedef ProjectionFactorRollingShutter This;

gtsam_unstable/slam/RelativeElevationFactor.h

@@ -25,13 +25,13 @@ namespace gtsam {
  *
  * TODO: enable use of a Pose3 for the target as well
  */
-class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> {
+class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN<Pose3, Point3> {
 private:
 
   double measured_; /** measurement */
 
   typedef RelativeElevationFactor This;
-  typedef NoiseModelFactor2<Pose3, Point3> Base;
+  typedef NoiseModelFactorN<Pose3, Point3> Base;
 
 public:
 
@@ -66,6 +66,7 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
     ar & boost::serialization::make_nvp("NoiseModelFactor2",
         boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);

gtsam_unstable/slam/TSAMFactors.h

@@ -26,11 +26,11 @@ namespace gtsam {
 /**
  * DeltaFactor: relative 2D measurement between Pose2 and Point2
  */
-class DeltaFactor: public NoiseModelFactor2<Pose2, Point2> {
+class DeltaFactor: public NoiseModelFactorN<Pose2, Point2> {
 
 public:
   typedef DeltaFactor This;
-  typedef NoiseModelFactor2<Pose2, Point2> Base;
+  typedef NoiseModelFactorN<Pose2, Point2> Base;
   typedef boost::shared_ptr<This> shared_ptr;
 
 private:
@@ -55,11 +55,11 @@ public:
 /**
  * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes
  */
-class DeltaFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> {
+class DeltaFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> {
 
 public:
   typedef DeltaFactorBase This;
-  typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> Base;
+  typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> Base;
   typedef boost::shared_ptr<This> shared_ptr;
 
 private:
@@ -110,11 +110,11 @@ public:
 /**
  * OdometryFactorBase: Pose2 odometry, with Basenodes
  */
-class OdometryFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> {
+class OdometryFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> {
 
 public:
   typedef OdometryFactorBase This;
-  typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> Base;
+  typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> Base;
   typedef boost::shared_ptr<This> shared_ptr;
 
 private:

gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h

@@ -32,7 +32,7 @@ namespace gtsam {
    * @ingroup slam
    */
   template<class VALUE>
-  class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?
+  class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ?
 
   public:
 

tests/simulated2D.h

@@ -124,9 +124,9 @@ namespace simulated2D {
    *  Unary factor encoding a soft prior on a vector
    */
   template<class VALUE = Point2>
-  class GenericPrior: public NoiseModelFactor1<VALUE> {
+  class GenericPrior: public NoiseModelFactorN<VALUE> {
   public:
-    typedef NoiseModelFactor1<VALUE> Base;  ///< base class
+    typedef NoiseModelFactorN<VALUE> Base;  ///< base class
     typedef GenericPrior<VALUE> This;
     typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
@@ -168,9 +168,9 @@ namespace simulated2D {
    * Binary factor simulating "odometry" between two Vectors
    */
   template<class VALUE = Point2>
-  class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
+  class GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
   public:
-    typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class
+    typedef NoiseModelFactorN<VALUE, VALUE> Base; ///< base class
     typedef GenericOdometry<VALUE> This;
     typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
@@ -214,9 +214,9 @@ namespace simulated2D {
    * Binary factor simulating "measurement" between two Vectors
    */
   template<class POSE, class LANDMARK>
-  class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> {
+  class GenericMeasurement: public NoiseModelFactorN<POSE, LANDMARK> {
   public:
-    typedef NoiseModelFactor2<POSE, LANDMARK> Base;  ///< base class
+    typedef NoiseModelFactorN<POSE, LANDMARK> Base;  ///< base class
     typedef GenericMeasurement<POSE, LANDMARK> This;
     typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
     typedef POSE Pose; ///< shortcut to Pose type

tests/simulated2DOriented.h

@@ -80,13 +80,13 @@ namespace simulated2DOriented {
 
   /// Unary factor encoding a soft prior on a vector
   template<class VALUE = Pose2>
-  struct GenericPosePrior: public NoiseModelFactor1<VALUE> {
+  struct GenericPosePrior: public NoiseModelFactorN<VALUE> {
 
     Pose2 measured_; ///< measurement
 
     /// Create generic pose prior
     GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
-      NoiseModelFactor1<VALUE>(model, key), measured_(measured) {
+      NoiseModelFactorN<VALUE>(model, key), measured_(measured) {
     }
 
     /// Evaluate error and optionally derivative
@@ -101,7 +101,7 @@ namespace simulated2DOriented {
    * Binary factor simulating "odometry" between two Vectors
    */
   template<class VALUE = Pose2>
-  struct GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
+  struct GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
     Pose2 measured_;   ///< Between measurement for odometry factor
 
     typedef GenericOdometry<VALUE> This;
@@ -111,7 +111,7 @@ namespace simulated2DOriented {
      */
     GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
         Key i1, Key i2) :
-          NoiseModelFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
+          NoiseModelFactorN<VALUE, VALUE>(model, i1, i2), measured_(measured) {
     }
 
     ~GenericOdometry() override {}