Merge pull request #1851 from borglab/feature/linearize

Better HybridNonlinear!

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
+#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <memory>
@@ -29,7 +30,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
   DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
   FactorValuePairs factorTree;
 
-  void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
+  void copyOrCheckContinuousKeys(const NoiseModelFactor::shared_ptr& factor) {
     if (!factor) return;
     if (continuousKeys.empty()) {
       continuousKeys = factor->keys();
@@ -40,7 +41,7 @@ struct HybridNonlinearFactor::ConstructorHelper {
   }
 
   ConstructorHelper(const DiscreteKey& discreteKey,
-                    const std::vector<NonlinearFactor::shared_ptr>& factors)
+                    const std::vector<NoiseModelFactor::shared_ptr>& factors)
       : discreteKeys({discreteKey}) {
     std::vector<NonlinearFactorValuePair> pairs;
     // Extract continuous keys from the first non-null factor
@@ -78,7 +79,7 @@ HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
 
 HybridNonlinearFactor::HybridNonlinearFactor(
     const DiscreteKey& discreteKey,
-    const std::vector<NonlinearFactor::shared_ptr>& factors)
+    const std::vector<NoiseModelFactor::shared_ptr>& factors)
     : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
 
 HybridNonlinearFactor::HybridNonlinearFactor(
@@ -158,8 +159,7 @@ bool HybridNonlinearFactor::equals(const HybridFactor& other,
   // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
   auto compare = [tol](const std::pair<sharedFactor, double>& a,
                        const std::pair<sharedFactor, double>& b) {
-    return traits<NonlinearFactor>::Equals(*a.first, *b.first, tol) &&
-           (a.second == b.second);
+    return a.first->equals(*b.first, tol) && (a.second == b.second);
   };
   if (!factors_.equals(f.factors_, compare)) return false;
 
@@ -185,7 +185,15 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
       [continuousValues](
           const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
     auto [factor, val] = f;
-    return {factor->linearize(continuousValues), val};
+    if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
+            factor->noiseModel())) {
+      return {factor->linearize(continuousValues),
+              val + gaussian->negLogConstant()};
+    } else {
+      throw std::runtime_error(
+          "HybridNonlinearFactor: linearize() only supports NoiseModelFactors "
+          "with Gaussian (or derived) noise models.");
+    }
   };
 
   DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>

gtsam/hybrid/HybridNonlinearFactor.h

@@ -26,25 +26,23 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Symbol.h>
 
-#include <algorithm>
-#include <cmath>
-#include <limits>
 #include <vector>
 
 namespace gtsam {
 
-/// Alias for a NonlinearFactor shared pointer and double scalar pair.
-using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>;
+/// Alias for a NoiseModelFactor shared pointer and double scalar pair.
+using NonlinearFactorValuePair =
+    std::pair<NoiseModelFactor::shared_ptr, double>;
 
 /**
  * @brief Implementation of a discrete-conditioned hybrid factor.
  *
  * Implements a joint discrete-continuous factor where the discrete variable
- * serves to "select" a hybrid component corresponding to a NonlinearFactor.
+ * serves to "select" a hybrid component corresponding to a NoiseModelFactor.
  *
  * This class stores all factors as HybridFactors which can then be typecast to
- * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform
- * the correct operation.
+ * one of (NoiseModelFactor, GaussianFactor) which can then be checked to
+ * perform the correct operation.
  *
  * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e.,
  * the negative log-likelihood for a Gaussian noise model.
@@ -62,11 +60,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   using Base = HybridFactor;
   using This = HybridNonlinearFactor;
   using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
-  using sharedFactor = std::shared_ptr<NonlinearFactor>;
+  using sharedFactor = std::shared_ptr<NoiseModelFactor>;
 
   /**
    * @brief typedef for DecisionTree which has Keys as node labels and
-   * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
+   * pairs of NoiseModelFactor & an arbitrary scalar as leaf nodes.
    */
   using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;
 
@@ -95,7 +93,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    */
   HybridNonlinearFactor(
       const DiscreteKey& discreteKey,
-      const std::vector<NonlinearFactor::shared_ptr>& factors);
+      const std::vector<NoiseModelFactor::shared_ptr>& factors);
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,

gtsam/hybrid/hybrid.i

@@ -245,16 +245,16 @@ class HybridNonlinearFactorGraph {
 class HybridNonlinearFactor : gtsam::HybridFactor {
   HybridNonlinearFactor(
       const gtsam::DiscreteKey& discreteKey,
-      const std::vector<gtsam::NonlinearFactor*>& factors);
+      const std::vector<gtsam::NoiseModelFactor*>& factors);
 
   HybridNonlinearFactor(
       const gtsam::DiscreteKey& discreteKey,
-      const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
+      const std::vector<std::pair<gtsam::NoiseModelFactor*, double>>& factors);
 
   HybridNonlinearFactor(
       const gtsam::DiscreteKeys& discreteKeys,
       const gtsam::DecisionTree<
-          gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
+          gtsam::Key, std::pair<gtsam::NoiseModelFactor*, double>>& factors);
 
   double error(const gtsam::Values& continuousValues,
                const gtsam::DiscreteValues& discreteValues) const;

gtsam/hybrid/tests/Switching.h

@@ -33,6 +33,7 @@
 
 #include "gtsam/linear/GaussianFactor.h"
 #include "gtsam/linear/GaussianFactorGraph.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 #pragma once
 
@@ -185,7 +186,7 @@ struct Switching {
   }
 
   // Create motion models for a given time step
-  static std::vector<NonlinearFactor::shared_ptr> motionModels(
+  static std::vector<NoiseModelFactor::shared_ptr> motionModels(
       size_t k, double sigma = 1.0) {
     auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
     auto still =

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -24,6 +24,7 @@
 
 #include "Switching.h"
 #include "TinyHybridExample.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -389,7 +390,7 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
       DiscreteKey(M(0), 2),
-      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
+      std::vector<NoiseModelFactor::shared_ptr>{zero_motion, one_motion});
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -39,6 +39,7 @@
 #include <bitset>
 
 #include "Switching.h"
+#include "gtsam/nonlinear/NonlinearFactor.h"
 
 using namespace std;
 using namespace gtsam;
@@ -435,8 +436,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   const auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
-                                                         one_motion};
+  std::vector<NoiseModelFactor::shared_ptr> components = {zero_motion,
+                                                          one_motion};
   nfg.emplace_shared<HybridNonlinearFactor>(m, components);
 
   return nfg;
@@ -526,49 +527,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
   }
 }
 
-/****************************************************************************/
-/**
- * Helper function to add the constant term corresponding to
- * the difference in noise models.
- */
-std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
-    const HybridNonlinearFactor& mf, const Values& initial, const Key& mode,
-    double noise_tight, double noise_loose, size_t d, size_t tight_index) {
-  HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial);
-
-  constexpr double log2pi = 1.8378770664093454835606594728112;
-  // logConstant will be of the tighter model
-  double logNormalizationConstant = log(1.0 / noise_tight);
-  double logConstant = -0.5 * d * log2pi + logNormalizationConstant;
-
-  auto func = [&](const Assignment<Key>& assignment,
-                  const GaussianFactor::shared_ptr& gf) {
-    if (assignment.at(mode) != tight_index) {
-      double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);
-
-      GaussianFactorGraph _gfg;
-      _gfg.push_back(gf);
-      Vector c(d);
-      for (size_t i = 0; i < d; i++) {
-        c(i) = std::sqrt(2.0 * (logConstant - factor_log_constant));
-      }
-
-      _gfg.emplace_shared<JacobianFactor>(c);
-      return std::make_shared<JacobianFactor>(_gfg);
-    } else {
-      return dynamic_pointer_cast<JacobianFactor>(gf);
-    }
-  };
-  auto updated_components = gmf->factors().apply(func);
-  auto updated_pairs = HybridGaussianFactor::FactorValuePairs(
-      updated_components,
-      [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
-        return {gf, 0.0};
-      });
-  return std::make_shared<HybridGaussianFactor>(gmf->discreteKeys(),
-                                                updated_pairs);
-}
-
 /****************************************************************************/
 TEST(HybridEstimation, ModeSelection) {
   HybridNonlinearFactorGraph graph;
@@ -588,15 +546,14 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};
 
   HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert(X(0), 0.0);
   initial.insert(X(1), 0.0);
 
-  auto gmf =
-      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
+  auto gmf = mf.linearize(initial);
   graph.add(gmf);
 
   auto gfg = graph.linearize(initial);
@@ -676,15 +633,14 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<NoiseModelFactor::shared_ptr> components = {model0, model1};
 
   HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);
   initial.insert<Vector3>(X(1), Z_3x1);
 
-  auto gmf =
-      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
+  auto gmf = mf.linearize(initial);
   graph.add(gmf);
 
   auto gfg = graph.linearize(initial);