diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 8c23ae9e54..a5ee774957 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,6 +18,9 @@ #pragma once +#if BOOST_VERSION >= 107400 +#include +#endif #include #include #include diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 24355c6845..e615afe83d 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -25,6 +25,7 @@ #include // includes for standard serialization types +#include #include #include #include diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 06b2856f8f..0d6c5e976f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/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); } diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 48d94a3837..cff1b69a63 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/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 diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index cfc9c1bb50..19af676f7d 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/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" diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8fd4f2c26d..6dcf662a90 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/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); diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 6d906d65e3..940ffd8828 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/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. diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index d87c39eeaf..cf3f78b731 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #ifdef __GNUC__ @@ -34,6 +35,9 @@ #include #include +// 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(); + 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(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 { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d93f65b425..6dd278536a 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -26,12 +26,15 @@ #include #include +#include // 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 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, - Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, + const SharedDiagonal& sigmas = SharedDiagonal()); /** constructor with two parents |Rx+Sy+Tz-d| */ - GaussianConditional(Key key, const Vector& d, const Matrix& R, - Key name1, const Matrix& S, Key name2, const Matrix& T, - const SharedDiagonal& sigmas = SharedDiagonal()); + GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1, + const Matrix& S, Key parent2, const Matrix& T, + const SharedDiagonal& sigmas = SharedDiagonal()); /** Constructor with arbitrary number of frontals and parents. * @tparam TERMS A container whose value type is std::pair, 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 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: diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index d9cde9b91c..2f7aa312b1 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,9 +23,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma) - { - return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); + GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, + const Vector& mean, + 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(Vector(d()), "d: "); + gtsam::print(mean(), "mean: "); + gtsam::print(covariance(), "covariance: "); if(model_) model_->print("Noise model: "); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 71af704ab8..f078d5db64 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -24,11 +24,10 @@ namespace gtsam { /** - * A Gaussian density. - * - * It is implemented as a GaussianConditional without parents. + * A GaussianDensity is 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 - static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma); + /// Construct using a mean and standard deviation + static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, + double sigma); /// print void print(const std::string& = "GaussianDensity", diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 4957dfa14b..20d4c955b7 100644 --- a/gtsam/linear/Sampler.cpp +++ b/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 Normal; - Normal dist(0.0, sigma); - result(i) = dist(generator_); + std::normal_distribution dist(0.0, sigma); + result(i) = dist(*rng); } } return result; } +/* ************************************************************************* */ +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { + return sampleDiagonal(sigmas, &generator_); +} + /* ************************************************************************* */ Vector Sampler::sample() const { assert(model_.get()); diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index bb5098f343..5be8b445da 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler { /// @name access functions /// @{ - size_t dim() const { - assert(model_.get()); - return model_->dim(); - } + size_t dim() const { return model_->dim(); } - Vector sigmas() const { - assert(model_.get()); - return model_->sigmas(); - } + Vector sigmas() const { 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: diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6a2514b358..62996af27d 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/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; 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_value) { std::pair 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()); } - /* ************************************************************************* */ + /* ************************************************************************ */ 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 << "
\n\n \n"; + + // Print out header row. + ss << " \n"; + + // Finish header and start body. + ss << " \n \n"; + + // Print out all rows. +#ifdef GTSAM_USE_TBB + // TBB uses un-ordered map, so inefficiently order them: + std::map ordered; + for (const auto& kv : *this) ordered.emplace(kv); + for (const auto& kv : ordered) { +#else + for (const auto& kv : *this) { +#endif + ss << " "; + ss << ""; + ss << "\n"; + } + ss << " \n
Variablevalue
" << keyFormatter(kv.first) << "" + << kv.second.transpose() << "
\n
"; + return ss.str(); + } + + /* ************************************************************************ */ } // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 9e60ff2aaa..1ff8c5c169 100644 --- a/gtsam/linear/VectorValues.h +++ b/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 Matlab syntactic sugar for linear algebra operations + /// @name Wrapper support /// @{ - //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; /// @} diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 674287b87f..f1bc92f69c 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -255,6 +255,7 @@ class VectorValues { // enabling serialization functionality void serialize() const; + string html() const; }; #include @@ -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 virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + // Constructors + 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() 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, diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index f62da15dde..2b125265f7 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -16,10 +16,12 @@ */ #include +#include #include #include #include #include +#include #include #include @@ -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) { diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index fae00e1e49..8525cf9e03 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -20,9 +20,10 @@ #include #include -#include +#include #include #include +#include #include #include @@ -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); +} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 29dc495912..14608e7949 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -17,10 +17,13 @@ **/ #include +#include + #include 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);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index f97f96aafd..521cc2289e 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -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 = + "
\n" + "\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + "
Variablevalue
x1 2 3.1
x2 4 5.2
x5 6 7.3
x7 8 9.4
\n" + "
"; + string actual = vv.html(); + EXPECT(actual == expected); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index f17a9e4217..4a60c8ba02 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1 { Eigen::Matrix 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 { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// @name Constructor /// @{ diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h index d07a75f6fb..4106c794ac 100644 --- a/python/gtsam/preamble/inference.h +++ b/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 \ No newline at end of file diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 3ae3b625cd..74191dcc7a 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -12,7 +12,9 @@ # 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 @@ def test_fragment(self): 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() diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index b8b6cf284f..a321aa25de 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/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 actualCovarianceX1; - GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); - actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); - EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); - EXPECT(assert_equal(expected1,actual1,tol)); + Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1); + auto m = bayesTree.marginalFactor(X(1), EliminateCholesky); + Matrix actualCovarianceX1 = m->information().inverse(); + EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol)); // Check marginal on x2 - double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); + double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically 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)); } /* ************************************************************************* */