Merge pull request #1095 from borglab/feature/linear_improvements

borglab · Feb 12, 2022 · 3e65779 · 3e65779
2 parents a5bee15 + 6337241
commit 3e65779
Show file tree

Hide file tree

Showing 11 changed files with 189 additions and 40 deletions.
diff --git a/gtsam/base/FastSet.h b/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>

diff --git a/gtsam/base/serialization.h b/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>

diff --git 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
@@ -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
@@ -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<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 {

diff --git 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
@@ -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,15 +150,15 @@ namespace gtsam {
     return os;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::print(const string& str,
                            const KeyFormatter& formatter) const {
     cout << str << ": " << size() << " elements\n";
     cout << key_formatter(formatter) << *this;
     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,26 +359,57 @@ 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)
       v *= alpha;
     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