12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small cleanups and fixes in included header files, functions in header file vs cpp/inst file, formatting, private/public, and inheritance.

release/4.3a0
Richard Roberts 2013-07-09 17:50:38 +00:00
parent ccb2d5c7fb
commit 9bdc5f1113
34 changed files with 278 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/inference/BayesNetUnordered-inst.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <boost/foreach.hpp>

11
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/BayesNetUnordered.h>
namespace gtsam {
template<class CLIQUE> class BayesTreeUnordered;
@ -41,8 +39,8 @@ namespace gtsam {
* @tparam CONDITIONAL The conditional type.
* \nosubgrouping */
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
struct BayesTreeCliqueBaseUnordered {
class BayesTreeCliqueBaseUnordered
{
private:
typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET> 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;

20
gtsam/inference/BayesTreeUnordered.h
View File

@ -39,12 +39,13 @@ namespace gtsam {
* \nosubgrouping
*/
template<class CLIQUE>
class BayesTreeUnordered {
public:
class BayesTreeUnordered
{
protected:
typedef BayesTreeUnordered<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
public:
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> 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 */

7
gtsam/inference/ConditionalUnordered-inst.h
View File

@ -38,4 +38,11 @@ namespace gtsam {
std::cout << ")" << std::endl;
}
/* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR>
bool ConditionalUnordered<FACTOR,DERIVEDFACTOR>::equals(const This& c, double tol = 1e-9) const
{
return nrFrontals_ == c.nrFrontals_;
}
}

6
gtsam/inference/ConditionalUnordered.h
View File

@ -52,6 +52,7 @@ namespace gtsam {
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> 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
/// @{

13
gtsam/inference/EliminateableFactorGraph.h
View File

@ -61,20 +61,23 @@ namespace gtsam {
typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
public:
/// Typedef to the specific EliminationTraits for this graph
typedef EliminationTraits<FactorGraphType> EliminationTraits;
/// Conditional type stored in the Bayes net produced by elimination
typedef typename EliminationTraits<FactorGraphType>::ConditionalType ConditionalType;
typedef typename EliminationTraits::ConditionalType ConditionalType;
/// Bayes net type produced by sequential elimination
typedef typename EliminationTraits<FactorGraphType>::BayesNetType BayesNetType;
typedef typename EliminationTraits::BayesNetType BayesNetType;
/// Elimination tree type that can do sequential elimination of this graph
typedef typename EliminationTraits<FactorGraphType>::EliminationTreeType EliminationTreeType;
typedef typename EliminationTraits::EliminationTreeType EliminationTreeType;
/// Bayes tree type produced by multifrontal elimination
typedef typename EliminationTraits<FactorGraphType>::BayesTreeType BayesTreeType;
typedef typename EliminationTraits::BayesTreeType BayesTreeType;
/// Junction tree type that can do multifrontal elimination of this graph
typedef typename EliminationTraits<FactorGraphType>::JunctionTreeType JunctionTreeType;
typedef typename EliminationTraits::JunctionTreeType JunctionTreeType;
/// The pair of conditional and remaining factor produced by a single dense elimination step on
/// a subgraph.

23
gtsam/inference/EliminationTreeUnordered.h
View File

@ -47,14 +47,15 @@ namespace gtsam {
* \nosubgrouping
*/
template<class BAYESNET, class GRAPH>
class EliminationTreeUnordered {
public:
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
class EliminationTreeUnordered
{
protected:
typedef EliminationTreeUnordered<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> 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<FactorType> 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<Node> sharedNode; ///< Shared pointer to Node
private:
protected:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode> roots_;
std::vector<sharedFactor> 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
/// @{

1
gtsam/inference/JunctionTreeUnordered-inst.h
View File

@ -24,6 +24,7 @@
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/JunctionTreeUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>

3
gtsam/inference/JunctionTreeUnordered.h
View File

@ -22,8 +22,6 @@
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
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
*/

16
gtsam/linear/GaussianBayesNetUnordered.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/linear/GaussianBayesNetUnordered.h>
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
@ -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<double,double>(log)).sum();
return logDet;
}
/* ************************************************************************* */

27
gtsam/linear/GaussianBayesNetUnordered.h
View File

@ -21,20 +21,21 @@
#pragma once
#include <gtsam/linear/GaussianConditionalUnordered.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/global_includes.h>
namespace gtsam {
/** A Bayes net made from linear-Gaussian densities */
class GTSAM_EXPORT GaussianBayesNetUnordered: public BayesNetUnordered<GaussianConditionalUnordered> {
class GTSAM_EXPORT GaussianBayesNetUnordered: public FactorGraphUnordered<GaussianConditionalUnordered> {
public:
typedef BayesNetUnordered<GaussianConditionalUnordered> Base;
typedef FactorGraphUnordered<GaussianConditionalUnordered> Base;
typedef GaussianBayesNetUnordered This;
typedef GaussianConditionalUnordered ConditionalType;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::shared_ptr<ConditionalType> 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

1
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/inference/BayesTreeUnordered-inst.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h>
#include <gtsam/linear/GaussianBayesTreeUnordered.h>
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/linear/GaussianBayesNetUnordered.h>
#include <gtsam/linear/VectorValuesUnordered.h>

19
gtsam/linear/GaussianBayesTreeUnordered.h
View File

@ -19,14 +19,14 @@
#pragma once
#include <gtsam/linear/GaussianBayesNetUnordered.h>
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
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<This> 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$.

22
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

@ -21,6 +21,10 @@
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/linear/VectorValuesUnordered.h>
#include <gtsam/linear/GaussianBayesTreeUnordered.h>
#include <gtsam/linear/GaussianEliminationTreeUnordered.h>
#include <gtsam/linear/GaussianJunctionTreeUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
@ -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<boost::tuple<size_t, size_t, double> > 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

6
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -195,6 +195,12 @@ namespace gtsam {
*/
//std::pair<Matrix,Vector> 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$,

7
gtsam/linear/GaussianFactorUnordered.h
View File

@ -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<Matrix,Vector> jacobian() const = 0;
virtual std::pair<Matrix,Vector> 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.

4
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -402,7 +402,7 @@ namespace gtsam {
}
/* ************************************************************************* */
pair<Matrix,Vector> JacobianFactorUnordered::matrix(bool weight) const {
pair<Matrix,Vector> 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());
}

34
gtsam/linear/JacobianFactorUnordered.h
View File

@ -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<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
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<Matrix, Vector> 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, Vector> 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.

2
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -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();
}

1
gtsam/linear/VectorValuesUnordered.h
View File

@ -97,6 +97,7 @@ namespace gtsam {
typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
typedef boost::shared_ptr<VectorValuesUnordered> shared_ptr; ///< shared_ptr to this class
typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef value_type KeyValuePair; ///< Typedef to pair<Key, Vector>, a key-value pair
/// @name Standard Constructors
/// @{

4
gtsam/linear/linearExceptions.h
View File

@ -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

5
gtsam/symbolic/SymbolicBayesNetUnordered.cpp
View File

@ -16,13 +16,14 @@
* @author Richard Roberts
*/
#include <gtsam/inference/BayesNetUnordered-inst.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
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);
}

11
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

@ -18,23 +18,26 @@
#pragma once
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/base/types.h>
namespace gtsam {
// Forward declarations
class SymbolicConditionalUnordered;
/** Symbolic Bayes Net
* \nosubgrouping
*/
class GTSAM_EXPORT SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
class GTSAM_EXPORT SymbolicBayesNetUnordered : public FactorGraphUnordered<SymbolicConditionalUnordered> {
public:
typedef BayesNetUnordered<SymbolicConditionalUnordered> Base;
typedef FactorGraphUnordered<SymbolicConditionalUnordered> Base;
typedef SymbolicBayesNetUnordered This;
typedef SymbolicConditionalUnordered ConditionalType;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
/// @name Standard Constructors
/// @{

86
gtsam/symbolic/SymbolicBayesTreeUnordered.cpp
View File

@ -21,87 +21,27 @@
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/BayesTreeUnordered-inst.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h>
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<SymbolicBayesTreeCliqueUnordered>(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<Index>::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<Index> 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();
// }
}

15
gtsam/symbolic/SymbolicBayesTreeUnordered.h
View File

@ -18,14 +18,14 @@
#pragma once
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
namespace gtsam {
// Forward declarations
class SymbolicFactorGraphUnordered;
class SymbolicBayesNetUnordered;
class SymbolicConditionalUnordered;
/* ************************************************************************* */
@ -55,6 +55,17 @@ namespace gtsam {
typedef SymbolicBayesTreeUnordered This;
typedef boost::shared_ptr<This> 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;
};

2
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

@ -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

27
gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp
View File

@ -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);
}
}

20
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

@ -25,7 +25,8 @@
namespace gtsam {
class GTSAM_EXPORT SymbolicEliminationTreeUnordered :
public EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> {
public EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered>
{
public:
typedef EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> 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;
};

8
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -22,11 +22,19 @@
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
#include <gtsam/symbolic/SymbolicJunctionTreeUnordered.h>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
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<SymbolicFactorUnordered>(key));

20
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -18,12 +18,10 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
// NOTE: Additional headers included at end of file for user convenience
#include <gtsam/base/types.h>
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 <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>

6
gtsam/symbolic/SymbolicFactorUnordered.cpp
View File

@ -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<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
SymbolicFactorUnordered::eliminate(const OrderingUnordered& keys) const

12
gtsam/symbolic/SymbolicFactorUnordered.h
View File

@ -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

15
gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp
View File

@ -18,11 +18,24 @@
#include <gtsam/inference/JunctionTreeUnordered-inst.h>
#include <gtsam/symbolic/SymbolicJunctionTreeUnordered.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
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;
}
}

38
gtsam/symbolic/SymbolicJunctionTreeUnordered.h
View File

@ -16,13 +16,35 @@
* @author Richard Roberts
*/
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/inference/JunctionTreeUnordered.h>
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<SymbolicBayesTreeUnordered, SymbolicFactorGraphUnordered> {
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);
};
}