Merge branch 'develop' into fix/python_wrapper

gtsam/discrete/AlgebraicDecisionTree.h

@@ -196,6 +196,42 @@ namespace gtsam {
       return this->apply(g, &Ring::div);
     }
 
+    /// Compute sum of all values
+    double sum() const {
+      double sum = 0;
+      auto visitor = [&](double y) { sum += y; };
+      this->visit(visitor);
+      return sum;
+    }
+
+    /**
+     * @brief Helper method to perform normalization such that all leaves in the
+     * tree sum to 1
+     *
+     * @param sum
+     * @return AlgebraicDecisionTree
+     */
+    AlgebraicDecisionTree normalize(double sum) const {
+      return this->apply([&sum](const double& x) { return x / sum; });
+    }
+
+    /// Find the minimum values amongst all leaves
+    double min() const {
+      double min = std::numeric_limits<double>::max();
+      auto visitor = [&](double x) { min = x < min ? x : min; };
+      this->visit(visitor);
+      return min;
+    }
+
+    /// Find the maximum values amongst all leaves
+    double max() const {
+      // Get the most negative value
+      double max = -std::numeric_limits<double>::max();
+      auto visitor = [&](double x) { max = x > max ? x : max; };
+      this->visit(visitor);
+      return max;
+    }
+
     /** sum out variable */
     AlgebraicDecisionTree sum(const L& label, size_t cardinality) const {
       return this->combine(label, cardinality, &Ring::add);

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -593,6 +593,55 @@ TEST(ADT, zero) {
   EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9);
 }
 
+/// Example ADT from 0 to 11.
+ADT exampleADT() {
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  ADT f(A & B & C, "0 6  2 8  4 10    1 7  3 9  5 11");
+  return f;
+}
+/* ************************************************************************** */
+// Test sum
+TEST(ADT, Sum) {
+  ADT a = exampleADT();
+  double expected_sum = 0;
+  for (double i = 0; i < 12; i++) {
+    expected_sum += i;
+  }
+  EXPECT_DOUBLES_EQUAL(expected_sum, a.sum(), 1e-9);
+}
+
+/* ************************************************************************** */
+// Test normalize
+TEST(ADT, Normalize) {
+  ADT a = exampleADT();
+  double sum = a.sum();
+  auto actual = a.normalize(sum);
+
+  DiscreteKey A(0, 2), B(1, 3), C(2, 2);
+  DiscreteKeys keys = DiscreteKeys{A, B, C};
+  std::vector<double> cpt{0 / sum, 6 / sum,  2 / sum, 8 / sum,
+                          4 / sum, 10 / sum, 1 / sum, 7 / sum,
+                          3 / sum, 9 / sum,  5 / sum, 11 / sum};
+  ADT expected(keys, cpt);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************** */
+// Test min
+TEST(ADT, Min) {
+  ADT a = exampleADT();
+  double min = a.min();
+  EXPECT_DOUBLES_EQUAL(0.0, min, 1e-9);
+}
+
+/* ************************************************************************** */
+// Test max
+TEST(ADT, Max) {
+  ADT a = exampleADT();
+  double max = a.max();
+  EXPECT_DOUBLES_EQUAL(11.0, max, 1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/Rot2.h

@@ -68,7 +68,7 @@ namespace gtsam {
       return fromAngle(theta * degree);
     }
 
-    /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
+    /// Named constructor from cos(theta),sin(theta) pair
     static Rot2 fromCosSin(double c, double s);
 
     /**

gtsam/geometry/geometry.i

@@ -856,7 +856,7 @@ class Cal3_S2Stereo {
   gtsam::Matrix K() const;
   gtsam::Point2 principalPoint() const;
   double baseline() const;
-  Vector6 vector() const;
+  gtsam::Vector6 vector() const;
   gtsam::Matrix inverse() const;
 };
 

gtsam/hybrid/GaussianMixture.h

@@ -67,7 +67,8 @@ class GTSAM_EXPORT GaussianMixture
   double logConstant_;         ///< log of the normalization constant.
 
   /**
-   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   * @brief Convert a DecisionTree of factors into
+   * a DecisionTree of Gaussian factor graphs.
    */
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;
 
@@ -214,7 +215,8 @@ class GTSAM_EXPORT GaussianMixture
    * @return AlgebraicDecisionTree<Key> A decision tree on the discrete keys
    * only, with the leaf values as the error for each assignment.
    */
-  AlgebraicDecisionTree<Key> errorTree(const VectorValues &continuousValues) const;
+  AlgebraicDecisionTree<Key> errorTree(
+      const VectorValues &continuousValues) const;
 
   /**
    * @brief Compute the logProbability of this Gaussian Mixture.

gtsam/hybrid/GaussianMixtureFactor.h

@@ -135,7 +135,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @return AlgebraicDecisionTree<Key> A decision tree with the same keys
    * as the factors involved, and leaf values as the error.
    */
-  AlgebraicDecisionTree<Key> errorTree(const VectorValues &continuousValues) const;
+  AlgebraicDecisionTree<Key> errorTree(
+      const VectorValues &continuousValues) const;
 
   /**
    * @brief Compute the log-likelihood, including the log-normalizing constant.

gtsam/hybrid/HybridValues.h

@@ -12,6 +12,7 @@
 /**
  *  @file HybridValues.h
  *  @date Jul 28, 2022
+ *  @author Varun Agrawal
  *  @author Shangjie Xue
  */
 
@@ -54,13 +55,13 @@ class GTSAM_EXPORT HybridValues {
   HybridValues() = default;
 
   /// Construct from DiscreteValues and VectorValues.
-  HybridValues(const VectorValues &cv, const DiscreteValues &dv)
-      : continuous_(cv), discrete_(dv){}
+  HybridValues(const VectorValues& cv, const DiscreteValues& dv)
+      : continuous_(cv), discrete_(dv) {}
 
   /// Construct from all values types.
   HybridValues(const VectorValues& cv, const DiscreteValues& dv,
                const Values& v)
-      : continuous_(cv), discrete_(dv), nonlinear_(v){}
+      : continuous_(cv), discrete_(dv), nonlinear_(v) {}
 
   /// @}
   /// @name Testable
@@ -101,9 +102,7 @@ class GTSAM_EXPORT HybridValues {
   bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }
 
   /// Check whether a variable with key \c j exists in values.
-  bool existsNonlinear(Key j) {
-    return nonlinear_.exists(j);
-  }
+  bool existsNonlinear(Key j) { return nonlinear_.exists(j); }
 
   /// Check whether a variable with key \c j exists.
   bool exists(Key j) {
@@ -128,9 +127,7 @@ class GTSAM_EXPORT HybridValues {
   }
 
   /// insert_or_assign() , similar to Values.h
-  void insert_or_assign(Key j, size_t value) {
-    discrete_[j] = value;
-  }
+  void insert_or_assign(Key j, size_t value) { discrete_[j] = value; }
 
   /** Insert all continuous values from \c values.  Throws an invalid_argument
    * exception if any keys to be inserted are already used. */

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -333,7 +333,6 @@ TEST(HybridEstimation, Probability) {
   for (auto discrete_conditional : *discreteBayesNet) {
     bayesNet->add(discrete_conditional);
   }
-  auto discreteConditional = discreteBayesNet->at(0)->asDiscrete();
 
   HybridValues hybrid_values = bayesNet->optimize();
 

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -680,12 +680,14 @@ conditional 0: Hybrid  P( x0 | x1 m0)
   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)
@@ -696,25 +698,29 @@ conditional 1: Hybrid  P( x1 | x2 m0 m1)
   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) 
  1 0 Leaf p(x1 | x2)
   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)
@@ -726,13 +732,15 @@ 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)
   R = [ 10.0494 ]
   d = [ -10.1479 ]
   mean: 1 elements
   x2: -1.0098
+  logNormalizationConstant: 1.38857
   No noise model
 
  1 Choice(m0) 
@@ -741,13 +749,15 @@ 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)
   R = [ 10.0494 ]
   d = [ -10.0494 ]
   mean: 1 elements
   x2: -1
+  logNormalizationConstant: 1.38857
   No noise model
 
 )";

gtsam/linear/GaussianBayesNet.cpp

@@ -243,5 +243,25 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+  double GaussianBayesNet::logNormalizationConstant() const {
+    /*
+    normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
+    logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
+    
+    log det(Sigma)) = -2.0 * logDeterminant()
+    thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
+
+    BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
+    = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
+    = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
+    */
+    double logNormConst = 0.0;
+    for (const sharedConditional& cg : *this) {
+      logNormConst += cg->logNormalizationConstant();
+    }
+    return logNormConst;
+  }
+
+  /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/linear/GaussianBayesNet.h

@@ -82,6 +82,12 @@ namespace gtsam {
     /** Check equality */
     bool equals(const This& bn, double tol = 1e-9) const;
 
+    /// Check exact equality.
+    friend bool operator==(const GaussianBayesNet& lhs,
+                           const GaussianBayesNet& rhs) {
+      return lhs.isEqual(rhs);
+    }
+
     /// print graph
     void print(
         const std::string& s = "",
@@ -228,6 +234,14 @@ namespace gtsam {
      * @return The determinant */
     double logDeterminant() const;
 
+    /**
+     * @brief Get the log of the normalization constant corresponding to the
+     * joint Gaussian density represented by this Bayes net.
+     *
+     * @return double
+     */
+    double logNormalizationConstant() const;
+
     /**
      * Backsubstitute with a different RHS vector than the one stored in this BayesNet.
      * gy=inv(R*inv(Sigma))*gx

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
@@ -184,8 +185,13 @@ namespace gtsam {
   double GaussianConditional::logNormalizationConstant() const {
     constexpr double log2pi = 1.8378770664093454835606594728112;
     size_t n = d().size();
-    // log det(Sigma)) = - 2.0 * logDeterminant()
-    return - 0.5 * n * log2pi + logDeterminant();
+    // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
+    // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
+    // Hence, log det(Sigma)) = -2.0 * logDeterminant()
+    // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
+    //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
+    //     = -0.5*n*log(2*pi) + logDeterminant()
+    return -0.5 * n * log2pi + logDeterminant();
   }
 
   /* ************************************************************************* */

gtsam/linear/VectorValues.h

@@ -263,6 +263,11 @@ namespace gtsam {
     /** equals required by Testable for unit testing */
     bool equals(const VectorValues& x, double tol = 1e-9) const;
 
+    /// Check equality.
+    friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) {
+      return lhs.equals(rhs);
+    }
+
     /// @{
     /// @name Advanced Interface
     /// @{

gtsam/linear/linear.i

@@ -375,7 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   void serialize() const;
 };
 
-pair<gtsam::GaussianConditional, gtsam::JacobianFactor*> EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
+pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> EliminateQR(
+    const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys);
 
 #include <gtsam/linear/HessianFactor.h>
 virtual class HessianFactor : gtsam::GaussianFactor {
@@ -510,12 +511,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T,
                       const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianConditional(const vector<std::pair<gtsam::Key, gtsam::Matrix>> terms,
+                      size_t nrFrontals, gtsam::Vector d,
+                      const gtsam::noiseModel::Diagonal* sigmas);
 
   // Constructors with no noise model
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S);
   GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
                       size_t name2, gtsam::Matrix T);
+  GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals,
+                      const gtsam::VerticalBlockMatrix& augmentedMatrix);
 
   // Named constructors
   static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,