diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 8c23ae9e5..a5ee77495 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 24355c684..e615afe83 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 06b2856f8..0d6c5e976 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 48d94a383..cff1b69a6 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/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 7bc23bd45..cf3f78b73 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -224,6 +224,50 @@ 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 { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e50928ba2..6dd278536 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -151,6 +151,13 @@ 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: diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6a2514b35..62996af27 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 9e60ff2aa..1ff8c5c16 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 113ebb8ec..f1bc92f69 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 @@ -491,6 +492,9 @@ virtual class GaussianConditional : gtsam::JacobianFactor { // 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; diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index df1b76d06..8525cf9e0 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -340,6 +340,33 @@ TEST(GaussianConditional, FromMeanAndStddev) { 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) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index f97f96aaf..521cc2289 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); } /* ************************************************************************* */