Change NoiseModelFactorN to be an alias to NoiseModelFactorT

Author: ProfFan

gtsam/inference/BayesTreeCliqueBase-inst.h

@@ -48,7 +48,7 @@ namespace gtsam {
     KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
     KeyVector S_setminus_B;
     std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
-      indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
+      indicesB.begin(), indicesB.end(), std::back_inserter(S_setminus_B));
     return S_setminus_B;
   }
 
@@ -64,10 +64,10 @@ namespace gtsam {
     KeyVector keep;
     // keep = S\B intersect allKeys (S_setminus_B is already sorted)
     std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
-      allKeys.begin(), allKeys.end(), back_inserter(keep));
+      allKeys.begin(), allKeys.end(), std::back_inserter(keep));
     // keep += B intersect allKeys
     std::set_intersection(indicesB.begin(), indicesB.end(), //
-      allKeys.begin(), allKeys.end(), back_inserter(keep));
+      allKeys.begin(), allKeys.end(), std::back_inserter(keep));
     return keep;
   }
 

gtsam/navigation/MagFactor.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 

gtsam/nonlinear/NoiseModelFactorN.h

@@ -31,7 +31,7 @@ namespace detail {
 /** Convenience base class to add aliases `X1`, `X2`, ..., `X6` -> ValueType<N>.
  * Usage example:
  * ```
- * class MyFactor : public NoiseModelFactorN<Pose3, Point3>,
+ * class MyFactor : public NoiseModelFactorT<Pose3, Point3>,
  *                  public NoiseModelFactorAliases<Pose3, Point3> {
  *  // class implementation ...
  * };
@@ -94,9 +94,9 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
  * between a Pose3 and Point3 could be implemented like so:
  *
  * ~~~~~~~~~~~~~~~~~~~~{.cpp}
- * class MyFactor : public NoiseModelFactorN<Pose3, Point3> {
+ * class MyFactor : public NoiseModelFactorT<Vector, Pose3, Point3> {
  *  public:
- *   using Base = NoiseModelFactorN<Pose3, Point3>;
+ *   using Base = NoiseModelFactorT<Pose3, Point3>;
  *
  *   MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel)
  *       : Base(noiseModel, pose_key, point_key) {}
@@ -121,7 +121,7 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
  *     return Vector1(error);
  *   }
  * };
- * 
+ *
  * // Unit Test
  * TEST(NonlinearFactor, MyFactor) {
  *   MyFactor f(X(1), X(2), noiseModel::Unit::Create(1));
@@ -138,8 +138,8 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
  * are typically more general than just vectors, e.g., Rot3 or Pose3, which are
  * objects in non-linear manifolds (Lie groups).
  */
-template <class... ValueTypes>
-class NoiseModelFactorN
+template <class OutputVec, class... ValueTypes>
+class NoiseModelFactorT
     : public NoiseModelFactor,
       public detail::NoiseModelFactorAliases<ValueTypes...> {
  public:
@@ -150,7 +150,7 @@ class NoiseModelFactorN
 
 protected:
   using Base = NoiseModelFactor;
-  using This = NoiseModelFactorN<ValueTypes...>;
+  using This = NoiseModelFactorT<OutputVec, ValueTypes...>;
 
   /// @name SFINAE aliases
   /// @{
@@ -212,88 +212,88 @@ class NoiseModelFactorN
   using MatrixTypeT = Matrix;
 
   public:
-  /**
-   * The type of the I'th template param can be obtained as ValueType<I>.
-   * I is 1-indexed for backwards compatibility/consistency!  So for example,
-   * ```
-   * using Factor = NoiseModelFactorN<Pose3, Point3>;
-   * Factor::ValueType<1>  // Pose3
-   * Factor::ValueType<2>  // Point3
-   * // Factor::ValueType<0> // ERROR!  Will not compile.
-   * // Factor::ValueType<3> // ERROR!  Will not compile.
-   * ```
-   *
-   * You can also use the shortcuts `X1`, ..., `X6` which are the same as
-   * `ValueType<1>`, ..., `ValueType<6>` respectively (see
-   * detail::NoiseModelFactorAliases).
-   *
-   * Note that, if your class is templated AND you want to use `ValueType<1>`
-   * inside your class, due to dependent types you need the `template` keyword:
-   * `typename MyFactor<T>::template ValueType<1>`.
-   */
-  template <int I, typename = IndexIsValid<I>>
-  using ValueType =
-      typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
+   /**
+    * The type of the I'th template param can be obtained as ValueType<I>.
+    * I is 1-indexed for backwards compatibility/consistency!  So for example,
+    * ```
+    * using Factor = NoiseModelFactorT<Vector, Pose3, Point3>;
+    * Factor::ValueType<1>  // Pose3
+    * Factor::ValueType<2>  // Point3
+    * // Factor::ValueType<0> // ERROR!  Will not compile.
+    * // Factor::ValueType<3> // ERROR!  Will not compile.
+    * ```
+    *
+    * You can also use the shortcuts `X1`, ..., `X6` which are the same as
+    * `ValueType<1>`, ..., `ValueType<6>` respectively (see
+    * detail::NoiseModelFactorAliases).
+    *
+    * Note that, if your class is templated AND you want to use `ValueType<1>`
+    * inside your class, due to dependent types you need the `template` keyword:
+    * `typename MyFactor<T>::template ValueType<1>`.
+    */
+   template <int I, typename = IndexIsValid<I>>
+   using ValueType =
+       typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
 
- public:
-
-  /// @name Constructors
-  /// @{
-
-  /// Default Constructor for I/O
-  NoiseModelFactorN() {}
-
-  /**
-   * Constructor.
-   * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN)
-   * @param noiseModel Shared pointer to noise model.
-   * @param keys Keys for the variables in this factor, passed in as separate
-   * arguments.
-   */
-  NoiseModelFactorN(const SharedNoiseModel& noiseModel,
-                    KeyType<ValueTypes>... keys)
-      : Base(noiseModel, std::array<Key, N>{keys...}) {}
-
-  /**
-   * Constructor.
-   * Example usage: `NoiseModelFactorN(noise, {key1, key2, ..., keyN})`
-   * Example usage: `NoiseModelFactorN(noise, keys)` where keys is a vector<Key>
-   * @param noiseModel Shared pointer to noise model.
-   * @param keys A container of keys for the variables in this factor.
-   */
-  template <typename CONTAINER = std::initializer_list<Key>,
-            typename = IsContainerOfKeys<CONTAINER>>
-  NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
-      : Base(noiseModel, keys) {
-    if (keys.size() != N) {
-      throw std::invalid_argument(
-          "NoiseModelFactorN: wrong number of keys given");
-    }
-  }
+  public:
+   /// @name Constructors
+   /// @{
+
+   /// Default Constructor for I/O
+   NoiseModelFactorT() {}
+
+   /**
+    * Constructor.
+    * Example usage: NoiseModelFactorT(noise, key1, key2, ..., keyN)
+    * @param noiseModel Shared pointer to noise model.
+    * @param keys Keys for the variables in this factor, passed in as separate
+    * arguments.
+    */
+   NoiseModelFactorT(const SharedNoiseModel& noiseModel,
+                     KeyType<ValueTypes>... keys)
+       : Base(noiseModel, std::array<Key, N>{keys...}) {}
+
+   /**
+    * Constructor.
+    * Example usage: `NoiseModelFactorT(noise, {key1, key2, ..., keyN})`
+    * Example usage: `NoiseModelFactorT(noise, keys)` where keys is a
+    * vector<Key>
+    * @param noiseModel Shared pointer to noise model.
+    * @param keys A container of keys for the variables in this factor.
+    */
+   template <typename CONTAINER = std::initializer_list<Key>,
+             typename = IsContainerOfKeys<CONTAINER>>
+   NoiseModelFactorT(const SharedNoiseModel& noiseModel, CONTAINER keys)
+       : Base(noiseModel, keys) {
+     if (keys.size() != N) {
+       throw std::invalid_argument(
+           "NoiseModelFactorT: wrong number of keys given");
+     }
+   }
 
   /// @}
 
-  ~NoiseModelFactorN() override {}
-
-  /** Returns a key. Usage: `key<I>()` returns the I'th key.
-   * I is 1-indexed for backwards compatibility/consistency!  So for example,
-   * ```
-   * NoiseModelFactorN<Pose3, Point3> factor(noise, key1, key2);
-   * key<1>()  // = key1
-   * key<2>()  // = key2
-   * // key<0>()  // ERROR!  Will not compile
-   * // key<3>()  // ERROR!  Will not compile
-   * ```
-   * 
-   * Note that, if your class is templated AND you are trying to call `key<1>`
-   * inside your class, due to dependent types you need the `template` keyword:
-   * `this->key1()`.
-   */
-  template <int I = 1>
-  inline Key key() const {
-    static_assert(I <= N, "Index out of bounds");
-    return keys_[I - 1];
-  }
+   ~NoiseModelFactorT() override {}
+
+   /** Returns a key. Usage: `key<I>()` returns the I'th key.
+    * I is 1-indexed for backwards compatibility/consistency!  So for example,
+    * ```
+    * NoiseModelFactorT<Vector, Pose3, Point3> factor(noise, key1, key2);
+    * key<1>()  // = key1
+    * key<2>()  // = key2
+    * // key<0>()  // ERROR!  Will not compile
+    * // key<3>()  // ERROR!  Will not compile
+    * ```
+    *
+    * Note that, if your class is templated AND you are trying to call `key<1>`
+    * inside your class, due to dependent types you need the `template` keyword:
+    * `this->key1()`.
+    */
+   template <int I = 1>
+   inline Key key() const {
+     static_assert(I <= N, "Index out of bounds");
+     return keys_[I - 1];
+   }
 
   /// @name NoiseModelFactor methods
   /// @{
@@ -347,15 +347,15 @@ class NoiseModelFactorN
    * as separate arguments.
    * @param[out] H The Jacobian with respect to each variable (optional).
    */
-  virtual Vector evaluateError(const ValueTypes&... x,
+  virtual OutputVec evaluateError(const ValueTypes&... x,
                                OptionalMatrixTypeT<ValueTypes>... H) const = 0;
 
   /** If all the optional arguments are matrices then redirect the call to
    * the one which takes pointers.
    * To get access to this version of the function from derived classes
    * one will need to use the "using" keyword and specify that like this:
    * public:
-   *   using NoiseModelFactorN<list the value types here>::evaluateError;
+   *   using NoiseModelFactorT<list the value types here>::evaluateError;
    */
   Vector evaluateError(const ValueTypes&... x,
                        MatrixTypeT<ValueTypes>&... H) const {
@@ -406,7 +406,7 @@ class NoiseModelFactorN
   /** Pack expansion with index_sequence template pattern, used to index into
    * `keys_` and `H`.
    *
-   * Example: For `NoiseModelFactorN<Pose3, Point3>`, the call would look like:
+   * Example: For `NoiseModelFactorT<Pose3, Point3>`, the call would look like:
    *    `const Vector error = unwhitenedError(0, 1, values, H);`
    */
   template <std::size_t... Indices>
@@ -471,7 +471,10 @@ class NoiseModelFactorN
 
   /// @}
 
-};  // \class NoiseModelFactorN
+};  // \class NoiseModelFactorT
+
+template <class... ValueTypes>
+using NoiseModelFactorN = NoiseModelFactorT<Vector, ValueTypes...>;
 
 #define NoiseModelFactor1 NoiseModelFactorN
 #define NoiseModelFactor2 NoiseModelFactorN

gtsam/slam/PlanarProjectionFactor.h

@@ -14,18 +14,17 @@
  */
 #pragma once
 
-#include <gtsam/base/Testable.h>
 #include <gtsam/base/Lie.h>
-
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/numericalDerivative.h>
-
 
 namespace gtsam {
 
@@ -139,7 +138,7 @@ namespace gtsam {
             const Cal3DS2& calib,
             const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
-            NoiseModelFactorN(model, poseKey),
+            NoiseModelFactorN<Pose2>(model, poseKey),
             landmark_(landmark),
             bTc_(bTc),
             calib_(calib) {}
@@ -201,7 +200,7 @@ namespace gtsam {
             const Cal3DS2& calib,
             const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
-            NoiseModelFactorN(model, poseKey, landmarkKey),
+            NoiseModelFactorN<Pose2, Point3>(model, poseKey, landmarkKey),
             bTc_(bTc),
             calib_(calib) {}
 
@@ -258,16 +257,13 @@ namespace gtsam {
          * @param measured corresponding point2 in the camera frame
          * @param model stddev of the measurements, ~one pixel?
          */
-        PlanarProjectionFactor3(
-            Key poseKey,
-            Key offsetKey,
-            Key calibKey,
-            const Point3& landmark,
-            const Point2& measured,
-            const SharedNoiseModel& model = {})
+        PlanarProjectionFactor3(Key poseKey, Key offsetKey, Key calibKey,
+                                const Point3& landmark, const Point2& measured,
+                                const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
-            NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
-            landmark_(landmark) {}
+              NoiseModelFactorN<Pose2, Pose3, Cal3DS2>(model, poseKey,
+                                                       offsetKey, calibKey),
+              landmark_(landmark) {}
 
         /**
          * @param wTb "world to body": estimated pose2

tests/testNonlinearFactor.cpp

@@ -31,6 +31,7 @@
 #include <tests/simulated2D.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/NoiseModelFactorN.h>
 #include <gtsam/inference/Symbol.h>
 
 using namespace std;
@@ -344,7 +345,7 @@ class TestFactor1 : public NoiseModelFactor1<double> {
   TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
 
   // Provide access to the Matrix& version of evaluateError:
-  using Base::NoiseModelFactor1;  // inherit constructors
+  using Base::NoiseModelFactorT;  // inherit constructors
 
   Vector evaluateError(const double& x1, OptionalMatrixType H1) const override {
     if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
@@ -401,7 +402,7 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
   TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
 
   // Provide access to the Matrix& version of evaluateError:
-  using Base::NoiseModelFactor4;  // inherit constructors
+  using Base::NoiseModelFactorT;  // inherit constructors
 
   Vector
     evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,