From 9bdc5f1113aff91ca4ac046a45599a8e2500de49 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 9 Jul 2013 17:50:38 +0000 Subject: [PATCH] Small cleanups and fixes in included header files, functions in header file vs cpp/inst file, formatting, private/public, and inheritance. --- gtsam/inference/BayesNetUnordered-inst.h | 1 + .../inference/BayesTreeCliqueBaseUnordered.h | 11 +-- gtsam/inference/BayesTreeUnordered.h | 20 ++--- gtsam/inference/ConditionalUnordered-inst.h | 7 ++ gtsam/inference/ConditionalUnordered.h | 6 +- gtsam/inference/EliminateableFactorGraph.h | 13 +-- gtsam/inference/EliminationTreeUnordered.h | 23 ++--- gtsam/inference/JunctionTreeUnordered-inst.h | 1 + gtsam/inference/JunctionTreeUnordered.h | 3 - gtsam/linear/GaussianBayesNetUnordered.cpp | 16 ++-- gtsam/linear/GaussianBayesNetUnordered.h | 27 +++--- gtsam/linear/GaussianBayesTreeUnordered.cpp | 1 - gtsam/linear/GaussianBayesTreeUnordered.h | 19 ++-- gtsam/linear/GaussianFactorGraphUnordered.cpp | 22 ++--- gtsam/linear/GaussianFactorGraphUnordered.h | 6 ++ gtsam/linear/GaussianFactorUnordered.h | 7 +- gtsam/linear/JacobianFactorUnordered.cpp | 4 +- gtsam/linear/JacobianFactorUnordered.h | 34 ++++---- gtsam/linear/VectorValuesUnordered.cpp | 2 +- gtsam/linear/VectorValuesUnordered.h | 1 + gtsam/linear/linearExceptions.h | 4 +- gtsam/symbolic/SymbolicBayesNetUnordered.cpp | 5 +- gtsam/symbolic/SymbolicBayesNetUnordered.h | 11 ++- gtsam/symbolic/SymbolicBayesTreeUnordered.cpp | 86 +++---------------- gtsam/symbolic/SymbolicBayesTreeUnordered.h | 15 +++- gtsam/symbolic/SymbolicConditionalUnordered.h | 2 + .../SymbolicEliminationTreeUnordered.cpp | 27 +++++- .../SymbolicEliminationTreeUnordered.h | 20 ++--- .../symbolic/SymbolicFactorGraphUnordered.cpp | 8 ++ gtsam/symbolic/SymbolicFactorGraphUnordered.h | 20 ++--- gtsam/symbolic/SymbolicFactorUnordered.cpp | 6 ++ gtsam/symbolic/SymbolicFactorUnordered.h | 12 ++- .../SymbolicJunctionTreeUnordered.cpp | 15 +++- .../symbolic/SymbolicJunctionTreeUnordered.h | 38 +++++--- 34 files changed, 278 insertions(+), 215 deletions(-) diff --git a/gtsam/inference/BayesNetUnordered-inst.h b/gtsam/inference/BayesNetUnordered-inst.h index 312b01bb1..fa9096051 100644 --- a/gtsam/inference/BayesNetUnordered-inst.h +++ b/gtsam/inference/BayesNetUnordered-inst.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered.h b/gtsam/inference/BayesTreeCliqueBaseUnordered.h index 3470e842b..c237e6eaf 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered.h @@ -18,8 +18,6 @@ #pragma once #include -#include -#include namespace gtsam { template class BayesTreeUnordered; @@ -41,8 +39,8 @@ namespace gtsam { * @tparam CONDITIONAL The conditional type. * \nosubgrouping */ template - struct BayesTreeCliqueBaseUnordered { - + class BayesTreeCliqueBaseUnordered + { private: typedef BayesTreeCliqueBaseUnordered This; typedef DERIVED DerivedType; @@ -84,10 +82,7 @@ namespace gtsam { /// @{ /** check equality */ - bool equals(const DERIVED& other, double tol = 1e-9) const { - return (!conditional_ && !other.conditional()) - || conditional_->equals(*other.conditional(), tol); - } + bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; diff --git a/gtsam/inference/BayesTreeUnordered.h b/gtsam/inference/BayesTreeUnordered.h index 8ced6d08e..ec0087737 100644 --- a/gtsam/inference/BayesTreeUnordered.h +++ b/gtsam/inference/BayesTreeUnordered.h @@ -39,12 +39,13 @@ namespace gtsam { * \nosubgrouping */ template - class BayesTreeUnordered { - - public: - + class BayesTreeUnordered + { + protected: typedef BayesTreeUnordered This; typedef boost::shared_ptr shared_ptr; + + public: typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique typedef Clique Node; ///< Synonym for Clique (TODO: remove) @@ -102,9 +103,6 @@ namespace gtsam { /** Copy constructor */ BayesTreeUnordered(const This& other); - /** Destructor */ - virtual ~BayesTreeUnordered() {} - /// @} /** Assignment operator */ @@ -225,14 +223,6 @@ namespace gtsam { /** add a clique (top down) */ void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); - /** - * Create a clone of this object as a shared pointer - * Necessary for inheritance in matlab interface - */ - virtual shared_ptr clone() const { - return shared_ptr(new This(*this)); - } - protected: /** private helper method for saving the Tree to a text file in GraphViz format */ diff --git a/gtsam/inference/ConditionalUnordered-inst.h b/gtsam/inference/ConditionalUnordered-inst.h index 0167271d0..11ae7f7a5 100644 --- a/gtsam/inference/ConditionalUnordered-inst.h +++ b/gtsam/inference/ConditionalUnordered-inst.h @@ -38,4 +38,11 @@ namespace gtsam { std::cout << ")" << std::endl; } + /* ************************************************************************* */ + template + bool ConditionalUnordered::equals(const This& c, double tol = 1e-9) const + { + return nrFrontals_ == c.nrFrontals_; + } + } diff --git a/gtsam/inference/ConditionalUnordered.h b/gtsam/inference/ConditionalUnordered.h index 6e14dbe6e..4d0c6ef9e 100644 --- a/gtsam/inference/ConditionalUnordered.h +++ b/gtsam/inference/ConditionalUnordered.h @@ -52,6 +52,7 @@ namespace gtsam { /** View of the separator keys (call parents()) */ typedef boost::iterator_range Parents; + protected: /// @name Standard Constructors /// @{ @@ -69,10 +70,11 @@ namespace gtsam { void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; /** check equality */ - bool equals(const This& c, double tol = 1e-9) const { - return asFactor().equals(c.asFactor()) && nrFrontals_ == c.nrFrontals_; } + bool equals(const This& c, double tol = 1e-9) const; /// @} + + public: /// @name Standard Interface /// @{ diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index e077d84f0..1ed2d2c3d 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -61,20 +61,23 @@ namespace gtsam { typedef typename EliminationTraits::FactorType _FactorType; public: + /// Typedef to the specific EliminationTraits for this graph + typedef EliminationTraits EliminationTraits; + /// Conditional type stored in the Bayes net produced by elimination - typedef typename EliminationTraits::ConditionalType ConditionalType; + typedef typename EliminationTraits::ConditionalType ConditionalType; /// Bayes net type produced by sequential elimination - typedef typename EliminationTraits::BayesNetType BayesNetType; + typedef typename EliminationTraits::BayesNetType BayesNetType; /// Elimination tree type that can do sequential elimination of this graph - typedef typename EliminationTraits::EliminationTreeType EliminationTreeType; + typedef typename EliminationTraits::EliminationTreeType EliminationTreeType; /// Bayes tree type produced by multifrontal elimination - typedef typename EliminationTraits::BayesTreeType BayesTreeType; + typedef typename EliminationTraits::BayesTreeType BayesTreeType; /// Junction tree type that can do multifrontal elimination of this graph - typedef typename EliminationTraits::JunctionTreeType JunctionTreeType; + typedef typename EliminationTraits::JunctionTreeType JunctionTreeType; /// The pair of conditional and remaining factor produced by a single dense elimination step on /// a subgraph. diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index 761a10500..38f160fa5 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -47,14 +47,15 @@ namespace gtsam { * \nosubgrouping */ template - class EliminationTreeUnordered { - - public: - - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors + class EliminationTreeUnordered + { + protected: typedef EliminationTreeUnordered This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals @@ -77,16 +78,13 @@ namespace gtsam { typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node - private: - + protected: /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); std::vector roots_; std::vector remainingFactors_; - public: - /// @name Standard Constructors /// @{ @@ -117,6 +115,8 @@ namespace gtsam { This& operator=(const This& other); /// @} + + public: /// @name Standard Interface /// @{ @@ -136,10 +136,13 @@ namespace gtsam { void print(const std::string& name = "EliminationTree: ", const KeyFormatter& formatter = DefaultKeyFormatter) const; + protected: /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; /// @} + + public: /// @name Advanced Interface /// @{ diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index 1dc1e2895..24c608821 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h index 39a8f9a61..27dd22f14 100644 --- a/gtsam/inference/JunctionTreeUnordered.h +++ b/gtsam/inference/JunctionTreeUnordered.h @@ -22,8 +22,6 @@ #include #include -#include -#include namespace gtsam { @@ -44,7 +42,6 @@ namespace gtsam { * The tree structure and elimination method are exactly analagous to the EliminationTree, * except that in the JunctionTree, at each node multiple variables are eliminated at a time. * - * * \addtogroup Multifrontal * \nosubgrouping */ diff --git a/gtsam/linear/GaussianBayesNetUnordered.cpp b/gtsam/linear/GaussianBayesNetUnordered.cpp index 6d4f4ea57..6909fcb9f 100644 --- a/gtsam/linear/GaussianBayesNetUnordered.cpp +++ b/gtsam/linear/GaussianBayesNetUnordered.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -30,7 +31,7 @@ using namespace gtsam; namespace gtsam { /* ************************************************************************* */ - bool GaussianBayesNetUnordered::equals(const This& bn, double tol = 1e-9) const + bool GaussianBayesNetUnordered::equals(const This& bn, double tol) const { return Base::equals(bn, tol); } @@ -124,13 +125,18 @@ namespace gtsam { //} /* ************************************************************************* */ - VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(GaussianFactorGraph(bayesNet), x0); + double GaussianBayesNetUnordered::determinant() const + { + return exp(logDeterminant()); } /* ************************************************************************* */ - void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(GaussianFactorGraph(bayesNet), g); + double GaussianBayesNetUnordered::logDeterminant() const + { + double logDet = 0.0; + BOOST_FOREACH(const sharedConditional& cg, *this) + logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + return logDet; } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNetUnordered.h b/gtsam/linear/GaussianBayesNetUnordered.h index 52418d0af..063147012 100644 --- a/gtsam/linear/GaussianBayesNetUnordered.h +++ b/gtsam/linear/GaussianBayesNetUnordered.h @@ -21,20 +21,21 @@ #pragma once #include -#include +#include #include namespace gtsam { /** A Bayes net made from linear-Gaussian densities */ - class GTSAM_EXPORT GaussianBayesNetUnordered: public BayesNetUnordered { + class GTSAM_EXPORT GaussianBayesNetUnordered: public FactorGraphUnordered { public: - typedef BayesNetUnordered Base; + typedef FactorGraphUnordered Base; typedef GaussianBayesNetUnordered This; typedef GaussianConditionalUnordered ConditionalType; typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors /// @{ @@ -128,16 +129,22 @@ namespace gtsam { //VectorValuesUnordered gradientAtZero() const; /** - * Computes the determinant of a GassianBayesNet - * A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements and take the exponent at the end - * because this is more numerically stable. + * Computes the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular + * matrix and for an upper triangular matrix determinant is the product of the diagonal + * elements. Instead of actually multiplying we add the logarithms of the diagonal elements and + * take the exponent at the end because this is more numerically stable. * @param bayesNet The input GaussianBayesNet - * @return The determinant - */ + * @return The determinant */ double determinant() const; + /** + * Computes the log of the determinant of a GassianBayesNet. A GaussianBayesNet is an upper + * triangular matrix and for an upper triangular matrix determinant is the product of the + * diagonal elements. + * @param bayesNet The input GaussianBayesNet + * @return The determinant */ + double logDeterminant() const; + /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx diff --git a/gtsam/linear/GaussianBayesTreeUnordered.cpp b/gtsam/linear/GaussianBayesTreeUnordered.cpp index 8ee514db3..7e21fe3a5 100644 --- a/gtsam/linear/GaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/GaussianBayesTreeUnordered.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/linear/GaussianBayesTreeUnordered.h b/gtsam/linear/GaussianBayesTreeUnordered.h index 974a47720..396a4a9c3 100644 --- a/gtsam/linear/GaussianBayesTreeUnordered.h +++ b/gtsam/linear/GaussianBayesTreeUnordered.h @@ -19,14 +19,14 @@ #pragma once +#include +#include #include #include namespace gtsam { // Forward declarations - class GaussianFactorGraphUnordered; - class GaussianBayesNetUnordered; class GaussianConditionalUnordered; class VectorValuesUnordered; @@ -56,16 +56,23 @@ namespace gtsam { typedef GaussianBayesTreeUnordered This; typedef boost::shared_ptr shared_ptr; + /** Default constructor, creates an empty Bayes tree */ + GaussianBayesTreeUnordered() {} + + /** Makes a deep copy of the tree structure, but only pointers to conditionals are + * copied, the conditionals and their matrices are not cloned. */ + GaussianBayesTreeUnordered(const GaussianBayesTreeUnordered& other); + + /** Makes a deep copy of the tree structure, but only pointers to conditionals are + * copied, the conditionals and their matrices are not cloned. */ + GaussianBayesTreeUnordered& operator=(const GaussianBayesTreeUnordered& other); + /** Check equality */ bool equals(const This& other, double tol = 1e-9) const; /** Recursively optimize the BayesTree to produce a vector solution. */ VectorValuesUnordered optimize() const; - /** Recursively optimize the BayesTree to produce a vector solution (same as optimize()), but - * write the solution in place in \c result. */ - //void optimizeInPlace(VectorValuesUnordered& result) const; - /** * Optimize along the gradient direction, with a closed-form computation to perform the line * search. The gradient is computed about \f$ \delta x=0 \f$. diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp index c36268c99..5d0c77ab0 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -21,6 +21,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -45,16 +49,6 @@ namespace gtsam { return keys; } - /* ************************************************************************* */ - GaussianFactorGraphUnordered GaussianFactorGraphUnordered::combine2( - const GaussianFactorGraphUnordered& lfg1, const GaussianFactorGraphUnordered& lfg2) - { - // Copy the first graph and add the second graph - GaussianFactorGraphUnordered fg = lfg1; - fg.push_back(lfg2); - return fg; - } - /* ************************************************************************* */ std::vector > GaussianFactorGraphUnordered::sparseJacobian() const { // First find dimensions of each variable @@ -139,7 +133,7 @@ namespace gtsam { Matrix GaussianFactorGraphUnordered::augmentedJacobian() const { // combine all factors JacobianFactorUnordered combined(*this); - return combined.matrix_augmented(); + return combined.augmentedJacobian(); } /* ************************************************************************* */ @@ -200,6 +194,12 @@ namespace gtsam { // combinedFactor->assertInvariants(); // return make_pair(conditional, combinedFactor); //} + + /* ************************************************************************* */ + VectorValuesUnordered GaussianFactorGraphUnordered::optimize(const Eliminate& function) const + { + return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + } ///* ************************************************************************* */ //VectorValuesUnordered GaussianFactorGraphUnordered::gradient(const VectorValuesUnordered& x0) const diff --git a/gtsam/linear/GaussianFactorGraphUnordered.h b/gtsam/linear/GaussianFactorGraphUnordered.h index a0c324014..2ca25f7ac 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.h +++ b/gtsam/linear/GaussianFactorGraphUnordered.h @@ -195,6 +195,12 @@ namespace gtsam { */ //std::pair hessian() const; + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using + * the dense elimination function specified in \c function (default EliminatePreferCholesky), + * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent + * to calling graph.eliminateMultifrontal()->optimize(). */ + VectorValuesUnordered optimize(const Eliminate& function = EliminationTraits::DefaultEliminate) const; + /** * Compute the gradient of the energy function, * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, diff --git a/gtsam/linear/GaussianFactorUnordered.h b/gtsam/linear/GaussianFactorUnordered.h index 296375088..33de77054 100644 --- a/gtsam/linear/GaussianFactorUnordered.h +++ b/gtsam/linear/GaussianFactorUnordered.h @@ -67,7 +67,7 @@ namespace gtsam { * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - virtual Matrix augmentedJacobian() const = 0; + virtual Matrix augmentedJacobian(bool weight = true) const = 0; /** * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, @@ -76,7 +76,7 @@ namespace gtsam { * GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::sparseJacobian. */ - virtual std::pair jacobian() const = 0; + virtual std::pair jacobian(bool weight = true) const = 0; /** Return the augmented information matrix represented by this GaussianFactorUnordered. * The augmented information matrix contains the information matrix with an @@ -96,6 +96,9 @@ namespace gtsam { /** Clone a factor (make a deep copy) */ virtual GaussianFactorUnordered::shared_ptr clone() const = 0; + /** Test whether the factor is empty */ + virtual bool empty() const = 0; + /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp index dd1298527..1059f97be 100644 --- a/gtsam/linear/JacobianFactorUnordered.cpp +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -402,7 +402,7 @@ namespace gtsam { } /* ************************************************************************* */ - pair JacobianFactorUnordered::matrix(bool weight) const { + pair JacobianFactorUnordered::jacobian(bool weight) const { Matrix A(Ab_.range(0, size())); Vector b(getb()); // divide in sigma so error is indeed 0.5*|Ax-b| @@ -411,7 +411,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix JacobianFactorUnordered::matrix_augmented(bool weight) const { + Matrix JacobianFactorUnordered::augmentedJacobian(bool weight) const { if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; } else return Ab_.range(0, Ab_.nBlocks()); } diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h index 31ff89b17..e14a5d894 100644 --- a/gtsam/linear/JacobianFactorUnordered.h +++ b/gtsam/linear/JacobianFactorUnordered.h @@ -95,7 +95,7 @@ namespace gtsam { explicit JacobianFactorUnordered(const GaussianFactorUnordered& gf); /** default constructor for I/O */ - JacobianFactorUnordered(); + JacobianFactorUnordered() {} /** Construct Null factor */ explicit JacobianFactorUnordered(const Vector& b_in); @@ -118,7 +118,7 @@ namespace gtsam { * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */ template - JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model); + JacobianFactorUnordered(const TERMS& terms, const Vector& b, const SharedDiagonal& model); /** Constructor with arbitrary number keys, and where the augmented matrix is given all together * instead of in block terms. Note that only the active view of the provided augmented matrix @@ -172,6 +172,20 @@ namespace gtsam { * GaussianFactor. */ virtual Matrix information() const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + virtual std::pair jacobian(bool weight = true) const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param set weight to use whitening to bake in weights + */ + virtual Matrix augmentedJacobian(bool weight = true) const; /** * Construct the corresponding anti-factor to negate information @@ -184,7 +198,7 @@ namespace gtsam { * not necessarily mean that the factor involves no variables (to check for * involving no variables use keys().empty()). */ - bool empty() const { return Ab_.rows() == 0;} + virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ bool isConstrained() const { return model_->isConstrained(); } @@ -239,20 +253,6 @@ namespace gtsam { /** x += A'*e */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesUnordered& x) const; - /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights - */ - std::pair matrix(bool weight = true) const; - - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param set weight to use whitening to bake in weights - */ - Matrix matrix_augmented(bool weight = true) const; - /** * Return vector of i, j, and s to generate an m-by-n sparse matrix * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. diff --git a/gtsam/linear/VectorValuesUnordered.cpp b/gtsam/linear/VectorValuesUnordered.cpp index 57625fb8f..dc252afca 100644 --- a/gtsam/linear/VectorValuesUnordered.cpp +++ b/gtsam/linear/VectorValuesUnordered.cpp @@ -68,7 +68,7 @@ namespace gtsam { void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; BOOST_FOREACH(const value_type& key_value, *this) - std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n"; + std::cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; std::cout.flush(); } diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index f4ded5dad..2db9c1898 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -97,6 +97,7 @@ namespace gtsam { typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair + typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair /// @name Standard Constructors /// @{ diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index a3543d244..68acf526b 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -117,7 +117,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n"; /* ************************************************************************* */ /** An exception indicating that the noise model dimension passed into a * JacobianFactor has a different dimensionality than the factor. */ - class InvalidNoiseModel : public std::exception { + class GTSAM_EXPORT InvalidNoiseModel : public std::exception { public: const DenseIndex factorDims; ///< The dimensionality of the factor const DenseIndex noiseModelDims; ///< The dimensionality of the noise model @@ -135,7 +135,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n"; /* ************************************************************************* */ /** An exception indicating that a matrix block passed into a * JacobianFactor has a different dimensionality than the factor. */ - class InvalidMatrixBlock : public std::exception { + class GTSAM_EXPORT InvalidMatrixBlock : public std::exception { public: const DenseIndex factorRows; ///< The dimensionality of the factor const DenseIndex blockRows; ///< The dimensionality of the noise model diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp index c43d6425b..aaf34d9b3 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts */ -#include +#include #include +#include namespace gtsam { /* ************************************************************************* */ - bool SymbolicBayesNetUnordered::equals(const This& bn, double tol = 1e-9) const + bool SymbolicBayesNetUnordered::equals(const This& bn, double tol) const { return Base::equals(bn, tol); } diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h index 9fbd4f7ad..1d4d1f608 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.h +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -18,23 +18,26 @@ #pragma once -#include -#include +#include #include namespace gtsam { + // Forward declarations + class SymbolicConditionalUnordered; + /** Symbolic Bayes Net * \nosubgrouping */ - class GTSAM_EXPORT SymbolicBayesNetUnordered: public BayesNetUnordered { + class GTSAM_EXPORT SymbolicBayesNetUnordered : public FactorGraphUnordered { public: - typedef BayesNetUnordered Base; + typedef FactorGraphUnordered Base; typedef SymbolicBayesNetUnordered This; typedef SymbolicConditionalUnordered ConditionalType; typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors /// @{ diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp index 00bc659b2..1146b7cfc 100644 --- a/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp @@ -21,87 +21,27 @@ #include #include #include +#include #include #include namespace gtsam { + /* ************************************************************************* */ + SymbolicBayesTreeUnordered::SymbolicBayesTreeUnordered(const SymbolicBayesTreeUnordered& other) : + Base(other) {} + + /* ************************************************************************* */ + SymbolicBayesTreeUnordered& SymbolicBayesTreeUnordered::operator=(const SymbolicBayesTreeUnordered& other) + { + (void) Base::operator=(other); + return *this; + } + + /* ************************************************************************* */\ bool SymbolicBayesTreeUnordered::equals(const This& other, double tol /* = 1e-9 */) const { return Base::equals(other, tol); } -// /* ************************************************************************* */ -// void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional) -// { -// static const bool debug = false; -// -// // get indices and parents -// const SymbolicConditionalUnordered::Parents& parents = conditional->parents(); -// -// if(debug) conditional->print("Adding conditional "); -// -// // if no parents, start a new root clique -// if (parents.empty()) { -// if(debug) std::cout << "No parents so making root" << std::endl; -// addClique(boost::make_shared(conditional)); -// return; -// } -// -// // otherwise, find the parent clique by using the index data structure -// // to find the lowest-ordered parent -// Index parentRepresentative = findParentClique(parents); -// if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; -// sharedClique parent_clique = bayesTree[parentRepresentative]; -// if(debug) parent_clique->print("Parent clique is "); -// -// // if the parents and parent clique have the same size, add to parent clique -// if ((*parent_clique)->size() == size_t(parents.size())) { -// if(debug) std::cout << "Adding to parent clique" << std::endl; -//#ifndef NDEBUG -// // Debug check that the parent indices of the new conditional match the indices -// // currently in the clique. -// // list::const_iterator parent = parents.begin(); -// // typename Clique::const_iterator cond = parent_clique->begin(); -// // while(parent != parents.end()) { -// // assert(cond != parent_clique->end()); -// // assert(*parent == (*cond)->key()); -// // ++ parent; -// // ++ cond; -// // } -//#endif -// addToCliqueFront(bayesTree, conditional, parent_clique); -// } else { -// if(debug) std::cout << "Starting new clique" << std::endl; -// // otherwise, start a new clique and add it to the tree -// bayesTree.addClique(conditional,parent_clique); -// } -// } -// -// /* ************************************************************************* */ -// void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { -// static const bool debug = false; -//#ifndef NDEBUG -// // Debug check to make sure the conditional variable is ordered lower than -// // its parents and that all of its parents are present either in this -// // clique or its separator. -// BOOST_FOREACH(Key parent, conditional->parents()) { -// assert(parent > conditional->lastFrontalKey()); -// const Clique& cliquer(*clique); -// assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); -// } -//#endif -// if(debug) conditional->print("Adding conditional "); -// if(debug) clique->print("To clique "); -// Index j = conditional->lastFrontalKey(); -// this->nodes_.resize(std::max(j+1, this->nodes_.size())); -// this->nodes_[j] = clique; -// FastVector newIndices(clique->conditional()->size() + 1); -// newIndices[0] = j; -// std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1); -// clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1); -// if(debug) clique->print("Expanded clique is "); -// clique->assertInvariants(); -// } - } diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.h b/gtsam/symbolic/SymbolicBayesTreeUnordered.h index 84bf43cc2..5d11240a5 100644 --- a/gtsam/symbolic/SymbolicBayesTreeUnordered.h +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.h @@ -18,14 +18,14 @@ #pragma once +#include +#include #include #include namespace gtsam { // Forward declarations - class SymbolicFactorGraphUnordered; - class SymbolicBayesNetUnordered; class SymbolicConditionalUnordered; /* ************************************************************************* */ @@ -55,6 +55,17 @@ namespace gtsam { typedef SymbolicBayesTreeUnordered This; typedef boost::shared_ptr shared_ptr; + /** Default constructor, creates an empty Bayes tree */ + SymbolicBayesTreeUnordered() {} + + /** Makes a deep copy of the tree structure, but only pointers to conditionals are + * copied, the conditionals and their matrices are not cloned. */ + SymbolicBayesTreeUnordered(const SymbolicBayesTreeUnordered& other); + + /** Makes a deep copy of the tree structure, but only pointers to conditionals are + * copied, the conditionals and their matrices are not cloned. */ + SymbolicBayesTreeUnordered& operator=(const SymbolicBayesTreeUnordered& other); + /** check equality */ bool equals(const This& other, double tol = 1e-9) const; }; diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.h b/gtsam/symbolic/SymbolicConditionalUnordered.h index 6cbd7a6dc..0915f7e82 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.h +++ b/gtsam/symbolic/SymbolicConditionalUnordered.h @@ -76,6 +76,8 @@ namespace gtsam { static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals) { return FromIterator(keys.begin(), keys.end(), nrFrontals); } + virtual ~SymbolicConditionalUnordered() {} + /// @} /// @name Testable diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp b/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp index 225e36b99..ad1c909dc 100644 --- a/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp @@ -21,8 +21,33 @@ namespace gtsam { - SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered() { + /* ************************************************************************* */ + SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered( + const SymbolicFactorGraphUnordered& factorGraph, const VariableIndexUnordered& structure, + const OrderingUnordered& order) : + Base(factorGraph, structure, order) {} + /* ************************************************************************* */ + SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered( + const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order) : + Base(factorGraph, order) {} + + /* ************************************************************************* */ + SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered( + const This& other) : + Base(other) {} + + /* ************************************************************************* */ + SymbolicEliminationTreeUnordered& SymbolicEliminationTreeUnordered::operator=(const This& other) + { + (void) Base::operator=(other); + return *this; + } + + /* ************************************************************************* */ + bool SymbolicEliminationTreeUnordered::equals(const This& other, double tol) const + { + return Base::equals(other, tol); } } diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h index 796609ffb..c1fc57195 100644 --- a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h @@ -25,7 +25,8 @@ namespace gtsam { class GTSAM_EXPORT SymbolicEliminationTreeUnordered : - public EliminationTreeUnordered { + public EliminationTreeUnordered + { public: typedef EliminationTreeUnordered Base; ///< Base class typedef SymbolicEliminationTreeUnordered This; ///< This class @@ -40,30 +41,29 @@ namespace gtsam { * @return The elimination tree */ SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, - const VariableIndexUnordered& structure, const OrderingUnordered& order) : - Base(factorGraph, structure, order) {} + const VariableIndexUnordered& structure, const OrderingUnordered& order); /** Build the elimination tree of a factor graph. Note that this has to compute the column * structure as a VariableIndex, so if you already have this precomputed, use the other * constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order) : - Base(factorGraph, order) {} + SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, + const OrderingUnordered& order); /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - SymbolicEliminationTreeUnordered(const This& other) : Base(other) {} + SymbolicEliminationTreeUnordered(const This& other); /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - This& operator=(const This& other) { (void) Base::operator=(other); return *this; } + This& operator=(const This& other); + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; private: - /// Private default constructor - SymbolicEliminationTreeUnordered(); - friend class ::EliminationTreeUnorderedTester; }; diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp index 9f6689713..9c2958e18 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp @@ -22,11 +22,19 @@ #include #include #include +#include +#include namespace gtsam { using namespace std; + /* ************************************************************************* */ + bool SymbolicFactorGraphUnordered::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } + /* ************************************************************************* */ void SymbolicFactorGraphUnordered::push_factor(Key key) { push_back(boost::make_shared(key)); diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h index 09fe99208..ec577581b 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -18,12 +18,10 @@ #pragma once -#include +#include #include #include -#include -// NOTE: Additional headers included at end of file for user convenience - +#include namespace gtsam { @@ -84,6 +82,14 @@ namespace gtsam { push_back_bayesTree(bayesTree); } /// @} + + /// @name Testable + /// @{ + + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface /// @{ @@ -106,9 +112,3 @@ namespace gtsam { }; } // namespace gtsam - -// These are not needed for this file but are returned from EliminateableFactorGraph functions so -// are included here for user convenience -#include -#include - diff --git a/gtsam/symbolic/SymbolicFactorUnordered.cpp b/gtsam/symbolic/SymbolicFactorUnordered.cpp index 02476aed4..6331c4609 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorUnordered.cpp @@ -59,6 +59,12 @@ namespace gtsam { SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end()))); } + /* ************************************************************************* */ + bool SymbolicFactorUnordered::equals(const This& other, double tol) const + { + return Base::equals(other, tol); + } + /* ************************************************************************* */ std::pair, boost::shared_ptr > SymbolicFactorUnordered::eliminate(const OrderingUnordered& keys) const diff --git a/gtsam/symbolic/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h index 9666e8e3e..1783d2309 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -28,11 +28,12 @@ namespace gtsam { // Forward declarations class SymbolicConditionalUnordered; + class OrderingUnordered; /** SymbolicFactorUnordered represents a symbolic factor that specifies graph topology but is not * associated with any numerical function. * \nosubgrouping */ - class GTSAM_EXPORT SymbolicFactorUnordered: public FactorUnordered { + class GTSAM_EXPORT SymbolicFactorUnordered : public FactorUnordered { public: @@ -73,6 +74,15 @@ namespace gtsam { SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + virtual ~SymbolicFactorUnordered() {} + + /// @} + + /// @name Testable + /// @{ + + bool equals(const This& other, double tol = 1e-9) const; + /// @} /// @name Advanced Constructors diff --git a/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp b/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp index c9a8e8f1e..5acee9b75 100644 --- a/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp +++ b/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp @@ -18,11 +18,24 @@ #include #include +#include namespace gtsam { - void SymbolicJunctionTreeUnordered::noop() const { + /* ************************************************************************* */ + SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered( + const SymbolicEliminationTreeUnordered& eliminationTree) : + Base(Base::FromEliminationTree(eliminationTree)) {} + /* ************************************************************************* */ + SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered(const This& other) : + Base(other) {} + + /* ************************************************************************* */ + SymbolicJunctionTreeUnordered& SymbolicJunctionTreeUnordered::operator=(const This& other) + { + (void) Base::operator=(other); + return *this; } } diff --git a/gtsam/symbolic/SymbolicJunctionTreeUnordered.h b/gtsam/symbolic/SymbolicJunctionTreeUnordered.h index 1049ad723..81c7b23d8 100644 --- a/gtsam/symbolic/SymbolicJunctionTreeUnordered.h +++ b/gtsam/symbolic/SymbolicJunctionTreeUnordered.h @@ -16,13 +16,35 @@ * @author Richard Roberts */ -#include #include -#include +#include #include namespace gtsam { + // Forward declarations + class SymbolicEliminationTreeUnordered; + + /** + * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with + * the additional property that it represents the clique tree associated with a Bayes net. + * + * In GTSAM a junction tree is an intermediate data structure in multifrontal + * variable elimination. Each node is a cluster of factors, along with a + * clique of variables that are eliminated all at once. In detail, every node k represents + * a clique (maximal fully connected subset) of an associated chordal graph, such as a + * chordal Bayes net resulting from elimination. + * + * The difference with the BayesTree is that a JunctionTree stores factors, whereas a + * BayesTree stores conditionals, that are the product of eliminating the factors in the + * corresponding JunctionTree cliques. + * + * The tree structure and elimination method are exactly analagous to the EliminationTree, + * except that in the JunctionTree, at each node multiple variables are eliminated at a time. + * + * \addtogroup Multifrontal + * \nosubgrouping + */ class GTSAM_EXPORT SymbolicJunctionTreeUnordered : public JunctionTreeUnordered { public: @@ -38,21 +60,15 @@ namespace gtsam { * named constructor instead. * @return The elimination tree */ - SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree) : - Base(Base::FromEliminationTree(eliminationTree)) {} + SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree); /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - SymbolicJunctionTreeUnordered(const This& other) : Base(other) {} + SymbolicJunctionTreeUnordered(const This& other); /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - This& operator=(const This& other) { (void) Base::operator=(other); return *this; } - - private: - - // Dummy method to export class type - void noop() const; + This& operator=(const This& other); }; }