Merge pull request #29 from varunagrawal/fix/hybrid-elimination

gtsam/hybrid/GaussianHybridFactorGraph.cpp

@@ -48,12 +48,20 @@ using Sum = DCGaussianMixtureFactor::Sum;
 // This is the version for Gaussian Factors
 static Sum& operator+=(Sum& sum, const GaussianFactor::shared_ptr& factor) {
   using Y = GaussianFactorGraph;
-  auto add = [&factor](const Y& graph) {
-    auto result = graph;
+  // If the decision tree is not intiialized, then intialize it.
+  if (sum.empty()) {
+    GaussianFactorGraph result;
     result.push_back(factor);
-    return result;
-  };
-  sum = sum.apply(add);
+    sum = Sum(result);
+
+  } else {
+    auto add = [&factor](const Y& graph) {
+      auto result = graph;
+      result.push_back(factor);
+      return result;
+    };
+    sum = sum.apply(add);
+  }
   return sum;
 }
 

gtsam/hybrid/GaussianHybridFactorGraph.h

@@ -163,6 +163,10 @@ class GTSAM_EXPORT GaussianHybridFactorGraph
    * Takes all factors, which *must* be all DCGaussianMixtureFactors or
    * GaussianFactors, and "add" them. This might involve decision-trees of
    * different structure, and creating a different decision tree for Gaussians.
+   *
+   * If no discrete variables are involved, i.e. only GaussianFactors provided
+   * or "leaf-only" DCGaussianMixtureFactors, then we will return a leaf-only
+   * DecisionTree with a single GaussianFactorGraph.
    */
   DCGaussianMixtureFactor::Sum sum() const;
 

gtsam/hybrid/HybridBayesNet.cpp

@@ -1,16 +1,41 @@
-//
-// Created by Fan Jiang on 1/24/22.
-//
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   HybridBayesNet.cpp
+ * @brief  A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author Fan Jiang
+ * @date   January 2022
+ */
 
 #include <gtsam/hybrid/HybridBayesNet.h>
 
 namespace gtsam {
 
-GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) {
+GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
   return boost::dynamic_pointer_cast<GaussianMixture>(factors_.at(i));
 }
-DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) {
+
+DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
   return boost::dynamic_pointer_cast<DiscreteConditional>(factors_.at(i));
 }
 
+GaussianBayesNet HybridBayesNet::choose(
+    const DiscreteValues& assignment) const {
+  GaussianBayesNet gbn;
+  for (size_t idx = 0; idx < size(); idx++) {
+    GaussianMixture gm = *this->atGaussian(idx);
+    gbn.push_back(gm(assignment));
+  }
+  return gbn;
 }
+
+}  // namespace gtsam

gtsam/hybrid/HybridBayesNet.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    HybridBayesNet.h
- * @brief   A set of GaussianFactors, indexed by a set of discrete keys.
+ * @brief   A bayes net of Gaussian Conditionals indexed by discrete keys.
  * @author  Varun Agrawal
  * @author  Fan Jiang
  * @author  Frank Dellaert
@@ -24,6 +24,7 @@
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/BayesNet.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianConditional.h>
 
 #include <iostream>  // TODO!
@@ -56,14 +57,16 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<AbstractConditional> {
    * (this checks array bounds and may throw an exception, as opposed to
    * operator[] which does not).
    */
-  GaussianMixture::shared_ptr atGaussian(size_t i);
+  GaussianMixture::shared_ptr atGaussian(size_t i) const;
 
   /**
    * Get a specific Gaussian mixture factor by index
    * (this checks array bounds and may throw an exception, as opposed to
    * operator[] which does not).
    */
-  DiscreteConditional::shared_ptr atDiscrete(size_t i);
+  DiscreteConditional::shared_ptr atDiscrete(size_t i) const;
+
+  GaussianBayesNet choose(const DiscreteValues &assignment) const;
 };
 
 }  // namespace gtsam

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -11,14 +11,15 @@
 
 /**
  * @file    testHybridBayesNet.cpp
- * @brief   Unit tests for DCFactorGraph
+ * @brief   Unit tests for HybridBayesNet
  * @author  Varun Agrawal
  * @author  Fan Jiang
  * @author  Frank Dellaert
  * @date    December 2021
  */
 
 #include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/tests/Switching.h>
 
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
@@ -45,6 +46,43 @@ TEST(HybridBayesNet, Creation) {
   EXPECT(df.equals(expected));
 }
 
+/* ****************************************************************************/
+// Test choosing an assignment of conditionals
+TEST(HybridBayesNet, Choose) {
+  Switching s(4);
+
+  Ordering ordering;
+  for (auto&& kvp : s.linearizationPoint) {
+    ordering += kvp.key;
+  }
+  HybridBayesNet::shared_ptr hybridBayesNet;
+  GaussianHybridFactorGraph::shared_ptr remainingFactorGraph;
+  std::tie(hybridBayesNet, remainingFactorGraph) =
+      s.linearizedFactorGraph.eliminatePartialSequential(ordering);
+
+  DiscreteValues assignment;
+  assignment[M(1)] = 1;
+  assignment[M(2)] = 1;
+  assignment[M(3)] = 0;
+
+  GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
+
+  EXPECT_LONGS_EQUAL(4, gbn.size());
+
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(0)))(assignment),
+                      *gbn.at(0)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(1)))(assignment),
+                      *gbn.at(1)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(2)))(assignment),
+                      *gbn.at(2)));
+  EXPECT(assert_equal(*(*boost::dynamic_pointer_cast<GaussianMixture>(
+                          hybridBayesNet->atGaussian(3)))(assignment),
+                      *gbn.at(3)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/hybrid/tests/testHybridFactorGraph.cpp

@@ -22,13 +22,15 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/DCFactor.h>
 #include <gtsam/hybrid/DCMixtureFactor.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/IncrementalHybrid.h>
 #include <gtsam/hybrid/NonlinearHybridFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 #include <numeric>
@@ -41,6 +43,7 @@
 using namespace std;
 using namespace gtsam;
 using noiseModel::Isotropic;
+using symbol_shorthand::L;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
 
@@ -643,6 +646,77 @@ factor 2:  GaussianMixture [x3 | m2 m1 ]{
   EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
 }
 
+/* ************************************************************************* */
+// Simple PlanarSLAM example test with 2 poses and 2 landmarks (each pose
+// connects to 1 landmark) to expose issue with default decision tree creation
+// in hybrid elimination. The hybrid factor is between the poses X0 and X1. The
+// issue arises if we eliminate a landmark variable first since it is not
+// connected to a DCFactor.
+TEST(HybridFactorGraph, DefaultDecisionTree) {
+  NonlinearHybridFactorGraph fg;
+
+  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and
+  // a noise model (covariance matrix)
+  Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
+  auto priorNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.3, 0.3, 0.1));  // 30cm std on x,y, 0.1 rad on theta
+  fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise);
+
+  using PlanarMotionModel = BetweenFactor<Pose2>;
+
+  // Add odometry factor
+  Pose2 odometry(2.0, 0.0, 0.0);
+  KeyVector contKeys = {X(0), X(1)};
+  auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
+  auto still = boost::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
+                                                     noise_model),
+       moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
+                                                      noise_model);
+  std::vector<PlanarMotionModel::shared_ptr> components = {still, moving};
+  auto dcFactor = boost::make_shared<DCMixtureFactor<PlanarMotionModel>>(
+      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
+  fg.push_back(dcFactor);
+
+  // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
+  // create a noise model for the landmark measurements
+  auto measurementNoise = noiseModel::Diagonal::Sigmas(
+      Vector2(0.1, 0.2));  // 0.1 rad std on bearing, 20cm on range
+  // create the measurement values - indices are (pose id, landmark id)
+  Rot2 bearing11 = Rot2::fromDegrees(45),
+       bearing22 = Rot2::fromDegrees(90);
+  double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
+
+  // Add Bearing-Range factors
+  fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
+      X(0), L(0), bearing11, range11, measurementNoise);
+  fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>(
+      X(1), L(1), bearing22, range22, measurementNoise);
+
+  // Create (deliberately inaccurate) initial estimate
+  Values initialEstimate;
+  initialEstimate.insert(X(0), Pose2(0.5, 0.0, 0.2));
+  initialEstimate.insert(X(1), Pose2(2.3, 0.1, -0.2));
+  initialEstimate.insert(L(0), Point2(1.8, 2.1));
+  initialEstimate.insert(L(1), Point2(4.1, 1.8));
+
+  // We want to eliminate variables not connected to DCFactors first.
+  Ordering ordering;
+  ordering += L(0);
+  ordering += L(1);
+  ordering += X(0);
+  ordering += X(1);
+
+  GaussianHybridFactorGraph linearized = fg.linearize(initialEstimate);
+  gtsam::HybridBayesNet::shared_ptr hybridBayesNet;
+  gtsam::GaussianHybridFactorGraph::shared_ptr remainingFactorGraph;
+
+  // This should NOT fail
+  std::tie(hybridBayesNet, remainingFactorGraph) =
+      linearized.eliminatePartialSequential(ordering);
+  EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
+  EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;