Merge branch 'develop' into release/4.2a5

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 GaussianBayesNet::optimize() const {
-  {
+    VectorValues solution;  // no missing variables -> create an empty vector
-    VectorValues soln; // no missing variables -> just create an empty vector
+    return optimize(solution);
-    return optimize(soln);
   }
 
-  /* ************************************************************************* */
+  VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
-  VectorValues GaussianBayesNet::optimize(
-      const VectorValues& solutionForMissing) const {
-    VectorValues soln(solutionForMissing); // possibly empty
     // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
-    /** solve each node in turn in topological sort order (parents first)*/
+    // solve each node in reverse topological sort order (parents first)
-    for (auto cg: boost::adaptors::reverse(*this)) {
+    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:))
+      // (Rii*xi + R_i*x(i+1:))./si = yi =>
-      soln.insert(cg->solve(soln));
+      // 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
+    /// Version of optimize for incomplete BayesNet, given missing variables
-    VectorValues optimize(const VectorValues& solutionForMissing) const;
+    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(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
+                                           const Matrix& S,
-  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  GaussianConditional::GaussianConditional(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
+                                           const Matrix& S, Key parent2,
-  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
+                                           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::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 {

gtsam/linear/GaussianConditional.h

@@ -26,12 +26,15 @@
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <random> // for std::mt19937_64 
+
 namespace gtsam {
 
   /**
-  * A conditional Gaussian functions as the node in a Bayes network
+  * A GaussianConditional functions as the node in a Bayes network.
   * It has a set of parents y,z, etc. and implements a probability density on x.
   * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
+  * @addtogroup linear
   */
   class GTSAM_EXPORT GaussianConditional :
     public JacobianFactor,
@@ -43,6 +46,9 @@ namespace gtsam {
     typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class
     typedef Conditional<BaseFactor, This> BaseConditional; ///< Typedef to our conditional base class
 
+    /// @name Constructors
+    /// @{
+
     /** default constructor needed for serialization */
     GaussianConditional() {}
 
@@ -51,13 +57,14 @@ namespace gtsam {
       const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with only one parent |Rx+Sy-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
+                        const Matrix& S,
+                        const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with two parents |Rx+Sy+Tz-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, Key name2, const Matrix& T,
+                        const Matrix& S, Key parent2, const Matrix& T,
-      const SharedDiagonal& sigmas = SharedDiagonal());
+                        const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with arbitrary number of frontals and parents.
     *   @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
@@ -76,6 +83,17 @@ namespace gtsam {
       const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
       const SharedDiagonal& sigmas = SharedDiagonal());
 
+    /// Construct from mean A1 p1 + b and standard deviation.
+    static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
+                                                 Key parent, const Vector& b,
+                                                 double sigma);
+
+    /// Construct from mean A1 p1 + A2 p2 + b and standard deviation.
+    static GaussianConditional FromMeanAndStddev(Key key,  //
+                                                 const Matrix& A1, Key parent1,
+                                                 const Matrix& A2, Key parent2,
+                                                 const Vector& b, double sigma);
+
     /** Combine several GaussianConditional into a single dense GC.  The conditionals enumerated by
     *   \c first and \c last must be in increasing order, meaning that the parents of any
     *   conditional may not include a conditional coming before it.
@@ -86,6 +104,10 @@ namespace gtsam {
     template<typename ITERATOR>
     static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
 
+    /// @}
+    /// @name Testable
+    /// @{
+
     /** print */
     void print(const std::string& = "GaussianConditional",
       const KeyFormatter& formatter = DefaultKeyFormatter) const override;
@@ -93,6 +115,10 @@ namespace gtsam {
     /** equals function */
     bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
 
+    /// @}
+    /// @name Standard Interface
+    /// @{
+
     /** Return a view of the upper-triangular R block of the conditional */
     constABlock R() const { return Ab_.range(0, nrFrontals()); }
 
@@ -125,10 +151,46 @@ namespace gtsam {
     /** Performs transpose backsubstition in place on values */
     void solveTransposeInPlace(VectorValues& gy) const;
 
+    /** Convert to a likelihood factor by providing value before bar. */
+    JacobianFactor::shared_ptr likelihood(
+        const VectorValues& frontalValues) const;
+
+    /** Single variable version of likelihood. */
+    JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
+
+    /**
+     * Sample from conditional, zero parent version
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   auto sample = gbn.sample(&rng);
+     */
+    VectorValues sample(std::mt19937_64* rng) const;
+
+    /**
+     * Sample from conditional, given missing variables
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   VectorValues given = ...;
+     *   auto sample = gbn.sample(given, &rng);
+     */
+    VectorValues sample(const VectorValues& parentsValues,
+                        std::mt19937_64* rng) const;
+
+    /// Sample, use default rng
+    VectorValues sample() const;
+
+    /// Sample with given values, use default rng
+    VectorValues sample(const VectorValues& parentsValues) const;
+
+    /// @}
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+    /// @name Deprecated
+    /// @{
     /** Scale the values in \c gy according to the sigmas for the frontal variables in this
      *  conditional. */
     void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
+    /// @}
 #endif
 
    private:

gtsam/linear/GaussianDensity.cpp

@@ -23,9 +23,12 @@ using namespace std;
 namespace gtsam {
 
   /* ************************************************************************* */
-  GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma)
+  GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
-  {
+                                                     const Vector& mean,
-    return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma);
+                                                     double sigma) {
+    return GaussianDensity(key, mean,
+                           Matrix::Identity(mean.size(), mean.size()),
+                           noiseModel::Isotropic::Sigma(mean.size(), sigma));
   }
 
   /* ************************************************************************* */
@@ -35,8 +38,8 @@ namespace gtsam {
     for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
       cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
     cout << endl;
-    gtsam::print(Matrix(R()), "R: ");
+    gtsam::print(mean(), "mean: ");
-    gtsam::print(Vector(d()), "d: ");
+    gtsam::print(covariance(), "covariance: ");
     if(model_)
       model_->print("Noise model: ");
   }

gtsam/linear/GaussianDensity.h

@@ -24,11 +24,10 @@
 namespace gtsam {
 
   /**
-  * A Gaussian density.
+  * A GaussianDensity is a GaussianConditional without parents.
-  *
-  * It is implemented as a GaussianConditional without parents.
   * The negative log-probability is given by \f$ |Rx - d|^2 \f$
   * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$
+  * @addtogroup linear
   */
   class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
 
@@ -52,8 +51,9 @@ namespace gtsam {
     GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
       GaussianConditional(key, d, R, noiseModel) {}
 
-    /// Construct using a mean and variance
+    /// Construct using a mean and standard deviation
-    static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma);
+    static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
+                                             double sigma);
 
     /// print
     void print(const std::string& = "GaussianDensity",

gtsam/linear/Sampler.cpp

@@ -22,14 +22,18 @@ namespace gtsam {
 /* ************************************************************************* */
 Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
                  uint_fast64_t seed)
-    : model_(model), generator_(seed) {}
+    : model_(model), generator_(seed) {
+  if (!model) {
+    throw std::invalid_argument("Sampler::Sampler needs a non-null model.");
+  }
+}
 
 /* ************************************************************************* */
 Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
     : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
 
 /* ************************************************************************* */
-Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
+Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) {
   size_t d = sigmas.size();
   Vector result(d);
   for (size_t i = 0; i < d; i++) {
@@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
     if (sigma == 0.0) {
       result(i) = 0.0;
     } else {
-      typedef std::normal_distribution<double> Normal;
+      std::normal_distribution<double> dist(0.0, sigma);
-      Normal dist(0.0, sigma);
+      result(i) = dist(*rng);
-      result(i) = dist(generator_);
     }
   }
   return result;
 }
 
+/* ************************************************************************* */
+Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
+  return sampleDiagonal(sigmas, &generator_);
+}
+
 /* ************************************************************************* */
 Vector Sampler::sample() const {
   assert(model_.get());

gtsam/linear/Sampler.h

@@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler {
   /// @name access functions
   /// @{
 
-  size_t dim() const {
+  size_t dim() const { return model_->dim(); }
-    assert(model_.get());
-    return model_->dim();
-  }
 
-  Vector sigmas() const {
+  Vector sigmas() const { return model_->sigmas(); }
-    assert(model_.get());
-    return model_->sigmas();
-  }
 
   const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
 
@@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler {
   /// sample from distribution
   Vector sample() const;
 
+  /// sample with given random number generator
+  static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
   /// @}
 
  protected:

gtsam/linear/VectorValues.cpp

@@ -33,7 +33,7 @@ namespace gtsam {
   using boost::adaptors::map_values;
   using boost::accumulate;
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
   {
     // Merge using predicate for comparing first of pair
@@ -44,7 +44,7 @@ namespace gtsam {
       throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
     using Pair = pair<const Key, size_t>;
     size_t j = 0;
@@ -61,7 +61,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
     size_t j = 0;
     for (const SlotEntry& v : scatter) {
@@ -74,7 +74,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
     VectorValues result;
@@ -87,7 +87,7 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
     std::pair<iterator, bool> result = values_.insert(key_value);
     if(!result.second)
@@ -97,7 +97,7 @@ namespace gtsam {
     return result.first;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::update(const VectorValues& values)
   {
     iterator hint = begin();
@@ -115,7 +115,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::insert(const VectorValues& values)
   {
     size_t originalSize = size();
@@ -124,14 +124,14 @@ namespace gtsam {
       throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::setZero()
   {
     for(Vector& v: values_ | map_values)
       v.setZero();
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
@@ -150,7 +150,7 @@ namespace gtsam {
     return os;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::print(const string& str,
                            const KeyFormatter& formatter) const {
     cout << str << ": " << size() << " elements\n";
@@ -158,7 +158,7 @@ namespace gtsam {
     cout.flush();
 }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   bool VectorValues::equals(const VectorValues& x, double tol) const {
     if(this->size() != x.size())
       return false;
@@ -170,7 +170,7 @@ namespace gtsam {
     return true;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   Vector VectorValues::vector() const {
     // Count dimensions
     DenseIndex totalDim = 0;
@@ -187,7 +187,7 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   Vector VectorValues::vector(const Dims& keys) const
   {
     // Count dimensions
@@ -203,12 +203,12 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::swap(VectorValues& other) {
     this->values_.swap(other.values_);
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   namespace internal
   {
     bool structureCompareOp(const boost::tuple<VectorValues::value_type,
@@ -219,14 +219,14 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   bool VectorValues::hasSameStructure(const VectorValues other) const
   {
     return accumulate(combine(*this, other)
       | transformed(internal::structureCompareOp), true, logical_and<bool>());
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::dot(const VectorValues& v) const
   {
     if(this->size() != v.size())
@@ -244,12 +244,12 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::norm() const {
     return std::sqrt(this->squaredNorm());
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::squaredNorm() const {
     double sumSquares = 0.0;
     using boost::adaptors::map_values;
@@ -258,7 +258,7 @@ namespace gtsam {
     return sumSquares;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::operator+(const VectorValues& c) const
   {
     if(this->size() != c.size())
@@ -278,13 +278,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::add(const VectorValues& c) const
   {
     return *this + c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::operator+=(const VectorValues& c)
   {
     if(this->size() != c.size())
@@ -301,13 +301,13 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::addInPlace(const VectorValues& c)
   {
     return *this += c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::addInPlace_(const VectorValues& c)
   {
     for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
@@ -320,7 +320,7 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::operator-(const VectorValues& c) const
   {
     if(this->size() != c.size())
@@ -340,13 +340,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::subtract(const VectorValues& c) const
   {
     return *this - c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues operator*(const double a, const VectorValues &v)
   {
     VectorValues result;
@@ -359,13 +359,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::scale(const double a) const
   {
     return a * *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::operator*=(double alpha)
   {
     for(Vector& v: *this | map_values)
@@ -373,12 +373,43 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::scaleInPlace(double alpha)
   {
     return *this *= alpha;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  string VectorValues::html(const KeyFormatter& keyFormatter) const {
+    stringstream ss;
+
+    // Print out preamble.
+    ss << "<div>\n<table class='VectorValues'>\n  <thead>\n";
+
+    // Print out header row.
+    ss << "    <tr><th>Variable</th><th>value</th></tr>\n";
+
+    // Finish header and start body.
+    ss << "  </thead>\n  <tbody>\n";
+
+    // Print out all rows.
+#ifdef GTSAM_USE_TBB
+    // TBB uses un-ordered map, so inefficiently order them:
+    std::map<Key, Vector> ordered;
+    for (const auto& kv : *this) ordered.emplace(kv);
+    for (const auto& kv : ordered) {
+#else
+    for (const auto& kv : *this) {
+#endif
+      ss << "    <tr>";
+      ss << "<th>" << keyFormatter(kv.first) << "</th><td>"
+         << kv.second.transpose() << "</td>";
+      ss << "</tr>\n";
+    }
+    ss << "  </tbody>\n</table>\n</div>";
+    return ss.str();
+  }
+
+  /* ************************************************************************ */
 
 } // \namespace gtsam

gtsam/linear/VectorValues.h

@@ -34,7 +34,7 @@
 namespace gtsam {
 
   /**
-   * This class represents a collection of vector-valued variables associated
+   * VectorValues represents a collection of vector-valued variables associated
    * each with a unique integer index.  It is typically used to store the variables
    * of a GaussianFactorGraph.  Optimizing a GaussianFactorGraph or GaussianBayesNet
    * returns this class.
@@ -69,7 +69,7 @@ namespace gtsam {
    * which is a view on the underlying data structure.
    *
    * This class is additionally used in gradient descent and dog leg to store the gradient.
-   * \nosubgrouping
+   * @addtogroup linear
    */
   class GTSAM_EXPORT VectorValues {
    protected:
@@ -344,11 +344,16 @@ namespace gtsam {
 
     /// @}
 
-    /// @}
+    /// @name Wrapper support
-    /// @name Matlab syntactic sugar for linear algebra operations
     /// @{
 
-    //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }
+    /**
+     * @brief Output as a html table.
+     *
+     * @param keyFormatter function that formats keys.
+     */
+    std::string html(
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /// @}
 

gtsam/linear/linear.i

@@ -255,6 +255,7 @@ class VectorValues {
 
   // enabling serialization functionality
   void serialize() const;
+  string html() const;
 };
 
 #include <gtsam/linear/GaussianFactor.h>
@@ -468,15 +469,36 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
                       size_t name2, Matrix T);
 
-  // Standard Interface
+  // Named constructors
+  static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
+                                                      const Matrix& A,
+                                                      gtsam::Key parent,
+                                                      const Vector& b,
+                                                      double sigma);
+
+  static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
+                                                      const Matrix& A1,
+                                                      gtsam::Key parent1, 
+                                                      const Matrix& A2,
+                                                      gtsam::Key parent2, 
+                                                      const Vector& b,
+                                                      double sigma);
+  // Testable
   void print(string s = "GaussianConditional",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GaussianConditional& cg, double tol) const;
+  
+  // Standard Interface
   gtsam::Key firstFrontalKey() const;
+  gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
+  gtsam::JacobianFactor* likelihood(
+      const gtsam::VectorValues& frontalValues) const;
+  gtsam::JacobianFactor* likelihood(Vector frontal) const;
+  gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
+  gtsam::VectorValues sample() const;
   
   // Advanced Interface
-  gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
   gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
                                     const gtsam::VectorValues& rhs) const;
   void solveTransposeInPlace(gtsam::VectorValues& gy) const;
@@ -490,14 +512,21 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
 
 #include <gtsam/linear/GaussianDensity.h>
 virtual class GaussianDensity : gtsam::GaussianConditional {
-    //Constructors
+  // Constructors
-  GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianDensity(gtsam::Key key, Vector d, Matrix R,
+                  const gtsam::noiseModel::Diagonal* sigmas);
 
-  //Standard Interface
+  static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key,
+                                                  const Vector& mean,
+                                                  double sigma);
+
+  // Testable
   void print(string s = "GaussianDensity",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianDensity &cg, double tol) const;
+  bool equals(const gtsam::GaussianDensity& cg, double tol) const;
+
+  // Standard Interface
   Vector mean() const;
   Matrix covariance() const;
 };
@@ -514,6 +543,21 @@ virtual class GaussianBayesNet {
   bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
   size_t size() const;
 
+  // Standard interface
+  void push_back(gtsam::GaussianConditional* conditional);
+  void push_back(const gtsam::GaussianBayesNet& bayesNet);
+  gtsam::GaussianConditional* front() const;
+  gtsam::GaussianConditional* back() const;
+
+  gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimize(gtsam::VectorValues given) const;
+  gtsam::VectorValues optimizeGradientSearch() const;
+  
+  gtsam::VectorValues sample(gtsam::VectorValues given) const;
+  gtsam::VectorValues sample() const;
+  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
+  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
+
   // FactorGraph derived interface
   gtsam::GaussianConditional* at(size_t idx) const;
   gtsam::KeySet keys() const;
@@ -522,22 +566,12 @@ virtual class GaussianBayesNet {
 
   void saveGraph(const string& s) const;
 
-  gtsam::GaussianConditional* front() const;
-  gtsam::GaussianConditional* back() const;
-  void push_back(gtsam::GaussianConditional* conditional);
-  void push_back(const gtsam::GaussianBayesNet& bayesNet);
-
-  gtsam::VectorValues optimize() const;
-  gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
   std::pair<Matrix, Vector> matrix() const; 
-  gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
   gtsam::VectorValues gradientAtZero() const;
   double error(const gtsam::VectorValues& x) const;
   double determinant() const;
   double logDeterminant() const;
-  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
-  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
 
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -16,10 +16,12 @@
  */
 
 #include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianDensity.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
@@ -35,6 +37,7 @@ using namespace boost::assign;
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
+using symbol_shorthand::X;
 
 static const Key _x_ = 11, _y_ = 22, _z_ = 33;
 
@@ -138,6 +141,30 @@ TEST( GaussianBayesNet, optimize3 )
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianBayesNet, sample) {
+  GaussianBayesNet gbn;
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 mean(20, 40), b(10, 10);
+  const double sigma = 0.01;
+
+  gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma));
+  gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
+
+  auto actual = gbn.sample();
+  EXPECT_LONGS_EQUAL(2, actual.size());
+  EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma));
+  EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma));
+
+  // Use a specific random generator
+  std::mt19937_64 rng(4242);
+  auto actual3 = gbn.sample(&rng);
+  EXPECT_LONGS_EQUAL(2, actual.size());
+  // regression is not repeatable across platforms/versions :-(
+  // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
+  // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
+}
+
 /* ************************************************************************* */
 TEST(GaussianBayesNet, ordering)
 {

gtsam/linear/tests/testGaussianConditional.cpp

@@ -20,9 +20,10 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianDensity.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 
 #include <boost/assign/std/list.hpp>
@@ -38,6 +39,7 @@
 using namespace gtsam;
 using namespace std;
 using namespace boost::assign;
+using symbol_shorthand::X;
 
 static const double tol = 1e-5;
 
@@ -316,5 +318,87 @@ TEST( GaussianConditional, isGaussianFactor ) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+// Test FromMeanAndStddev named constructors
+TEST(GaussianConditional, FromMeanAndStddev) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
+  const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
+  const double sigma = 3;
+
+  VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2);
+
+  auto conditional1 =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+  Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma;
+  double expected1 = 0.5 * e1.dot(e1);
+  EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
+
+  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
+                                                             X(2), b, sigma);
+  Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
+  double expected2 = 0.5 * e2.dot(e2);
+  EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
+}
+
+/* ************************************************************************* */
+// Test likelihood method (conversion to JacobianFactor)
+TEST(GaussianConditional, likelihood) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x0(1, 2);
+  const double sigma = 0.01;
+
+  // |x0 - A1 x1 - b|^2
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  VectorValues frontalValues;
+  frontalValues.insert(X(0), x0);
+  auto actual1 = conditional.likelihood(frontalValues);
+  CHECK(actual1);
+
+  // |(-A1) x1 - (b - x0)|^2
+  JacobianFactor expected(X(1), -A1, b - x0,
+                          noiseModel::Isotropic::Sigma(2, sigma));
+  EXPECT(assert_equal(expected, *actual1, tol));
+
+  // Check single vector version
+  auto actual2 = conditional.likelihood(x0);
+  CHECK(actual2);
+  EXPECT(assert_equal(expected, *actual2, tol));
+}
+
+/* ************************************************************************* */
+// Test sampling
+TEST(GaussianConditional, sample) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x1(3, 4);
+  const double sigma = 0.01;
+
+  auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
+  auto actual1 = density.sample();
+  EXPECT_LONGS_EQUAL(1, actual1.size());
+  EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma));
+
+  VectorValues given;
+  given.insert(X(1), x1);
+
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+  auto actual2 = conditional.sample(given);
+  EXPECT_LONGS_EQUAL(1, actual2.size());
+  EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma));
+
+  // Use a specific random generator
+  std::mt19937_64 rng(4242);
+  auto actual3 = conditional.sample(given, &rng);
+  EXPECT_LONGS_EQUAL(1, actual2.size());
+  // regression is not repeatable across platforms/versions :-(
+  // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/linear/tests/testGaussianDensity.cpp

@@ -17,10 +17,13 @@
  **/
 
 #include <gtsam/linear/GaussianDensity.h>
+#include <gtsam/inference/Symbol.h>
+
 #include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 using namespace std;
+using symbol_shorthand::X;
 
 /* ************************************************************************* */
 TEST(GaussianDensity, constructor)
@@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor)
   EXPECT(assert_equal(s, copied.get_model()->sigmas()));
 }
 
+/* ************************************************************************* */
+// Test FromMeanAndStddev named constructor
+TEST(GaussianDensity, FromMeanAndStddev) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x0(1, 2);
+  const double sigma = 3;
+
+  VectorValues values;
+  values.insert(X(0), x0);
+
+  auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
+  Vector2 e = (x0 - b) / sigma;
+  double expected = 0.5 * e.dot(e);
+  EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam/linear/tests/testVectorValues.cpp

@@ -17,7 +17,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/LabeledSymbol.h>
+#include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -248,6 +248,33 @@ TEST(VectorValues, print)
   EXPECT(expected == actual.str());
 }
 
+/* ************************************************************************* */
+// Check html representation.
+TEST(VectorValues, html) {
+  VectorValues vv;
+  using symbol_shorthand::X;
+  vv.insert(X(1), Vector2(2, 3.1));
+  vv.insert(X(2), Vector2(4, 5.2));
+  vv.insert(X(5), Vector2(6, 7.3));
+  vv.insert(X(7), Vector2(8, 9.4));
+  string expected =
+      "<div>\n"
+      "<table class='VectorValues'>\n"
+      "  <thead>\n"
+      "    <tr><th>Variable</th><th>value</th></tr>\n"
+      "  </thead>\n"
+      "  <tbody>\n"
+      "    <tr><th>x1</th><td>  2 3.1</td></tr>\n"
+      "    <tr><th>x2</th><td>  4 5.2</td></tr>\n"
+      "    <tr><th>x5</th><td>  6 7.3</td></tr>\n"
+      "    <tr><th>x7</th><td>  8 9.4</td></tr>\n"
+      "  </tbody>\n"
+      "</table>\n"
+      "</div>";
+  string actual = vv.html();
+  EXPECT(actual == expected);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/FrobeniusFactor.h

@@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// Constructor
   FrobeniusPrior(Key j, const MatrixNN& M,
                  const SharedNoiseModel& model = nullptr)
@@ -106,6 +108,8 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// @name Constructor
   /// @{
 

python/gtsam/preamble/inference.h

@@ -10,3 +10,5 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
+
+#include <pybind11/stl.h>

python/gtsam/tests/test_DiscreteBayesNet.py

@@ -12,7 +12,9 @@ Author: Frank Dellaert
 # pylint: disable=no-name-in-module, invalid-name
 
 import unittest
+import textwrap
 
+import gtsam
 from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
                    DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
@@ -126,6 +128,39 @@ class TestDiscreteBayesNet(GtsamTestCase):
         actual = fragment.sample(given)
         self.assertEqual(len(actual), 5)
 
+    def test_dot(self):
+        """Check that dot works with position hints."""
+        fragment = DiscreteBayesNet()
+        fragment.add(Either, [Tuberculosis, LungCancer], "F T T T")
+        MyAsia = gtsam.symbol('a', 0), 2  # use a symbol!
+        fragment.add(Tuberculosis, [MyAsia], "99/1 95/5")
+        fragment.add(LungCancer, [Smoking], "99/1 90/10")
+
+        # Make sure we can *update* position hints
+        writer = gtsam.DotWriter()
+        ph: dict = writer.positionHints
+        ph.update({'a': 2})  # hint at symbol position
+        writer.positionHints = ph
+
+        # Check the output of dot
+        actual = fragment.dot(writer=writer)
+        expected_result = """\
+            digraph {
+              size="5,5";
+
+              var3[label="3"];
+              var4[label="4"];
+              var5[label="5"];
+              var6[label="6"];
+              vara0[label="a0", pos="0,2!"];
+
+              var4->var6
+              vara0->var3
+              var3->var5
+              var6->var5
+            }"""
+        self.assertEqual(actual, textwrap.dedent(expected_result))
+
 
 if __name__ == "__main__":
     unittest.main()

tests/testGaussianBayesTreeB.cpp

@@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
    C4      x7 : x6
 
 ************************************************************************* */
-TEST( GaussianBayesTree, balanced_smoother_marginals )
+TEST(GaussianBayesTree, balanced_smoother_marginals) {
-{
   // Create smoother with 7 nodes
   GaussianFactorGraph smoother = createSmoother(7);
 
   // Create the Bayes tree
   Ordering ordering;
-  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
+  ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4);
   GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
 
   VectorValues actualSolution = bayesTree.optimize();
   VectorValues expectedSolution = VectorValues::Zero(actualSolution);
-  EXPECT(assert_equal(expectedSolution,actualSolution,tol));
+  EXPECT(assert_equal(expectedSolution, actualSolution, tol));
 
-  LONGS_EQUAL(4, (long)bayesTree.size());
+  LONGS_EQUAL(4, bayesTree.size());
 
-  double tol=1e-5;
+  double tol = 1e-5;
 
   // Check marginal on x1
-  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
   JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
-  Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
+  Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
-  Matrix actualCovarianceX1;
+  auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
-  GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
+  Matrix actualCovarianceX1 = m->information().inverse();
-  actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
+  EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
-  EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
-  EXPECT(assert_equal(expected1,actual1,tol));
 
   // Check marginal on x2
-  double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
+  double sigmax2 = 0.68712938;  // FIXME: this should be corrected analytically
-  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
   JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
-  EXPECT(assert_equal(expected2,actual2,tol));
+  Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
+  EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
 
   // Check marginal on x3
-  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
   JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
-  EXPECT(assert_equal(expected3,actual3,tol));
+  Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
+  EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
 
   // Check marginal on x4
-  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
   JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
-  EXPECT(assert_equal(expected4,actual4,tol));
+  Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
+  EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
 
   // Check marginal on x7 (should be equal to x1)
-  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
   JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
-  EXPECT(assert_equal(expected7,actual7,tol));
+  Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
+  EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
 }
 
 /* ************************************************************************* */