Merge branch 'develop' into release/4.2a5

Author: dellaert

gtsam/base/FastSet.h

@@ -18,6 +18,9 @@
 
 #pragma once
 
+#if BOOST_VERSION >= 107400
+#include <boost/serialization/library_version_type.hpp>
+#endif
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
 #include <gtsam/base/FastDefaultAllocator.h>

gtsam/base/serialization.h

@@ -25,6 +25,7 @@
 #include <string>
 
 // includes for standard serialization types
+#include <boost/serialization/version.hpp>
 #include <boost/serialization/optional.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/vector.hpp>

gtsam/discrete/DiscreteConditional.cpp

@@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
 
 /* ****************************************************************************/
 DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
-    size_t parent_value) const {
+    size_t frontal) const {
   if (nrFrontals() != 1)
     throw std::invalid_argument(
         "Single value likelihood can only be invoked on single-variable "
         "conditional");
   DiscreteValues values;
-  values.emplace(keys_[0], parent_value);
+  values.emplace(keys_[0], frontal);
   return likelihood(values);
 }
 

gtsam/discrete/DiscreteConditional.h

@@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
       const DiscreteValues& frontalValues) const;
 
   /** Single variable version of likelihood. */
-  DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
+  DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
 
   /**
    * sample

gtsam/discrete/tests/testDiscreteBayesNet.cpp

@@ -150,7 +150,6 @@ TEST(DiscreteBayesNet, Dot) {
   fragment.add((Either | Tuberculosis, LungCancer) = "F T T T");
 
   string actual = fragment.dot();
-  cout << actual << endl;
   EXPECT(actual ==
          "digraph {\n"
          "  size=\"5,5\";\n"

gtsam/linear/GaussianBayesNet.cpp

@@ -26,6 +26,9 @@
 using namespace std;
 using namespace gtsam;
 
+// In Wrappers we have no access to this so have a default ready
+static std::mt19937_64 kRandomNumberGenerator(42);
+
 namespace gtsam {
 
   // Instantiate base class
@@ -37,28 +40,50 @@ namespace gtsam {
     return Base::equals(bn, tol);
   }
 
-  /* ************************************************************************* */
-  VectorValues GaussianBayesNet::optimize() const
-  {
-    VectorValues soln; // no missing variables -> just create an empty vector
-    return optimize(soln);
+  /* ************************************************************************ */
+  VectorValues GaussianBayesNet::optimize() const {
+    VectorValues solution;  // no missing variables -> create an empty vector
+    return optimize(solution);
   }
 
-  /* ************************************************************************* */
-  VectorValues GaussianBayesNet::optimize(
-      const VectorValues& solutionForMissing) const {
-    VectorValues soln(solutionForMissing); // possibly empty
+  VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
     // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
-    /** solve each node in turn in topological sort order (parents first)*/
-    for (auto cg: boost::adaptors::reverse(*this)) {
+    // solve each node in reverse topological sort order (parents first)
+    for (auto cg : boost::adaptors::reverse(*this)) {
       // i^th part of R*x=y, x=inv(R)*y
-      // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
-      soln.insert(cg->solve(soln));
+      // (Rii*xi + R_i*x(i+1:))./si = yi =>
+      // xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
+      solution.insert(cg->solve(solution));
     }
-    return soln;
+    return solution;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const {
+    VectorValues result;  // no missing variables -> create an empty vector
+    return sample(result, rng);
+  }
+
+  VectorValues GaussianBayesNet::sample(VectorValues result,
+                                        std::mt19937_64* rng) const {
+    // sample each node in reverse topological sort order (parents first)
+    for (auto cg : boost::adaptors::reverse(*this)) {
+      const VectorValues sampled = cg->sample(result, rng);
+      result.insert(sampled);
+    }
+    return result;
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianBayesNet::sample() const {
+    return sample(&kRandomNumberGenerator);
+  }
+
+  VectorValues GaussianBayesNet::sample(VectorValues given) const {
+    return sample(given, &kRandomNumberGenerator);
+  }
+
+  /* ************************************************************************ */
   VectorValues GaussianBayesNet::optimizeGradientSearch() const
   {
     gttic(GaussianBayesTree_optimizeGradientSearch);

gtsam/linear/GaussianBayesNet.h

@@ -88,11 +88,35 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution
+    /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
+    /// back-substitution
     VectorValues optimize() const;
 
-    /// Version of optimize for incomplete BayesNet, needs solution for missing variables
-    VectorValues optimize(const VectorValues& solutionForMissing) const;
+    /// Version of optimize for incomplete BayesNet, given missing variables
+    VectorValues optimize(const VectorValues given) const;
+
+    /**
+     * Sample using ancestral sampling
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   auto sample = gbn.sample(&rng);
+     */
+    VectorValues sample(std::mt19937_64* rng) const;
+
+    /**
+     * Sample from an incomplete BayesNet, given missing variables
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   VectorValues given = ...;
+     *   auto sample = gbn.sample(given, &rng);
+     */
+    VectorValues sample(VectorValues given, std::mt19937_64* rng) const;
+
+    /// Sample using ancestral sampling, use default rng
+    VectorValues sample() const;
+
+    /// Sample from an incomplete BayesNet, use default rng
+    VectorValues sample(VectorValues given) const;
 
     /**
      * Return ordering corresponding to a topological sort.

gtsam/linear/GaussianConditional.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/Sampler.h>
 
 #include <boost/format.hpp>
 #ifdef __GNUC__
@@ -34,6 +35,9 @@
 #include <list>
 #include <string>
 
+// In Wrappers we have no access to this so have a default ready
+static std::mt19937_64 kRandomNumberGenerator(42);
+
 using namespace std;
 
 namespace gtsam {
@@ -43,19 +47,47 @@ namespace gtsam {
     Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
   BaseFactor(key, R, d, sigmas), BaseConditional(1) {}
 
-  /* ************************************************************************* */
-  GaussianConditional::GaussianConditional(
-    Key key, const Vector& d, const Matrix& R,
-    Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
-  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
+  /* ************************************************************************ */
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
+                                           const Matrix& R, Key parent1,
+                                           const Matrix& S,
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
+
+  /* ************************************************************************ */
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
+                                           const Matrix& R, Key parent1,
+                                           const Matrix& S, Key parent2,
+                                           const Matrix& T,
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
+        BaseConditional(1) {}
+
+  /* ************************************************************************ */
+  GaussianConditional GaussianConditional::FromMeanAndStddev(
+      Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
+    // |Rx + Sy - d| = |x-(Ay + b)|/sigma
+    const Matrix R = Matrix::Identity(b.size(), b.size());
+    const Matrix S = -A;
+    const Vector d = b;
+    return GaussianConditional(key, d, R, parent, S,
+                               noiseModel::Isotropic::Sigma(b.size(), sigma));
+  }
 
-  /* ************************************************************************* */
-  GaussianConditional::GaussianConditional(
-    Key key, const Vector& d, const Matrix& R,
-    Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
-  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
+  /* ************************************************************************ */
+  GaussianConditional GaussianConditional::FromMeanAndStddev(
+      Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
+      const Vector& b, double sigma) {
+    // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
+    const Matrix R = Matrix::Identity(b.size(), b.size());
+    const Matrix S = -A1;
+    const Matrix T = -A2;
+    const Vector d = b;
+    return GaussianConditional(key, d, R, parent1, S, parent2, T,
+                               noiseModel::Isotropic::Sigma(b.size(), sigma));
+  }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
     cout << s << "  Conditional density ";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
@@ -192,7 +224,88 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const VectorValues& frontalValues) const {
+    // Error is |Rx - (d - Sy - Tz - ...)|^2
+    // so when we instantiate x (which has to be completely known) we beget:
+    // |Sy + Tz + ... - (d - Rx)|^2
+    // The noise model just transfers over!
+
+    // Get frontalValues as vector
+    const Vector x =
+        frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
+
+    // Copy the augmented Jacobian matrix:
+    auto newAb = Ab_;
+
+    // Restrict view to parent blocks
+    newAb.firstBlock() += nrFrontals_;
+
+    // Update right-hand-side (last column)
+    auto last = newAb.matrix().cols() - 1;
+    const auto RR = R().triangularView<Eigen::Upper>();
+    newAb.matrix().col(last) -= RR * x;
+
+    // The keys now do not include the frontal keys:
+    KeyVector newKeys;
+    newKeys.reserve(nrParents());
+    for (auto&& key : parents()) newKeys.push_back(key);
+
+    // Hopefully second newAb copy below is optimized out...
+    return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
+  }
+
+  /* **************************************************************************/
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const Vector& frontal) const {
+    if (nrFrontals() != 1)
+      throw std::invalid_argument(
+          "GaussianConditional Single value likelihood can only be invoked on "
+          "single-variable conditional");
+    VectorValues values;
+    values.insert(keys_[0], frontal);
+    return likelihood(values);
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
+                                           std::mt19937_64* rng) const {
+    if (nrFrontals() != 1) {
+      throw std::invalid_argument(
+          "GaussianConditional::sample can only be called on single variable "
+          "conditionals");
+    }
+    if (!model_) {
+      throw std::invalid_argument(
+          "GaussianConditional::sample can only be called if a diagonal noise "
+          "model was specified at construction.");
+    }
+    VectorValues solution = solve(parentsValues);
+    Key key = firstFrontalKey();
+    const Vector& sigmas = model_->sigmas();
+    solution[key] += Sampler::sampleDiagonal(sigmas, rng);
+    return solution;
+  }
+
+  VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
+    if (nrParents() != 0)
+      throw std::invalid_argument(
+          "sample() can only be invoked on no-parent prior");
+    VectorValues values;
+    return sample(values);
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianConditional::sample() const {
+    return sample(&kRandomNumberGenerator);
+  }
+
+  VectorValues GaussianConditional::sample(const VectorValues& given) const {
+    return sample(given, &kRandomNumberGenerator);
+  }
+
+  /* ************************************************************************ */
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   void GTSAM_DEPRECATED
   GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {