Replace NoiseModelFactor1 in with NoiseModelFactorN in pre-made factors

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> {
-  typedef NoiseModelFactor1<Pose3> Base;
+class ResectioningFactor: public NoiseModelFactorN<Pose3> {
+  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;
-          key2 = pose3Between->key2() + firstKey;
+          key1 = pose3Between->key<1>() + firstKey;
+          key2 = pose3Between->key<2>() + firstKey;
         }else{
-          key1 = pose3Between->key1() - firstKey;
-          key2 = pose3Between->key2() - firstKey;
+          key1 = pose3Between->key<1>() - 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 NoiseModelFactor2<Pose,Pose> NM2;
+typedef NoiseModelFactorN<Pose> NM1;
+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,34 +313,34 @@ 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
         newFactors.push_back(factor);
         const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(factor->key1() > factor->key2()) {
-          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
+        if(factor->key<1>() > factor->key<2>()) {
+          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());
-              newVariables.insert(factor->key1(), prevPose * measured.inverse());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
+              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());
-              newVariables.insert(factor->key2(), prevPose * measured);
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
+              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__
-#  define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
+#ifdef __GNUC__
+#  define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \
+  _Pragma("GCC diagnostic push") \
+  DO_PRAGMA(GCC diagnostic ignored diag)
+#else
+#  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 CLANG_DIAGNOSTIC_POP()
+#  define DIAGNOSTIC_POP()
 #endif
 
 namespace gtsam {

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 key2 = factor->key2();
+    KEY key1 = factor->template key<1>();
+    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 key2 = factor2->key2();
+    KEY key1 = factor2->template key<1>();
+    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/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()) << ","
-      << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
+  cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << ","
+      << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ",";
   _PIM_.print("  preintegrated measurements:");
   noiseModel_->print("  noise model: ");
 }

gtsam/navigation/AHRSFactor.h

@@ -128,10 +128,10 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
   }
 };
 
-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 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
   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 @@ class AttitudeFactor {
  * 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 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public At
   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 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
   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(key2()) << "\n";
+         << keyFormatter(key<1>()) << "Barometric Bias on "
+         << 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->key3()) << "," << keyFormatter(this->key4()) << ","
-       << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
+       << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
+       << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << ","
+       << 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 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType
  *
  * @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 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, P
   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{};
 
     /**