diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index c9ef922be..22f1918e4 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -138,23 +138,33 @@ namespace gtsam { //} /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const { + Ordering GaussianBayesNet::ordering() const { GaussianFactorGraph factorGraph(*this); - KeySet keys = factorGraph.keys(); + auto keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - for (const sharedConditional& cg: *this) + for (const sharedConditional& cg : *this) if (cg) { - for (Key key: cg->frontals()) { + for (Key key : cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - for (Key key: keys) - ordering.push_back(key); - // return matrix and RHS - return factorGraph.jacobian(ordering); + for (Key key : keys) ordering.push_back(key); + return ordering; + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix(boost::optional ordering) const { + if (ordering) { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } else { + // recursively call with default ordering + return matrix(this->ordering()); + } } ///* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 401583bbf..64627a276 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -74,6 +74,14 @@ namespace gtsam { /// Version of optimize for incomplete BayesNet, needs solution for missing variables VectorValues optimize(const VectorValues& solutionForMissing) const; + /** + * Return ordering corresponding to a topological sort. + * There are many topological sorts of a Bayes net. This one + * corresponds to the one that makes 'matrix' below upper-triangular. + * In case Bayes net is incomplete any non-frontal are added to the end. + */ + Ordering ordering() const; + ///@} ///@name Linear Algebra @@ -81,8 +89,10 @@ namespace gtsam { /** * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix() const; + std::pair matrix(boost::optional ordering = boost::none) const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index b037aad92..7ff773bd5 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -142,19 +142,25 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector() const - { + Vector VectorValues::vector(boost::optional ordering) const { // Count dimensions DenseIndex totalDim = 0; - for(const Vector& v: *this | map_values) - totalDim += v.size(); + for (const Vector& v : *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - for(const Vector& v: *this | map_values) { - result.segment(pos, v.size()) = v; - pos += v.size(); + if (ordering) { + for (const auto& key : *ordering) { + const auto& v = (*this)[key]; + result.segment(pos, v.size()) = v; + pos += v.size(); + } + } else { + for (const Vector& v : *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } } return result; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..5360edeff 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -244,7 +244,7 @@ namespace gtsam { /// @{ /** Retrieve the entire solution as a single vector */ - Vector vector() const; + Vector vector(boost::optional ordering = boost::none) const; /** Access a vector that is a subset of relevant keys. */ template