Merge pull request #1705 from borglab/hybrid-lognormconstant

gtsam/hybrid/GaussianMixture.cpp

@@ -174,6 +174,8 @@ void GaussianMixture::print(const std::string &s,
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
   std::cout << "\n";
+  std::cout << " logNormalizationConstant: " << logConstant_ << "\n"
+            << std::endl;
   conditionals_.print(
       "", [&](Key k) { return formatter(k); },
       [&](const GaussianConditional::shared_ptr &gf) -> std::string {

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -677,33 +677,41 @@ factor 6:  P( m1 | m0 ):
 size: 3
 conditional 0: Hybrid  P( x0 | x1 m0)
  Discrete Keys = (m0, 2), 
+ logNormalizationConstant: 1.38862
+
  Choice(m0) 
  0 Leaf p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.85087 ]
+  logNormalizationConstant: 1.38862
   No noise model
 
  1 Leaf p(x0 | x1)
   R = [ 10.0499 ]
   S[x1] = [ -0.0995037 ]
   d = [ -9.95037 ]
+  logNormalizationConstant: 1.38862
   No noise model
 
 conditional 1: Hybrid  P( x1 | x2 m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
+ logNormalizationConstant: 1.3935
+
  Choice(m1) 
  0 Choice(m0) 
  0 0 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.99901 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  0 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -9.90098 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  1 Choice(m0) 
@@ -711,16 +719,20 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10.098 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
  1 1 Leaf p(x1 | x2)
   R = [ 10.099 ]
   S[x2] = [ -0.0990196 ]
   d = [ -10 ]
+  logNormalizationConstant: 1.3935
   No noise model
 
 conditional 2: Hybrid  P( x2 | m0 m1)
  Discrete Keys = (m0, 2), (m1, 2), 
+ logNormalizationConstant: 1.38857
+
  Choice(m1) 
  0 Choice(m0) 
  0 0 Leaf p(x2)
@@ -728,6 +740,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1489 ]
   mean: 1 elements
   x2: -1.0099
+  logNormalizationConstant: 1.38857
   No noise model
 
  0 1 Leaf p(x2)
@@ -735,6 +748,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.1479 ]
   mean: 1 elements
   x2: -1.0098
+  logNormalizationConstant: 1.38857
   No noise model
 
  1 Choice(m0) 
@@ -743,6 +757,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0504 ]
   mean: 1 elements
   x2: -1.0001
+  logNormalizationConstant: 1.38857
   No noise model
 
  1 1 Leaf p(x2)
@@ -750,6 +765,7 @@ conditional 2: Hybrid  P( x2 | m0 m1)
   d = [ -10.0494 ]
   mean: 1 elements
   x2: -1
+  logNormalizationConstant: 1.38857
   No noise model
 
 )";

gtsam/linear/GaussianConditional.cpp

@@ -121,6 +121,7 @@ namespace gtsam {
       const auto mean = solve({});  // solve for mean.
       mean.print("  mean", formatter);
     }
+    cout << "  logNormalizationConstant: " << logNormalizationConstant() << std::endl;
     if (model_)
       model_->print("  Noise model: ");
     else

gtsam/linear/linear.i

@@ -489,12 +489,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
                       size_t name2, Matrix T,
                       const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianConditional(const vector<std::pair<gtsam::Key, Matrix>> terms,
+                      size_t nrFrontals, Vector d,
+                      const gtsam::noiseModel::Diagonal* sigmas);
 
   // Constructors with no noise model
   GaussianConditional(size_t key, Vector d, Matrix R);
   GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
   GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
                       size_t name2, Matrix T);
+  GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals,
+                      const gtsam::VerticalBlockMatrix& augmentedMatrix);
 
   // Named constructors
   static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 

gtsam/linear/tests/testGaussianConditional.cpp

@@ -516,6 +516,7 @@ TEST(GaussianConditional, Print) {
     "  d = [ 20 40 ]\n"
     "  mean: 1 elements\n"
     "  x0: 20 40\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
 
@@ -530,6 +531,7 @@ TEST(GaussianConditional, Print) {
     "  S[x1] = [ -1 -2 ]\n"
     "          [ -3 -4 ]\n"
     "  d = [ 20 40 ]\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
 
@@ -545,6 +547,7 @@ TEST(GaussianConditional, Print) {
     "  S[y1] = [ -5 -6 ]\n"
     "          [ -7 -8 ]\n"
     "  d = [ 20 40 ]\n"
+    "  logNormalizationConstant: -4.0351\n"
     "isotropic dim=2 sigma=3\n";
   EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
 }