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

Code cleanup: lean and mean VariableIndex (got rid of different types for different FG's, slots, and storage template), renamed Conditional.h and Factor.h to match class names ConditionalBase and FactorBase (not ideal names but prevents conflict with typedefs), added typedef for symbolic multifrontal solver.

release/4.3a0
Richard Roberts 2010-10-22 18:02:55 +00:00
parent 105a4a9b7f
commit f9e0ed07a3
44 changed files with 734 additions and 646 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
base/FastList.h Normal file
View File

@ -0,0 +1,55 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FastList.h
* @brief A thin wrapper around std::list that uses boost's fast_pool_allocator.
* @author Richard Roberts
* @created Oct 22, 2010
*/
#pragma once
#include <list>
#include <boost/pool/pool_alloc.hpp>
namespace gtsam {
/**
* FastSet is a thin wrapper around std::set that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
*/
template<typename VALUE>
class FastList: public std::list<VALUE, boost::fast_pool_allocator<VALUE> > {
public:
typedef std::list<VALUE, boost::fast_pool_allocator<VALUE> > Base;
/** Default constructor */
FastList() {}
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
FastList(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastList */
FastList(const FastList<VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */
FastList(const Base& x) : Base(x) {}
};
}

5
base/FastSet.h
View File

@ -37,11 +37,14 @@ public:
typedef std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > Base; typedef std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > Base;
/** Default constructor */
FastSet() {}
/** Constructor from a range, passes through to base class */ /** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR> template<typename INPUTITERATOR>
FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastMap */ /** Copy constructor from another FastSet */
FastSet(const FastSet<VALUE>& x) : Base(x) {} FastSet(const FastSet<VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */ /** Copy constructor from the base map class */

2
base/Makefile.am
View File

@ -27,7 +27,7 @@ sources += LieVector.cpp
check_PROGRAMS += tests/testLieVector tests/testLieScalar check_PROGRAMS += tests/testLieVector tests/testLieScalar
# Data structures # Data structures
headers += BTree.h DSF.h FastMap.h headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h
sources += DSFVector.cpp sources += DSFVector.cpp
check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector

10
inference/BayesTree-inl.h
View File

@ -19,6 +19,11 @@
#pragma once #pragma once
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
#include <iostream> #include <iostream>
#include <algorithm> #include <algorithm>
@ -32,11 +37,6 @@
using namespace boost::assign; using namespace boost::assign;
namespace lam = boost::lambda; namespace lam = boost::lambda;
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;

2
inference/Conditional.h → inference/ConditionalBase.h
View File

@ -26,7 +26,7 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorBase.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>
namespace gtsam { namespace gtsam {

9
inference/EliminationTree-inl.h
View File

@ -47,7 +47,7 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& structure) { vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables // Number of factors and variables
const size_t m = structure.nFactors(); const size_t m = structure.nFactors();
@ -62,8 +62,7 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& str
// for column j \in 1 to n do // for column j \in 1 to n do
for (Index j = 0; j < n; j++) { for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do // for row i \in Struct[A*j] do
BOOST_FOREACH(typename VariableIndex<>::mapped_factor_type i_pos, structure[j]) { BOOST_FOREACH(const size_t i, structure[j]) {
const size_t i = i_pos.factorIndex;
if (prevCol[i] != none) { if (prevCol[i] != none) {
Index k = prevCol[i]; Index k = prevCol[i];
// find root r of the current tree that contains k // find root r of the current tree that contains k
@ -83,7 +82,7 @@ vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& str
template<class FACTOR> template<class FACTOR>
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex<>& structure) { EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& structure) {
// Compute the tree structure // Compute the tree structure
vector<Index> parents(ComputeParents(structure)); vector<Index> parents(ComputeParents(structure));
@ -125,7 +124,7 @@ template<class FACTOR>
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) { EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
return Create(factorGraph, VariableIndex<>(factorGraph)); return Create(factorGraph, VariableIndex(factorGraph));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
inference/EliminationTree.h
View File

@ -55,7 +55,7 @@ private:
* Static internal function to build a vector of parent pointers using the * Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT. * algorithm of Gilbert et al., 2001, BIT.
*/ */
static std::vector<Index> ComputeParents(const VariableIndex<>& structure); static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** /**
* Recursive routine that eliminates the factors arranged in an elimination tree * Recursive routine that eliminates the factors arranged in an elimination tree
@ -72,7 +72,7 @@ public:
* pre-computed column structure. * pre-computed column structure.
*/ */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex<>& structure); static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph, const VariableIndex& structure);
/** Named constructor to build the elimination tree of a factor graph */ /** Named constructor to build the elimination tree of a factor graph */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>

2
inference/Factor-inl.h → inference/FactorBase-inl.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorBase.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>

9
inference/Factor.h → inference/FactorBase.h
View File

@ -99,15 +99,6 @@ public:
FactorBase(Key key1, Key key2, Key key3, Key key4) : keys_(4) { FactorBase(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/** Named constructor for combining a set of factors with pre-computed set of
* variables. (Old style - will be removed when scalar elimination is
* removed in favor of the EliminationTree). */
template<class DERIVED, class FACTORGRAPHTYPE, class VARIABLEINDEXSTORAGE>
static typename DERIVED::shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph,
const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const std::vector<size_t>& factors,
const std::vector<KEY>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); }
/** Create a combined joint factor (new style for EliminationTree). */ /** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVED> template<class DERIVED>
static typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots); static typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots);

32
inference/GenericMultifrontalSolver-inl.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GenericMultifrontalSolver-inl.h * @file GenericMultifrontalSolver-inl.h
* @brief * @brief
@ -8,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h> #include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/FactorBase-inl.h>
#include <gtsam/inference/JunctionTree-inl.h> #include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
@ -18,19 +29,22 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR, class JUNCTIONTREE>
GenericMultifrontalSolver<FACTOR>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) : GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
junctionTree_(new JunctionTree<FactorGraph<FACTOR> >(factorGraph)) {} junctionTree_(factorGraph) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR, class JUNCTIONTREE>
typename BayesTree<typename FACTOR::Conditional>::shared_ptr GenericMultifrontalSolver<FACTOR>::eliminate() const { typename JUNCTIONTREE::BayesTree::shared_ptr
return junctionTree_->eliminate(); GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree);
bayesTree->insert(junctionTree_.eliminate());
return bayesTree;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR>::marginal(Index j) const { typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginal(Index j) const {
return eliminate()->marginal(j); return eliminate()->marginal(j);
} }

19
inference/GenericMultifrontalSolver.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GenericMultifrontalSolver.h * @file GenericMultifrontalSolver.h
* @brief * @brief
@ -15,13 +26,13 @@
namespace gtsam { namespace gtsam {
template<class FACTOR> template<class FACTOR, class JUNCTIONTREE>
class GenericMultifrontalSolver { class GenericMultifrontalSolver {
protected: protected:
// Elimination tree that performs elimination. // Junction tree that performs elimination.
typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr junctionTree_; JUNCTIONTREE junctionTree_;
public: public:
@ -35,7 +46,7 @@ public:
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
typename BayesTree<typename FACTOR::Conditional>::shared_ptr eliminate() const; typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const;
/** /**
* Compute the marginal Gaussian density over a variable, by integrating out * Compute the marginal Gaussian density over a variable, by integrating out

13
inference/GenericSequentialSolver-inl.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GenericSequentialSolver.cpp * @file GenericSequentialSolver.cpp
* @brief * @brief
@ -8,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/inference/GenericSequentialSolver.h> #include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/FactorBase-inl.h>
#include <gtsam/inference/EliminationTree-inl.h> #include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>

13
inference/GenericSequentialSolver.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GenericSequentialSolver.h * @file GenericSequentialSolver.h
* @brief * @brief
@ -24,7 +35,7 @@ protected:
FactorGraph<FACTOR> factors_; FactorGraph<FACTOR> factors_;
// Column structure of the factor graph // Column structure of the factor graph
VariableIndex<> structure_; VariableIndex structure_;
// Elimination tree that performs elimination. // Elimination tree that performs elimination.
typename EliminationTree<FACTOR>::shared_ptr eliminationTree_; typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;

2
inference/ISAM-inl.h
View File

@ -21,7 +21,7 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/ConditionalBase.h>
#include <gtsam/inference/ISAM.h> #include <gtsam/inference/ISAM.h>
#include <gtsam/inference/BayesTree-inl.h> #include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h> #include <gtsam/inference/GenericSequentialSolver-inl.h>

2
inference/IndexConditional.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/ConditionalBase.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>
namespace gtsam { namespace gtsam {

2
inference/IndexFactor.cpp
View File

@ -16,7 +16,7 @@
* @created Oct 17, 2010 * @created Oct 17, 2010
*/ */
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/FactorBase-inl.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>
namespace gtsam { namespace gtsam {

8
inference/IndexFactor.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/FactorBase.h>
namespace gtsam { namespace gtsam {
@ -63,12 +63,6 @@ public:
static shared_ptr static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots); Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
template<class FACTORGRAPHTYPE, class VARIABLEINDEXSTORAGE>
static shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph,
const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return Base::Combine<This>(factorGraph, variableIndex, factors, variables, variablePositions); }
/** /**
* eliminate the first variable involved in this factor * eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable * @return a conditional on the eliminated variable

8
inference/Makefile.am
View File

@ -15,19 +15,19 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# GTSAM core # GTSAM core
headers += Factor.h Factor-inl.h Conditional.h headers += FactorBase.h FactorBase-inl.h ConditionalBase.h
# Symbolic Inference # Symbolic Inference
sources += SymbolicFactorGraph.cpp SymbolicSequentialSolver.cpp sources += SymbolicFactorGraph.cpp SymbolicMultifrontalSolver.cpp SymbolicSequentialSolver.cpp
check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
# Inference # Inference
headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-inl.h
headers += inference-inl.h VariableSlots-inl.h headers += inference-inl.h VariableSlots-inl.h
sources += inference.cpp VariableSlots.cpp Permutation.cpp sources += inference.cpp VariableSlots.cpp Permutation.cpp VariableIndex.cpp
sources += IndexFactor.cpp IndexConditional.cpp sources += IndexFactor.cpp IndexConditional.cpp
headers += graph.h graph-inl.h headers += graph.h graph-inl.h
headers += VariableIndex.h
headers += FactorGraph.h FactorGraph-inl.h headers += FactorGraph.h FactorGraph-inl.h
headers += ClusterTree.h ClusterTree-inl.h headers += ClusterTree.h ClusterTree-inl.h
headers += JunctionTree.h JunctionTree-inl.h headers += JunctionTree.h JunctionTree-inl.h

1
inference/SymbolicFactorGraph.h
View File

@ -36,7 +36,6 @@ typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
class SymbolicFactorGraph: public FactorGraph<IndexFactor> { class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public: public:
typedef SymbolicBayesNet bayesnet_type; typedef SymbolicBayesNet bayesnet_type;
typedef VariableIndex<> variableindex_type;
/** Construct empty factor graph */ /** Construct empty factor graph */
SymbolicFactorGraph() {} SymbolicFactorGraph() {}

26
inference/SymbolicMultifrontalSolver.cpp Normal file
View File

@ -0,0 +1,26 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicMultifrontalSolver.cpp
* @brief
* @author Richard Roberts
* @created Oct 22, 2010
*/
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
#include <gtsam/inference/JunctionTree-inl.h>
namespace gtsam {
template class GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > >;
}

30
inference/SymbolicMultifrontalSolver.h Normal file
View File

@ -0,0 +1,30 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicMultifrontalSolver.h
* @brief
* @author Richard Roberts
* @created Oct 22, 2010
*/
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {
// The base class provides all of the needed functionality
typedef GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > > SymbolicMultifrontalSolver;
}

39
inference/SymbolicSequentialSolver.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file SymbolicSequentialSolver.cpp * @file SymbolicSequentialSolver.cpp
* @brief * @brief
@ -10,19 +21,21 @@
namespace gtsam { namespace gtsam {
// An explicit instantiation to be compiled into the library
template class GenericSequentialSolver<IndexFactor>;
/* ************************************************************************* */ ///* ************************************************************************* */
SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) : //SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) :
Base(factorGraph) {} // Base(factorGraph) {}
//
/* ************************************************************************* */ ///* ************************************************************************* */
BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const { //BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const {
return Base::eliminate(); // return Base::eliminate();
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const { //SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const {
return SymbolicFactorGraph::shared_ptr(new SymbolicFactorGraph(*Base::joint(js))); // return Base::joint(js);
} //}
} }

75
inference/SymbolicSequentialSolver.h
View File

@ -1,10 +1,20 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file SymbolicSequentialSolver.h * @file SymbolicSequentialSolver.h
* @brief * @brief
* @author Richard Roberts * @author Richard Roberts
* @created Oct 21, 2010 * @created Oct 21, 2010
*/ */
#pragma once #pragma once
#include <gtsam/inference/GenericSequentialSolver.h> #include <gtsam/inference/GenericSequentialSolver.h>
@ -12,37 +22,40 @@
namespace gtsam { namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> { // The base class provides all of the needed functionality
typedef GenericSequentialSolver<IndexFactor> SymbolicSequentialSolver;
protected: //class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
//
typedef GenericSequentialSolver<IndexFactor> Base; //protected:
//
public: // typedef GenericSequentialSolver<IndexFactor> Base;
//
SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph); //public:
//
/** // SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph);
* Eliminate the factor graph sequentially. Uses a column elimination tree //
* to recursively eliminate. // /**
*/ // * Eliminate the factor graph sequentially. Uses a column elimination tree
BayesNet<IndexConditional>::shared_ptr eliminate() const; // * to recursively eliminate.
// */
/** // BayesNet<IndexConditional>::shared_ptr eliminate() const;
* Compute the marginal Gaussian density over a variable, by integrating out //
* all of the other variables. This function returns the result as a factor. // /**
*/ // * Compute the marginal Gaussian density over a variable, by integrating out
IndexFactor::shared_ptr marginal(Index j) const; // * all of the other variables. This function returns the result as a factor.
// */
/** // IndexFactor::shared_ptr marginal(Index j) const;
* Compute the marginal joint over a set of variables, by integrating out //
* all of the other variables. This function returns the result as an upper- // /**
* triangular R factor and right-hand-side, i.e. a GaussianBayesNet with // * Compute the marginal joint over a set of variables, by integrating out
* R*x = d. To get a mean and covariance matrix, use jointStandard(...) // * all of the other variables. This function returns the result as an upper-
*/ // * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with
SymbolicFactorGraph::shared_ptr joint(const std::vector<Index>& js) const; // * R*x = d. To get a mean and covariance matrix, use jointStandard(...)
// */
}; // SymbolicFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
//
//};
} }

76
inference/VariableIndex.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VariableIndex.cpp
* @brief
* @author Richard Roberts
* @created Oct 22, 2010
*/
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
/* ************************************************************************* */
void VariableIndex::permute(const Permutation& permutation) {
#ifndef NDEBUG
// Assert that the permutation does not leave behind any non-empty variables,
// otherwise the nFactors and nEntries counts would be incorrect.
for(Index j=0; j<this->index_.size(); ++j)
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
assert(this->operator[](j).empty());
#endif
index_.permute(permutation);
}
/* ************************************************************************* */
void VariableIndex::rebaseFactors(ptrdiff_t baseIndexChange) {
BOOST_FOREACH(Factors& factors, index_.container()) {
BOOST_FOREACH(size_t& factor, factors) {
factor += baseIndexChange;
}
}
}
/* ************************************************************************* */
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var)
if(var >= this->index_.size() || var >= other.index_.size()) {
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
(var >= other.index_.size() && this->index_[var].empty())))
return false;
} else if(this->index_[var] != other.index_[var])
return false;
} else
return false;
return true;
}
/* ************************************************************************* */
void VariableIndex::print(const std::string& str) const {
std::cout << str;
Index var = 0;
BOOST_FOREACH(const Factors& variable, index_.container()) {
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
assert(rvar != index_.permutation().end());
std::cout << "var " << (rvar-index_.permutation().begin()) << ":";
BOOST_FOREACH(const size_t factor, variable) {
std::cout << " " << factor;
}
std::cout << "\n";
++ var;
}
std::cout << std::flush;
}
}

134
inference/VariableIndex.h
View File

@ -19,10 +19,10 @@
#pragma once #pragma once
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>
#include <vector> #include <vector>
#include <deque>
#include <iostream> #include <iostream>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -31,43 +31,23 @@ namespace gtsam {
class Inference; class Inference;
/**
* The VariableIndex can internally use either a vector or a deque to store
* variables. For one-shot elimination, using a vector will give the best
* performance, and this is the default. For incremental updates, such as in
* ISAM and ISAM2, deque storage is used to prevent frequent reallocation.
*/
struct VariableIndexStorage_vector { template<typename T> struct type_factory { typedef std::vector<T> type; }; };
struct VariableIndexStorage_deque { template<typename T> struct type_factory { typedef std::deque<T> type; }; };
/** /**
* The VariableIndex class stores the information necessary to perform * The VariableIndex class stores the information necessary to perform
* elimination, including an index of factors involving each variable and * elimination, including an index of factors involving each variable and
* the structure of the intermediate joint factors that develop during * the structure of the intermediate joint factors that develop during
* elimination. This maps variables to deque's of pair<size_t,size_t>, * elimination. This maps variables to collections of factor indices.
* which is pairs the factor index with the position of the variable within
* the factor.
*/ */
struct _mapped_factor_type {
size_t factorIndex;
size_t variablePosition;
_mapped_factor_type(size_t _factorIndex, size_t _variablePosition) : factorIndex(_factorIndex), variablePosition(_variablePosition) {}
bool operator==(const _mapped_factor_type& o) const { return factorIndex == o.factorIndex && variablePosition == o.variablePosition; }
};
template<class VARIABLEINDEXSTORAGE=VariableIndexStorage_vector>
class VariableIndex { class VariableIndex {
public: public:
typedef _mapped_factor_type mapped_factor_type;
typedef boost::shared_ptr<VariableIndex> shared_ptr; typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef std::deque<mapped_factor_type> mapped_type; typedef FastList<size_t> Factors;
typedef typename mapped_type::iterator factor_iterator; typedef typename Factors::iterator Factor_iterator;
typedef typename mapped_type::const_iterator const_factor_iterator; typedef typename Factors::const_iterator Factor_const_iterator;
protected: protected:
typedef typename VARIABLEINDEXSTORAGE::template type_factory<mapped_type>::type storage_type; std::vector<Factors> indexUnpermuted_;
storage_type indexUnpermuted_; Permuted<std::vector<Factors>, Factors&> index_;
Permuted<storage_type, typename storage_type::value_type&> index_;
size_t nFactors_; size_t nFactors_;
size_t nEntries_; // Sum of involved variable counts of each factor size_t nEntries_; // Sum of involved variable counts of each factor
@ -79,13 +59,13 @@ public:
Index size() const { return index_.size(); } Index size() const { return index_.size(); }
size_t nFactors() const { return nFactors_; } size_t nFactors() const { return nFactors_; }
size_t nEntries() const { return nEntries_; } size_t nEntries() const { return nEntries_; }
const mapped_type& operator[](Index variable) const { checkVar(variable); return index_[variable]; } const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
mapped_type& operator[](Index variable) { checkVar(variable); return index_[variable]; } Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
void permute(const Permutation& permutation); void permute(const Permutation& permutation);
template<class FactorGraph> void augment(const FactorGraph& factorGraph); template<class FactorGraph> void augment(const FactorGraph& factorGraph);
void rebaseFactors(ptrdiff_t baseIndexChange); void rebaseFactors(ptrdiff_t baseIndexChange);
template<class Derived> bool equals(const Derived& other, double tol=0.0) const; bool equals(const VariableIndex& other, double tol=0.0) const;
void print(const std::string& str = "VariableIndex: ") const; void print(const std::string& str = "VariableIndex: ") const;
protected: protected:
@ -94,56 +74,32 @@ protected:
template<class FactorGraph> void fill(const FactorGraph& factorGraph); template<class FactorGraph> void fill(const FactorGraph& factorGraph);
factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
const_factor_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
const_factor_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
friend class Inference;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage>
void VariableIndex<Storage>::permute(const Permutation& permutation) {
#ifndef NDEBUG
// Assert that the permutation does not leave behind any non-empty variables,
// otherwise the nFactors and nEntries counts would be incorrect.
for(Index j=0; j<this->index_.size(); ++j)
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
assert(this->operator[](j).empty());
#endif
index_.permute(permutation);
// storage_type original(this->index_.size());
// this->index_.swap(original);
// for(Index j=0; j<permutation.size(); ++j)
// this->index_[j].swap(original[permutation[j]]);
}
/* ************************************************************************* */
template<class Storage>
template<class FactorGraph> template<class FactorGraph>
void VariableIndex<Storage>::fill(const FactorGraph& factorGraph) { void VariableIndex::fill(const FactorGraph& factorGraph) {
// Build index mapping from variable id to factor index // Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi) for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) { if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) { BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) { if(key < index_.size()) {
index_[key].push_back(mapped_factor_type(fi, fvari)); index_[key].push_back(fi);
++ fvari;
++ nEntries_; ++ nEntries_;
} }
} }
++ nFactors_; ++ nFactors_;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage>
template<class FactorGraph> template<class FactorGraph>
VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) : VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
// If the factor graph is empty, return an empty index because inside this // If the factor graph is empty, return an empty index because inside this
@ -166,21 +122,18 @@ VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
fill(factorGraph); fill(factorGraph);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage>
template<class FactorGraph> template<class FactorGraph>
VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
fill(factorGraph); fill(factorGraph);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage>
template<class FactorGraph> template<class FactorGraph>
void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) { void VariableIndex::augment(const FactorGraph& factorGraph) {
// If the factor graph is empty, return an empty index because inside this // If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor. // if block we assume at least one factor.
if(factorGraph.size() > 0) { if(factorGraph.size() > 0) {
@ -206,10 +159,8 @@ void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) {
size_t orignFactors = nFactors_; size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factorGraph.size(); ++fi) for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) { if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) { BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
index_[key].push_back(mapped_factor_type(orignFactors + fi, fvari)); index_[key].push_back(orignFactors + fi);
++ fvari;
++ nEntries_; ++ nEntries_;
} }
++ nFactors_; ++ nFactors_;
@ -217,49 +168,4 @@ void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) {
} }
} }
/* ************************************************************************* */
template<class Storage>
void VariableIndex<Storage>::rebaseFactors(ptrdiff_t baseIndexChange) {
BOOST_FOREACH(mapped_type& factors, index_.container()) {
BOOST_FOREACH(mapped_factor_type& factor, factors) {
factor.factorIndex += baseIndexChange;
}
}
}
/* ************************************************************************* */
template<class Storage>
template<class Derived>
bool VariableIndex<Storage>::equals(const Derived& other, double tol) const {
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var)
if(var >= this->index_.size() || var >= other.index_.size()) {
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
(var >= other.index_.size() && this->index_[var].empty())))
return false;
} else if(this->index_[var] != other.index_[var])
return false;
} else
return false;
return true;
}
/* ************************************************************************* */
template<class Storage>
void VariableIndex<Storage>::print(const std::string& str) const {
std::cout << str;
Index var = 0;
BOOST_FOREACH(const mapped_type& variable, index_.container()) {
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
assert(rvar != index_.permutation().end());
std::cout << "var " << (rvar-index_.permutation().begin()) << ":";
BOOST_FOREACH(const mapped_factor_type& factor, variable) {
std::cout << " " << factor.factorIndex << "-" << factor.variablePosition;
}
std::cout << "\n";
++ var;
}
std::cout << std::flush;
}
} }

12
inference/inference-inl.h
View File

@ -33,7 +33,7 @@
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <gtsam/inference/FactorGraph-inl.h> #include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/ConditionalBase.h>
using namespace std; using namespace std;
@ -54,13 +54,13 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& va
p[0] = 0; p[0] = 0;
int count = 0; int count = 0;
for(Index var = 0; var < variableIndex.size(); ++var) { for(Index var = 0; var < variableIndex.size(); ++var) {
const typename VARIABLEINDEXTYPE::mapped_type& column(variableIndex[var]); const typename VARIABLEINDEXTYPE::Factors& column(variableIndex[var]);
size_t lastFactorId = numeric_limits<size_t>::max(); size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(const typename VARIABLEINDEXTYPE::mapped_factor_type& factor_pos, column) { BOOST_FOREACH(const size_t& factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max()) if(lastFactorId != numeric_limits<size_t>::max())
assert(factor_pos.factorIndex > lastFactorId); assert(factorIndex > lastFactorId);
A[count++] = factor_pos.factorIndex; // copy sparse column A[count++] = factorIndex; // copy sparse column
if(debug) cout << "A[" << count-1 << "] = " << factor_pos.factorIndex << endl; if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl;
} }
p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
cmember[var] = 0; cmember[var] = 0;

4
inference/tests/testVariableIndex.cpp
View File

@ -39,8 +39,8 @@ TEST(VariableIndex, augment) {
SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
VariableIndex<> expected(fgCombined); VariableIndex expected(fgCombined);
VariableIndex<> actual(fg1); VariableIndex actual(fg1);
actual.augment(fg2); actual.augment(fg2);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));

380
linear/GaussianFactor.cpp
View File

@ -556,196 +556,196 @@ struct _RowSource {
bool operator<(const _RowSource& o) const { return firstNonzeroVar < o.firstNonzeroVar; } bool operator<(const _RowSource& o) const { return firstNonzeroVar < o.firstNonzeroVar; }
}; };
/* Explicit instantiations for storage types */ ///* Explicit instantiations for storage types */
template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); //template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); //template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
//
// Utility function to determine row count and check if any noise models are constrained //// Utility function to determine row count and check if any noise models are constrained
// TODO: would be nicer if this function was split and are factorgraph methods //// TODO: would be nicer if this function was split and are factorgraph methods
static std::pair<size_t,bool> rowCount(const FactorGraph<GaussianFactor>& factorGraph, //static std::pair<size_t,bool> rowCount(const FactorGraph<GaussianFactor>& factorGraph,
const vector<size_t>& factorIndices) // const vector<size_t>& factorIndices)
{ //{
static const bool debug = false; // static const bool debug = false;
tic("Combine: count sizes"); // tic("Combine: count sizes");
size_t m = 0; // size_t m = 0;
bool constrained = false; // bool constrained = false;
BOOST_FOREACH(const size_t i, factorIndices) // BOOST_FOREACH(const size_t i, factorIndices)
{ // {
assert(factorGraph[i] != NULL); // assert(factorGraph[i] != NULL);
assert(!factorGraph[i]->keys().empty()); // assert(!factorGraph[i]->keys().empty());
m += factorGraph[i]->numberOfRows(); // m += factorGraph[i]->numberOfRows();
if (debug) cout << "Combining factor " << i << endl; // if (debug) cout << "Combining factor " << i << endl;
if (debug) factorGraph[i]->print(" :"); // if (debug) factorGraph[i]->print(" :");
if (!constrained && factorGraph[i]->isConstrained()) { // if (!constrained && factorGraph[i]->isConstrained()) {
constrained = true; // constrained = true;
if (debug) std::cout << "Found a constraint!" << std::endl; // if (debug) std::cout << "Found a constraint!" << std::endl;
} // }
} // }
toc("Combine: count sizes"); // toc("Combine: count sizes");
return make_pair(m,constrained); // return make_pair(m,constrained);
} //}
//
// Determine row and column counts and check if any noise models are constrained //// Determine row and column counts and check if any noise models are constrained
template<class STORAGE> //template<class STORAGE>
static vector<size_t> columnDimensions( //static vector<size_t> columnDimensions(
const GaussianVariableIndex<STORAGE>& variableIndex, // const GaussianVariableIndex<STORAGE>& variableIndex,
const vector<Index>& variables) // const vector<Index>& variables)
{ //{
tic("Combine: count dims"); // tic("Combine: count dims");
static const bool debug = false; // static const bool debug = false;
vector<size_t> dims(variables.size() + 1); // vector<size_t> dims(variables.size() + 1);
size_t n = 0; // size_t n = 0;
{ // {
size_t j = 0; // size_t j = 0;
BOOST_FOREACH(const Index& var, variables) // BOOST_FOREACH(const Index& var, variables)
{ // {
if (debug) cout << "Have variable " << var << endl; // if (debug) cout << "Have variable " << var << endl;
dims[j] = variableIndex.dim(var); // dims[j] = variableIndex.dim(var);
n += dims[j]; // n += dims[j];
++j; // ++j;
} // }
dims[j] = 1; // dims[j] = 1;
} // }
toc("Combine: count dims"); // toc("Combine: count dims");
return dims; // return dims;
} //}
//
// To do this, we merge-sort the rows so that the column indices of the first structural //// To do this, we merge-sort the rows so that the column indices of the first structural
// non-zero in each row increase monotonically. //// non-zero in each row increase monotonically.
vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorIndices, //vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorIndices,
const FactorGraph<GaussianFactor>& factorGraph) { // const FactorGraph<GaussianFactor>& factorGraph) {
vector<_RowSource> rowSources; // vector<_RowSource> rowSources;
rowSources.reserve(m); // rowSources.reserve(m);
size_t i1 = 0; // size_t i1 = 0;
BOOST_FOREACH(const size_t i2, factorIndices) { // BOOST_FOREACH(const size_t i2, factorIndices) {
const GaussianFactor& factor(*factorGraph[i2]); // const GaussianFactor& factor(*factorGraph[i2]);
size_t factorRowI = 0; // size_t factorRowI = 0;
assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows()); // assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows());
BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) { // BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) {
Index firstNonzeroVar; // Index firstNonzeroVar;
firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos]; // firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos];
rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI)); // rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI));
++ factorRowI; // ++ factorRowI;
} // }
assert(factorRowI == factor.numberOfRows()); // assert(factorRowI == factor.numberOfRows());
++ i1; // ++ i1;
} // }
assert(rowSources.size() == m); // assert(rowSources.size() == m);
assert(i1 == factorIndices.size()); // assert(i1 == factorIndices.size());
sort(rowSources.begin(), rowSources.end()); // sort(rowSources.begin(), rowSources.end());
return rowSources; // return rowSources;
} //}
//
void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row, //void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row,
const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions, // const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions,
const size_t factorRow, const size_t factorI, const vector<Index>& variables) { // const size_t factorRow, const size_t factorI, const vector<Index>& variables) {
const static bool debug = false; // const static bool debug = false;
const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow]; // const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow];
std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos; // std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos;
std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos; // std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
combinedFactor->set_firstNonzeroBlocks(row, *varposIt); // combinedFactor->set_firstNonzeroBlocks(row, *varposIt);
if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl; // if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
std::vector<Index>::const_iterator keyitend = factor.keys().end(); // std::vector<Index>::const_iterator keyitend = factor.keys().end();
while(keyit != keyitend) { // while(keyit != keyitend) {
const size_t varpos = *varposIt; // const size_t varpos = *varposIt;
assert(variables[varpos] == *keyit); // assert(variables[varpos] == *keyit);
GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos)); // GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos));
const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit)); // const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit));
ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow); // ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
++ keyit; // ++ keyit;
++ varposIt; // ++ varposIt;
} // }
} //}
//
template<class STORAGE> //template<class STORAGE>
GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph, //GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices, // const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) { // const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
//
// Debugging flags // // Debugging flags
static const bool verbose = false; // static const bool verbose = false;
static const bool debug = false; // static const bool debug = false;
if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl; // if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl;
assert(factorIndices.size() == variablePositions.size()); // assert(factorIndices.size() == variablePositions.size());
//
// Determine row count and check if any noise models are constrained // // Determine row count and check if any noise models are constrained
size_t m; bool constrained; // size_t m; bool constrained;
boost::tie(m,constrained) = rowCount(factorGraph,factorIndices); // boost::tie(m,constrained) = rowCount(factorGraph,factorIndices);
//
// Determine column dimensions // // Determine column dimensions
vector<size_t> dims = columnDimensions(variableIndex,variables); // vector<size_t> dims = columnDimensions(variableIndex,variables);
//
// Allocate return value, the combined factor, the augmented Ab matrix and other arrays // // Allocate return value, the combined factor, the augmented Ab matrix and other arrays
tic("Combine: set up empty"); // tic("Combine: set up empty");
shared_ptr combinedFactor(boost::make_shared<GaussianFactor>()); // shared_ptr combinedFactor(boost::make_shared<GaussianFactor>());
combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims.begin(), dims.end(), m)); // combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims.begin(), dims.end(), m));
ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix<double>(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); // ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix<double>(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2());
combinedFactor->firstNonzeroBlocks_.resize(m); // combinedFactor->firstNonzeroBlocks_.resize(m);
Vector sigmas(m); // Vector sigmas(m);
//
// Copy keys // // Copy keys
combinedFactor->keys_.reserve(variables.size()); // combinedFactor->keys_.reserve(variables.size());
combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end()); // combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end());
toc("Combine: set up empty"); // toc("Combine: set up empty");
//
// Compute a row permutation that maintains a staircase pattern in the new combined factor. // // Compute a row permutation that maintains a staircase pattern in the new combined factor.
tic("Combine: sort rows"); // tic("Combine: sort rows");
vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph); // vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph);
toc("Combine: sort rows"); // toc("Combine: sort rows");
//
// Fill in the rows of the new factor in sorted order. Fill in the array of // // Fill in the rows of the new factor in sorted order. Fill in the array of
// the left-most nonzero for each row and the first structural zero in each column. // // the left-most nonzero for each row and the first structural zero in each column.
// todo SL: smarter ignoring of zero factor variables (store first possible like above) // // todo SL: smarter ignoring of zero factor variables (store first possible like above)
//
if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: "); // if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: ");
//
tic("Combine: copy rows"); // tic("Combine: copy rows");
#ifndef NDEBUG //#ifndef NDEBUG
size_t lastRowFirstVarpos = 0; // size_t lastRowFirstVarpos = 0;
#endif //#endif
for(size_t row=0; row<m; ++row) { // for(size_t row=0; row<m; ++row) {
//
const _RowSource& rowSource = rowSources[row]; // const _RowSource& rowSource = rowSources[row];
assert(rowSource.factorI < factorGraph.size()); // assert(rowSource.factorI < factorGraph.size());
const size_t factorI = rowSource.factorI; // const size_t factorI = rowSource.factorI;
const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]); // const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]);
const size_t factorRow = rowSource.factorRowI; // const size_t factorRow = rowSource.factorRowI;
//
if(debug) // if(debug)
cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl; // cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl;
//
// Copy rhs b and sigma // // Copy rhs b and sigma
combinedFactor->getb()(row) = factor.getb()(factorRow); // combinedFactor->getb()(row) = factor.getb()(factorRow);
sigmas(row) = factor.get_sigmas()(factorRow); // sigmas(row) = factor.get_sigmas()(factorRow);
//
// Copy the row of A variable by variable, starting at the first nonzero variable. // // Copy the row of A variable by variable, starting at the first nonzero variable.
copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables); // copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables);
//
#ifndef NDEBUG //#ifndef NDEBUG
// Debug check, make sure the first column of nonzeros increases monotonically // // Debug check, make sure the first column of nonzeros increases monotonically
if(row != 0) // if(row != 0)
assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos); // assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos);
lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row]; // lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row];
#endif //#endif
} // }
toc("Combine: copy rows"); // toc("Combine: copy rows");
//
if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl; // if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl;
//
if (constrained) { // if (constrained) {
combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas); // combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas);
if (verbose) combinedFactor->model_->print("Just created Constraint ^"); // if (verbose) combinedFactor->model_->print("Just created Constraint ^");
} else { // } else {
combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas); // combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas);
if (verbose) combinedFactor->model_->print("Just created Diagonal"); // if (verbose) combinedFactor->model_->print("Just created Diagonal");
} // }
//
if(debug) combinedFactor->print("Combined factor: "); // if(debug) combinedFactor->print("Combined factor: ");
//
combinedFactor->assertInvariants(); // combinedFactor->assertInvariants();
//
return combinedFactor; // return combinedFactor;
} //}
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for Combine // Helper functions for Combine

15
linear/GaussianFactor.h
View File

@ -48,7 +48,6 @@
namespace gtsam { namespace gtsam {
class GaussianFactorGraph; class GaussianFactorGraph;
template<class VARIABLEINDEXSTORAGE=VariableIndexStorage_vector> class GaussianVariableIndex;
/** A map from key to dimension, useful in various contexts */ /** A map from key to dimension, useful in various contexts */
typedef std::map<Index, size_t> Dimensions; typedef std::map<Index, size_t> Dimensions;
@ -146,13 +145,13 @@ public:
*/ */
void permuteWithInverse(const Permutation& inversePermutation); void permuteWithInverse(const Permutation& inversePermutation);
/** // /**
* Named constructor for combining a set of factors with pre-computed set of variables. // * Named constructor for combining a set of factors with pre-computed set of variables.
*/ // */
template<class STORAGE> // template<class STORAGE>
static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph, // static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<STORAGE>& variableIndex, const std::vector<size_t>& factorIndices, // const GaussianVariableIndex<STORAGE>& variableIndex, const std::vector<size_t>& factorIndices,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions); // const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions);
/** /**
* Named constructor for combining a set of factors with pre-computed set of variables. * Named constructor for combining a set of factors with pre-computed set of variables.

12
linear/GaussianFactorGraph.cpp
View File

@ -146,25 +146,21 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const { GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<size_t>& dimensions) const {
// start with this factor graph // start with this factor graph
GaussianFactorGraph result = *this; GaussianFactorGraph result = *this;
// for each of the variables, add a prior // for each of the variables, add a prior
for(Index var=0; var<variableIndex.size(); ++var) { for(Index j=0; j<dimensions.size(); ++j) {
size_t dim = variableIndex.dim(var); size_t dim = dimensions[j];
Matrix A = eye(dim); Matrix A = eye(dim);
Vector b = zero(dim); Vector b = zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
sharedFactor prior(new GaussianFactor(var,A,b, model)); sharedFactor prior(new GaussianFactor(j, A, b, model));
result.push_back(prior); result.push_back(prior);
} }
return result; return result;
} }
GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
return add_priors(sigma, GaussianVariableIndex<>(*this)) ;
}
} // namespace gtsam } // namespace gtsam

117
linear/GaussianFactorGraph.h
View File

@ -39,7 +39,6 @@ namespace gtsam {
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr; typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr;
typedef GaussianBayesNet bayesnet_type; typedef GaussianBayesNet bayesnet_type;
typedef GaussianVariableIndex<> variableindex_type;
/** /**
* Default constructor * Default constructor
@ -152,122 +151,8 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable * Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian * @param sigma Standard deviation of Gaussian
*/ */
GaussianFactorGraph add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const; GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& dimensions) const;
GaussianFactorGraph add_priors(double sigma) const;
}; };
/* ************************************************************************* */
template<class VARIABLEINDEXSTORAGE>
class GaussianVariableIndex : public VariableIndex<VARIABLEINDEXSTORAGE> {
public:
typedef VariableIndex<VARIABLEINDEXSTORAGE> Base;
typedef typename VARIABLEINDEXSTORAGE::template type_factory<size_t>::type storage_type;
storage_type dims_;
public:
typedef boost::shared_ptr<GaussianVariableIndex> shared_ptr;
/** Construct an empty GaussianVariableIndex */
GaussianVariableIndex() {}
/**
* Constructor from a GaussianFactorGraph, lets the base class build the
* column-wise index then fills the dims_ array.
*/
GaussianVariableIndex(const GaussianFactorGraph& factorGraph);
/**
* Constructor to "upgrade" from the base class without recomputing the
* column index, i.e. just fills the dims_ array.
*/
GaussianVariableIndex(const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const GaussianFactorGraph& factorGraph);
/**
* Another constructor to upgrade from the base class using an existing
* array of variable dimensions.
*/
GaussianVariableIndex(const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const storage_type& dimensions);
const storage_type& dims() const { return dims_; }
size_t dim(Index variable) const { Base::checkVar(variable); return dims_[variable]; }
/** Permute */
void permute(const Permutation& permutation);
/** Augment this variable index with the contents of another one */
void augment(const GaussianFactorGraph& factorGraph);
protected:
GaussianVariableIndex(size_t nVars) : Base(nVars), dims_(nVars) {}
void fillDims(const GaussianFactorGraph& factorGraph);
};
/* ************************************************************************* */
template<class STORAGE>
GaussianVariableIndex<STORAGE>::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) :
Base(factorGraph), dims_(Base::index_.size()) {
fillDims(factorGraph); }
/* ************************************************************************* */
template<class STORAGE>
GaussianVariableIndex<STORAGE>::GaussianVariableIndex(
const VariableIndex<STORAGE>& variableIndex, const GaussianFactorGraph& factorGraph) :
Base(variableIndex), dims_(Base::index_.size()) {
fillDims(factorGraph); }
/* ************************************************************************* */
template<class STORAGE>
GaussianVariableIndex<STORAGE>::GaussianVariableIndex(
const VariableIndex<STORAGE>& variableIndex, const storage_type& dimensions) :
Base(variableIndex), dims_(dimensions) {
assert(Base::index_.size() == dims_.size()); }
/* ************************************************************************* */
template<class STORAGE>
void GaussianVariableIndex<STORAGE>::fillDims(const GaussianFactorGraph& factorGraph) {
// Store dimensions of each variable
assert(dims_.size() == Base::index_.size());
for(Index var=0; var<Base::index_.size(); ++var)
if(!Base::index_[var].empty()) {
size_t factorIndex = Base::operator [](var).front().factorIndex;
size_t variablePosition = Base::operator [](var).front().variablePosition;
boost::shared_ptr<const GaussianFactor> factor(factorGraph[factorIndex]);
dims_[var] = factor->getDim(factor->begin() + variablePosition);
} else
dims_[var] = 0;
}
/* ************************************************************************* */
template<class STORAGE>
void GaussianVariableIndex<STORAGE>::permute(const Permutation& permutation) {
VariableIndex<STORAGE>::permute(permutation);
storage_type original(this->dims_.size());
this->dims_.swap(original);
for(Index j=0; j<permutation.size(); ++j)
this->dims_[j] = original[permutation[j]];
}
/* ************************************************************************* */
template<class STORAGE>
void GaussianVariableIndex<STORAGE>::augment(const GaussianFactorGraph& factorGraph) {
Base::augment(factorGraph);
dims_.resize(Base::index_.size(), 0);
BOOST_FOREACH(boost::shared_ptr<const GaussianFactor> factor, factorGraph) {
for(GaussianFactor::const_iterator var=factor->begin(); var!=factor->end(); ++var) {
#ifndef NDEBUG
if(dims_[*var] != 0)
assert(dims_[*var] == factor->getDim(var));
#endif
if(dims_[*var] == 0)
dims_[*var] = factor->getDim(var);
}
}
}
} // namespace gtsam } // namespace gtsam

23
linear/GaussianMultifrontalSolver.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GaussianMultifrontalSolver.cpp * @file GaussianMultifrontalSolver.cpp
* @brief * @brief
@ -13,23 +24,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) : GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) :
junctionTree_(new GaussianJunctionTree(factorGraph)) {} Base(factorGraph) {}
/* ************************************************************************* */ /* ************************************************************************* */
BayesTree<GaussianConditional>::sharedClique GaussianMultifrontalSolver::eliminate() const { BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate() const {
return junctionTree_->eliminate(); return Base::eliminate();
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize())); return VectorValues::shared_ptr(new VectorValues(junctionTree_.optimize()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
BayesTree<GaussianConditional> bayesTree; return Base::marginal(j);
bayesTree.insert(junctionTree_->eliminate());
return bayesTree.marginal(j);
} }
} }

18
linear/GaussianMultifrontalSolver.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file GaussianMultifrontalSolver.h * @file GaussianMultifrontalSolver.h
* @brief * @brief
@ -7,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
@ -38,11 +50,11 @@ namespace gtsam {
* typedef'ed in linear/GaussianBayesNet, on which this class calls * typedef'ed in linear/GaussianBayesNet, on which this class calls
* optimize(...) to perform back-substitution. * optimize(...) to perform back-substitution.
*/ */
class GaussianMultifrontalSolver { class GaussianMultifrontalSolver : GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> {
protected: protected:
GaussianJunctionTree::shared_ptr junctionTree_; typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base;
public: public:
@ -56,7 +68,7 @@ public:
* Eliminate the factor graph sequentially. Uses a column elimination tree * Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate. * to recursively eliminate.
*/ */
BayesTree<GaussianConditional>::sharedClique eliminate() const; BayesTree<GaussianConditional>::shared_ptr eliminate() const;
/** /**
* Compute the least-squares solution of the GaussianFactorGraph. This * Compute the least-squares solution of the GaussianFactorGraph. This

11
linear/GaussianSequentialSolver.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file SequentialSolver.cpp * @file SequentialSolver.cpp
* @brief * @brief

11
linear/GaussianSequentialSolver.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file SequentialSolver.h * @file SequentialSolver.h
* @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination. * @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination.

10
linear/SubgraphPreconditioner.cpp
View File

@ -41,11 +41,11 @@ namespace gtsam {
return x; return x;
} }
SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { // SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
SubgraphPreconditioner result = *this ; // SubgraphPreconditioner result = *this ;
result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; // result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
return result ; // return result ;
} // }
/* ************************************************************************* */ /* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const { double SubgraphPreconditioner::error(const VectorValues& y) const {

2
linear/SubgraphPreconditioner.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable * Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian * @param sigma Standard deviation of Gaussian
*/ */
SubgraphPreconditioner add_priors(double sigma) const; // SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */ /* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const; VectorValues x(const VectorValues& y) const;

114
linear/tests/testGaussianFactor.cpp
View File

@ -84,59 +84,59 @@ TEST( GaussianFactor, constructor2)
CHECK(assert_equal(b, actualb)); CHECK(assert_equal(b, actualb));
} }
/* ************************************************************************* */ ///* ************************************************************************* */
TEST(GaussianFactor, Combine) //TEST(GaussianFactor, Combine)
{ //{
Matrix A00 = Matrix_(3,3, // Matrix A00 = Matrix_(3,3,
1.0, 0.0, 0.0, // 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, // 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); // 0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 0.0, 0.0, 0.0); // Vector b0 = Vector_(3, 0.0, 0.0, 0.0);
Vector s0 = Vector_(3, 0.0, 0.0, 0.0); // Vector s0 = Vector_(3, 0.0, 0.0, 0.0);
//
Matrix A10 = Matrix_(3,3, // Matrix A10 = Matrix_(3,3,
0.0, -2.0, -4.0, // 0.0, -2.0, -4.0,
2.0, 0.0, 2.0, // 2.0, 0.0, 2.0,
0.0, 0.0, -10.0); // 0.0, 0.0, -10.0);
Matrix A11 = Matrix_(3,3, // Matrix A11 = Matrix_(3,3,
2.0, 0.0, 0.0, // 2.0, 0.0, 0.0,
0.0, 2.0, 0.0, // 0.0, 2.0, 0.0,
0.0, 0.0, 10.0); // 0.0, 0.0, 10.0);
Vector b1 = Vector_(3, 6.0, 2.0, 0.0); // Vector b1 = Vector_(3, 6.0, 2.0, 0.0);
Vector s1 = Vector_(3, 1.0, 1.0, 1.0); // Vector s1 = Vector_(3, 1.0, 1.0, 1.0);
//
Matrix A20 = Matrix_(3,3, // Matrix A20 = Matrix_(3,3,
1.0, 0.0, 0.0, // 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, // 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); // 0.0, 0.0, 1.0);
Vector b2 = Vector_(3, 0.0, 0.0, 0.0); // Vector b2 = Vector_(3, 0.0, 0.0, 0.0);
Vector s2 = Vector_(3, 100.0, 100.0, 100.0); // Vector s2 = Vector_(3, 100.0, 100.0, 100.0);
//
GaussianFactorGraph gfg; // GaussianFactorGraph gfg;
gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true)); // gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); // gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true)); // gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true));
//
GaussianVariableIndex<> varindex(gfg); // GaussianVariableIndex varindex(gfg);
vector<size_t> factors(3); factors[0]=0; factors[1]=1; factors[2]=2; // vector<size_t> factors(3); factors[0]=0; factors[1]=1; factors[2]=2;
vector<size_t> variables(2); variables[0]=0; variables[1]=1; // vector<size_t> variables(2); variables[0]=0; variables[1]=1;
vector<vector<size_t> > variablePositions(3); // vector<vector<size_t> > variablePositions(3);
variablePositions[0].resize(1); variablePositions[0][0]=0; // variablePositions[0].resize(1); variablePositions[0][0]=0;
variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1; // variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1;
variablePositions[2].resize(1); variablePositions[2][0]=0; // variablePositions[2].resize(1); variablePositions[2][0]=0;
//
GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions); // GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions);
//
Matrix zero3x3 = zeros(3,3); // Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A00, &A10, &A20); // Matrix A0 = gtsam::stack(3, &A00, &A10, &A20);
Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3); // Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3);
Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); // Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); // Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
//
GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); // GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
//
CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GaussianFactor, Combine2) TEST(GaussianFactor, Combine2)
@ -712,23 +712,23 @@ TEST(GaussianFactor, permuteWithInverse)
GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0)); GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual))); GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual)));
GaussianVariableIndex<> actualIndex(actualFG); VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation); actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse()); // actualIndex.permute(*inversePermutation.inverse());
GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected))); GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected)));
// GaussianVariableIndex<> expectedIndex(expectedFG); // GaussianVariableIndex expectedIndex(expectedFG);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// // todo: fix this!!! VariableIndex should not hold slots // // todo: fix this!!! VariableIndex should not hold slots
// for(Index j=0; j<actualIndex.size(); ++j) { // for(Index j=0; j<actualIndex.size(); ++j) {
// BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, actualIndex[j]) { // BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, actualIndex[j]) {
// factor_pos.variablePosition = numeric_limits<Index>::max(); } // factor_pos.variablePosition = numeric_limits<Index>::max(); }
// } // }
// for(Index j=0; j<expectedIndex.size(); ++j) { // for(Index j=0; j<expectedIndex.size(); ++j) {
// BOOST_FOREACH( GaussianVariableIndex<>::mapped_factor_type& factor_pos, expectedIndex[j]) { // BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, expectedIndex[j]) {
// factor_pos.variablePosition = numeric_limits<Index>::max(); } // factor_pos.variablePosition = numeric_limits<Index>::max(); }
// } // }
// CHECK(assert_equal(expectedIndex, actualIndex)); // CHECK(assert_equal(expectedIndex, actualIndex));

2
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -68,7 +68,7 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
boost::tie(symbolic,ordering) = this->symbolic(config); boost::tie(symbolic,ordering) = this->symbolic(config);
// Compute the VariableIndex (column-wise index) // Compute the VariableIndex (column-wise index)
VariableIndex<> variableIndex(*symbolic); VariableIndex variableIndex(*symbolic);
// Compute a fill-reducing ordering with COLAMD // Compute a fill-reducing ordering with COLAMD
Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex)); Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex));

23
nonlinear/NonlinearOptimizer-inl.h
View File

@ -75,14 +75,14 @@ namespace gtsam {
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values config, shared_ordering ordering, double lambda) : shared_values config, shared_ordering ordering, double lambda) :
graph_(graph), config_(config), ordering_(ordering), lambda_(lambda) { graph_(graph), config_(config), error_(graph->error(*config)),
ordering_(ordering), lambda_(lambda), dimensions_(new vector<size_t>(config->dims(*ordering))) {
if (!graph) throw std::invalid_argument( if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL"); "NonlinearOptimizer constructor: graph = NULL");
if (!config) throw std::invalid_argument( if (!config) throw std::invalid_argument(
"NonlinearOptimizer constructor: config = NULL"); "NonlinearOptimizer constructor: config = NULL");
if (!ordering) throw std::invalid_argument( if (!ordering) throw std::invalid_argument(
"NonlinearOptimizer constructor: ordering = NULL"); "NonlinearOptimizer constructor: ordering = NULL");
error_ = graph->error(*config);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -91,7 +91,7 @@ namespace gtsam {
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const { VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const {
boost::shared_ptr<L> linearized = graph_->linearize(*config_, *ordering_); boost::shared_ptr<L> linearized = graph_->linearize(*config_, *ordering_);
NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); // NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_);
return *S(*linearized).optimize(); return *S(*linearized).optimize();
} }
@ -115,7 +115,7 @@ namespace gtsam {
if (verbosity >= Parameters::CONFIG) if (verbosity >= Parameters::CONFIG)
newValues->print("newValues"); newValues->print("newValues");
NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, ordering_, lambda_); NonlinearOptimizer newOptimizer = newValues_(newValues);
if (verbosity >= Parameters::ERROR) if (verbosity >= Parameters::ERROR)
cout << "error: " << newOptimizer.error_ << endl; cout << "error: " << newOptimizer.error_ << endl;
@ -172,7 +172,7 @@ namespace gtsam {
cout << "trying lambda = " << lambda_ << endl; cout << "trying lambda = " << lambda_ << endl;
// add prior-factors // add prior-factors
L damped = linear.add_priors(1.0/sqrt(lambda_)); L damped = linear.add_priors(1.0/sqrt(lambda_), *dimensions_);
if (verbosity >= Parameters::DAMPED) if (verbosity >= Parameters::DAMPED)
damped.print("damped"); damped.print("damped");
@ -187,7 +187,7 @@ namespace gtsam {
// newValues->print("config"); // newValues->print("config");
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
NonlinearOptimizer next(graph_, newValues, ordering_, lambda_ / factor); NonlinearOptimizer next(newValuesNewLambda_(newValues, lambda_ / factor));
if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << next.error_ << endl; if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << next.error_ << endl;
if(lambdaMode >= Parameters::CAUTIOUS) { if(lambdaMode >= Parameters::CAUTIOUS) {
@ -199,7 +199,7 @@ namespace gtsam {
// If we're cautious, see if the current lambda is better // If we're cautious, see if the current lambda is better
// todo: include stopping criterion here? // todo: include stopping criterion here?
if(lambdaMode == Parameters::CAUTIOUS) { if(lambdaMode == Parameters::CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); NonlinearOptimizer sameLambda(newValues_(newValues));
if(sameLambda.error_ <= next.error_) if(sameLambda.error_ <= next.error_)
return sameLambda; return sameLambda;
} }
@ -212,7 +212,7 @@ namespace gtsam {
// A more adventerous lambda was worse. If we're cautious, try the same lambda. // A more adventerous lambda was worse. If we're cautious, try the same lambda.
if(lambdaMode == Parameters::CAUTIOUS) { if(lambdaMode == Parameters::CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); NonlinearOptimizer sameLambda(newValues_(newValues));
if(sameLambda.error_ <= error_) if(sameLambda.error_ <= error_)
return sameLambda; return sameLambda;
} }
@ -223,9 +223,9 @@ namespace gtsam {
// TODO: can we avoid copying the config ? // TODO: can we avoid copying the config ?
if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) { if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) {
return NonlinearOptimizer(graph_, newValues, ordering_, lambda_);; return NonlinearOptimizer(newValues_(newValues));
} else { } else {
NonlinearOptimizer cautious(graph_, config_, ordering_, lambda_ * factor); NonlinearOptimizer cautious(newLambda_(lambda_ * factor));
return cautious.try_lambda(linear, verbosity, factor, lambdaMode); return cautious.try_lambda(linear, verbosity, factor, lambdaMode);
} }
@ -249,13 +249,12 @@ namespace gtsam {
// linearize all factors once // linearize all factors once
boost::shared_ptr<L> linear = graph_->linearize(*config_, *ordering_); boost::shared_ptr<L> linear = graph_->linearize(*config_, *ordering_);
NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_);
if (verbosity >= Parameters::LINEAR) if (verbosity >= Parameters::LINEAR)
linear->print("linear"); linear->print("linear");
// try lambda steps with successively larger lambda until we achieve descent // try lambda steps with successively larger lambda until we achieve descent
if (verbosity >= Parameters::LAMBDA) cout << "Trying Lambda for the first time" << endl; if (verbosity >= Parameters::LAMBDA) cout << "Trying Lambda for the first time" << endl;
return prepared.try_lambda(*linear, verbosity, lambdaFactor, lambdaMode); return try_lambda(*linear, verbosity, lambdaFactor, lambdaMode);
} }
/* ************************************************************************* */ /* ************************************************************************* */

40
nonlinear/NonlinearOptimizer.h
View File

@ -70,13 +70,14 @@ namespace gtsam {
// For performance reasons in recursion, we store configs in a shared_ptr // For performance reasons in recursion, we store configs in a shared_ptr
typedef boost::shared_ptr<const T> shared_values; typedef boost::shared_ptr<const T> shared_values;
typedef boost::shared_ptr<const G> shared_graph; typedef boost::shared_ptr<const G> shared_graph;
typedef boost::shared_ptr<Ordering> shared_ordering; typedef boost::shared_ptr<const Ordering> shared_ordering;
//typedef boost::shared_ptr<const GS> shared_solver;
//typedef const GS solver;
typedef NonlinearOptimizationParameters Parameters; typedef NonlinearOptimizationParameters Parameters;
private: private:
typedef NonlinearOptimizer<G, T, L, GS> This;
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
// keep a reference to const version of the graph // keep a reference to const version of the graph
// These normally do not change // These normally do not change
const shared_graph graph_; const shared_graph graph_;
@ -84,7 +85,7 @@ namespace gtsam {
// keep a values structure and its error // keep a values structure and its error
// These typically change once per iteration (in a functional way) // These typically change once per iteration (in a functional way)
const shared_values config_; const shared_values config_;
double error_; // TODO FD: no more const because in constructor I need to set it after checking :-( const double error_;
const shared_ordering ordering_; const shared_ordering ordering_;
// the linear system solver // the linear system solver
@ -94,10 +95,30 @@ namespace gtsam {
// TODO: red flag, should we have an LM class ? // TODO: red flag, should we have an LM class ?
const double lambda_; const double lambda_;
// The dimensions of each linearized variable
const shared_dimensions dimensions_;
// Recursively try to do tempered Gauss-Newton steps until we succeed // Recursively try to do tempered Gauss-Newton steps until we succeed
NonlinearOptimizer try_lambda(const L& linear, NonlinearOptimizer try_lambda(const L& linear,
Parameters::verbosityLevel verbosity, double factor, Parameters::LambdaMode lambdaMode) const; Parameters::verbosityLevel verbosity, double factor, Parameters::LambdaMode lambdaMode) const;
/**
* Constructor that does not do any computation
*/
NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering,
const double error, const double lambda, shared_dimensions dimensions): graph_(graph), config_(config),
error_(error), ordering_(ordering), lambda_(lambda), dimensions_(dimensions) {}
/** Create a new NonlinearOptimizer with a different lambda */
This newLambda_(double newLambda) const {
return NonlinearOptimizer(graph_, config_, ordering_, error_, newLambda, dimensions_); }
This newValues_(shared_values newValues) const {
return NonlinearOptimizer(graph_, newValues, ordering_, graph_->error(*newValues), lambda_, dimensions_); }
This newValuesNewLambda_(shared_values newValues, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, ordering_, graph_->error(*newValues), newLambda, dimensions_); }
public: public:
/** /**
@ -106,19 +127,12 @@ namespace gtsam {
NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering,
const double lambda = 1e-5); const double lambda = 1e-5);
/**
* Constructor that does not do any computation
*/
NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering,
const double error, const double lambda): graph_(graph), config_(config),
error_(error), ordering_(ordering), lambda_(lambda) {}
/** /**
* Copy constructor * Copy constructor
*/ */
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) : NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
graph_(optimizer.graph_), config_(optimizer.config_), graph_(optimizer.graph_), config_(optimizer.config_), error_(optimizer.error_),
error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_) {} ordering_(optimizer.ordering_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {}
/** /**
* Return current error * Return current error

4
tests/testGaussianFactorGraph.cpp
View File

@ -344,7 +344,7 @@ TEST( GaussianFactorGraph, add_priors )
{ {
Ordering ordering; ordering += "l1","x1","x2"; Ordering ordering; ordering += "l1","x1","x2";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph actual = fg.add_priors(3, GaussianVariableIndex<>(fg)); GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
GaussianFactorGraph expected = createGaussianFactorGraph(ordering); GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
Matrix A = eye(2); Matrix A = eye(2);
Vector b = zero(2); Vector b = zero(2);
@ -460,7 +460,7 @@ TEST( GaussianFactorGraph, getOrdering)
{ {
Ordering original; original += "l1","x1","x2"; Ordering original; original += "l1","x1","x2";
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original)); FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*Inference::PermutationCOLAMD(VariableIndex<>(symbolic))); Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic)));
Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
Ordering expected; expected += "l1","x2","x1"; Ordering expected; expected += "l1","x2","x1";
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));

2
tests/testGaussianISAM.cpp
View File

@ -266,7 +266,7 @@ TEST( BayesTree, balanced_smoother_shortcuts )
// push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2)); // push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]]; // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]];
// GaussianFactorGraph marginal = C3->marginal(R); // GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex<> varIndex(marginal); // GaussianVariableIndex varIndex(marginal);
// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
// Permutation toFrontInverse(*toFront.inverse()); // Permutation toFrontInverse(*toFront.inverse());
// varIndex.permute(toFront); // varIndex.permute(toFront);

2
tests/testGaussianJunctionTree.cpp
View File

@ -94,7 +94,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
VectorValues actual = tree.optimize(); VectorValues actual = tree.optimize();
// verify // verify
VectorValues expected(GaussianVariableIndex<>(fg).dims()); // expected solution VectorValues expected(vector<size_t>(7,2)); // expected solution
Vector v = Vector_(2, 0., 0.); Vector v = Vector_(2, 0., 0.);
for (int i=1; i<=7; i++) for (int i=1; i<=7; i++)
expected[ordering[Symbol('x',i)]] = v; expected[ordering[Symbol('x',i)]] = v;