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;
/** Default constructor */
FastSet() {}
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
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) {}
/** 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
# Data structures
headers += BTree.h DSF.h FastMap.h
headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h
sources += DSFVector.cpp
check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector

10
inference/BayesTree-inl.h
View File

@ -19,6 +19,11 @@
#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 <algorithm>
@ -32,11 +37,6 @@
using namespace boost::assign;
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 {
using namespace std;

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

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

9
inference/EliminationTree-inl.h
View File

@ -47,7 +47,7 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
/* ************************************************************************* */
template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& structure) {
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
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 (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do
BOOST_FOREACH(typename VariableIndex<>::mapped_factor_type i_pos, structure[j]) {
const size_t i = i_pos.factorIndex;
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
Index k = prevCol[i];
// 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 DERIVEDFACTOR>
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
vector<Index> parents(ComputeParents(structure));
@ -125,7 +124,7 @@ template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTree<FACTOR>::shared_ptr
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
* 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
@ -72,7 +72,7 @@ public:
* pre-computed column structure.
*/
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 */
template<class DERIVEDFACTOR>

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

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/FactorBase.h>
#include <boost/foreach.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) {
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). */
template<class DERIVED>
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
* @brief
@ -8,7 +19,7 @@
#pragma once
#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/BayesNet-inl.h>
#include <gtsam/inference/inference-inl.h>
@ -18,19 +29,22 @@
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericMultifrontalSolver<FACTOR>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
junctionTree_(new JunctionTree<FactorGraph<FACTOR> >(factorGraph)) {}
template<class FACTOR, class JUNCTIONTREE>
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
junctionTree_(factorGraph) {}
/* ************************************************************************* */
template<class FACTOR>
typename BayesTree<typename FACTOR::Conditional>::shared_ptr GenericMultifrontalSolver<FACTOR>::eliminate() const {
return junctionTree_->eliminate();
template<class FACTOR, class JUNCTIONTREE>
typename JUNCTIONTREE::BayesTree::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree);
bayesTree->insert(junctionTree_.eliminate());
return bayesTree;
}
/* ************************************************************************* */
template<class FACTOR>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR>::marginal(Index j) const {
template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginal(Index j) const {
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
* @brief
@ -15,13 +26,13 @@
namespace gtsam {
template<class FACTOR>
template<class FACTOR, class JUNCTIONTREE>
class GenericMultifrontalSolver {
protected:
// Elimination tree that performs elimination.
typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr junctionTree_;
// Junction tree that performs elimination.
JUNCTIONTREE junctionTree_;
public:
@ -35,7 +46,7 @@ public:
* Eliminate the factor graph sequentially. Uses a column elimination tree
* 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

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
* @brief
@ -8,7 +19,7 @@
#pragma once
#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/BayesNet-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
* @brief
@ -24,7 +35,7 @@ protected:
FactorGraph<FACTOR> factors_;
// Column structure of the factor graph
VariableIndex<> structure_;
VariableIndex structure_;
// Elimination tree that performs elimination.
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 +=
using namespace boost::assign;
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/ConditionalBase.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>

2
inference/IndexConditional.h
View File

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

2
inference/IndexFactor.cpp
View File

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

8
inference/IndexFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/FactorBase.h>
namespace gtsam {
@ -63,12 +63,6 @@ public:
static shared_ptr
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
* @return a conditional on the eliminated variable

8
inference/Makefile.am
View File

@ -15,19 +15,19 @@ check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# GTSAM core
headers += Factor.h Factor-inl.h Conditional.h
headers += FactorBase.h FactorBase-inl.h ConditionalBase.h
# 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/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
# Inference
headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-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
headers += graph.h graph-inl.h
headers += VariableIndex.h
headers += FactorGraph.h FactorGraph-inl.h
headers += ClusterTree.h ClusterTree-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> {
public:
typedef SymbolicBayesNet bayesnet_type;
typedef VariableIndex<> variableindex_type;
/** Construct empty factor graph */
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
* @brief
@ -10,19 +21,21 @@
namespace gtsam {
// An explicit instantiation to be compiled into the library
template class GenericSequentialSolver<IndexFactor>;
/* ************************************************************************* */
SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) :
Base(factorGraph) {}
/* ************************************************************************* */
BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const {
return Base::eliminate();
}
/* ************************************************************************* */
SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const {
return SymbolicFactorGraph::shared_ptr(new SymbolicFactorGraph(*Base::joint(js)));
}
///* ************************************************************************* */
//SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) :
// Base(factorGraph) {}
//
///* ************************************************************************* */
//BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const {
// return Base::eliminate();
//}
//
///* ************************************************************************* */
//SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const {
// 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
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
@ -12,37 +22,40 @@
namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
// The base class provides all of the needed functionality
typedef GenericSequentialSolver<IndexFactor> SymbolicSequentialSolver;
protected:
typedef GenericSequentialSolver<IndexFactor> Base;
public:
SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* 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.
*/
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
* R*x = d. To get a mean and covariance matrix, use jointStandard(...)
*/
SymbolicFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
};
//class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
//
//protected:
//
// typedef GenericSequentialSolver<IndexFactor> Base;
//
//public:
//
// SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph);
//
// /**
// * Eliminate the factor graph sequentially. Uses a column elimination tree
// * 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.
// */
// 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
// * 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
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/Permutation.h>
#include <vector>
#include <deque>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
@ -31,43 +31,23 @@ namespace gtsam {
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
* elimination, including an index of factors involving each variable and
* the structure of the intermediate joint factors that develop during
* elimination. This maps variables to deque's of pair<size_t,size_t>,
* which is pairs the factor index with the position of the variable within
* the factor.
* elimination. This maps variables to collections of factor indices.
*/
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 {
public:
typedef _mapped_factor_type mapped_factor_type;
typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef std::deque<mapped_factor_type> mapped_type;
typedef typename mapped_type::iterator factor_iterator;
typedef typename mapped_type::const_iterator const_factor_iterator;
typedef FastList<size_t> Factors;
typedef typename Factors::iterator Factor_iterator;
typedef typename Factors::const_iterator Factor_const_iterator;
protected:
typedef typename VARIABLEINDEXSTORAGE::template type_factory<mapped_type>::type storage_type;
storage_type indexUnpermuted_;
Permuted<storage_type, typename storage_type::value_type&> index_;
std::vector<Factors> indexUnpermuted_;
Permuted<std::vector<Factors>, Factors&> index_;
size_t nFactors_;
size_t nEntries_; // Sum of involved variable counts of each factor
@ -79,13 +59,13 @@ public:
Index size() const { return index_.size(); }
size_t nFactors() const { return nFactors_; }
size_t nEntries() const { return nEntries_; }
const mapped_type& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
mapped_type& operator[](Index variable) { checkVar(variable); return index_[variable]; }
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
void permute(const Permutation& permutation);
template<class FactorGraph> void augment(const FactorGraph& factorGraph);
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;
protected:
@ -94,56 +74,32 @@ protected:
template<class FactorGraph> void fill(const FactorGraph& factorGraph);
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_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
const_factor_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
friend class Inference;
Factor_iterator factorsBegin(Index variable) { 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_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
};
/* ************************************************************************* */
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>
void VariableIndex<Storage>::fill(const FactorGraph& factorGraph) {
void VariableIndex::fill(const FactorGraph& factorGraph) {
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) {
index_[key].push_back(mapped_factor_type(fi, fvari));
++ fvari;
index_[key].push_back(fi);
++ nEntries_;
}
}
++ nFactors_;
}
}
/* ************************************************************************* */
template<class Storage>
template<class FactorGraph>
VariableIndex<Storage>::VariableIndex(const FactorGraph& factorGraph) :
VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
// 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);
}
}
/* ************************************************************************* */
template<class Storage>
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) {
fill(factorGraph);
}
/* ************************************************************************* */
template<class Storage>
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 block we assume at least one factor.
if(factorGraph.size() > 0) {
@ -206,10 +159,8 @@ void VariableIndex<Storage>::augment(const FactorGraph& factorGraph) {
size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factorGraph.size(); ++fi)
if(factorGraph[fi]) {
Index fvari = 0;
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
index_[key].push_back(mapped_factor_type(orignFactors + fi, fvari));
++ fvari;
index_[key].push_back(orignFactors + fi);
++ nEntries_;
}
++ 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/FactorGraph-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/ConditionalBase.h>
using namespace std;
@ -54,13 +54,13 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& va
p[0] = 0;
int count = 0;
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();
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())
assert(factor_pos.factorIndex > lastFactorId);
A[count++] = factor_pos.factorIndex; // copy sparse column
if(debug) cout << "A[" << count-1 << "] = " << factor_pos.factorIndex << endl;
assert(factorIndex > lastFactorId);
A[count++] = factorIndex; // copy sparse column
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
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);
VariableIndex<> expected(fgCombined);
VariableIndex<> actual(fg1);
VariableIndex expected(fgCombined);
VariableIndex actual(fg1);
actual.augment(fg2);
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; }
};
/* 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_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
// TODO: would be nicer if this function was split and are factorgraph methods
static std::pair<size_t,bool> rowCount(const FactorGraph<GaussianFactor>& factorGraph,
const vector<size_t>& factorIndices)
{
static const bool debug = false;
tic("Combine: count sizes");
size_t m = 0;
bool constrained = false;
BOOST_FOREACH(const size_t i, factorIndices)
{
assert(factorGraph[i] != NULL);
assert(!factorGraph[i]->keys().empty());
m += factorGraph[i]->numberOfRows();
if (debug) cout << "Combining factor " << i << endl;
if (debug) factorGraph[i]->print(" :");
if (!constrained && factorGraph[i]->isConstrained()) {
constrained = true;
if (debug) std::cout << "Found a constraint!" << std::endl;
}
}
toc("Combine: count sizes");
return make_pair(m,constrained);
}
// Determine row and column counts and check if any noise models are constrained
template<class STORAGE>
static vector<size_t> columnDimensions(
const GaussianVariableIndex<STORAGE>& variableIndex,
const vector<Index>& variables)
{
tic("Combine: count dims");
static const bool debug = false;
vector<size_t> dims(variables.size() + 1);
size_t n = 0;
{
size_t j = 0;
BOOST_FOREACH(const Index& var, variables)
{
if (debug) cout << "Have variable " << var << endl;
dims[j] = variableIndex.dim(var);
n += dims[j];
++j;
}
dims[j] = 1;
}
toc("Combine: count dims");
return dims;
}
// To do this, we merge-sort the rows so that the column indices of the first structural
// non-zero in each row increase monotonically.
vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorIndices,
const FactorGraph<GaussianFactor>& factorGraph) {
vector<_RowSource> rowSources;
rowSources.reserve(m);
size_t i1 = 0;
BOOST_FOREACH(const size_t i2, factorIndices) {
const GaussianFactor& factor(*factorGraph[i2]);
size_t factorRowI = 0;
assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows());
BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) {
Index firstNonzeroVar;
firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos];
rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI));
++ factorRowI;
}
assert(factorRowI == factor.numberOfRows());
++ i1;
}
assert(rowSources.size() == m);
assert(i1 == factorIndices.size());
sort(rowSources.begin(), rowSources.end());
return rowSources;
}
void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row,
const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions,
const size_t factorRow, const size_t factorI, const vector<Index>& variables) {
const static bool debug = false;
const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow];
std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos;
std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
combinedFactor->set_firstNonzeroBlocks(row, *varposIt);
if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
std::vector<Index>::const_iterator keyitend = factor.keys().end();
while(keyit != keyitend) {
const size_t varpos = *varposIt;
assert(variables[varpos] == *keyit);
GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos));
const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit));
ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
++ keyit;
++ varposIt;
}
}
template<class STORAGE>
GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
// Debugging flags
static const bool verbose = false;
static const bool debug = false;
if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl;
assert(factorIndices.size() == variablePositions.size());
// Determine row count and check if any noise models are constrained
size_t m; bool constrained;
boost::tie(m,constrained) = rowCount(factorGraph,factorIndices);
// Determine column dimensions
vector<size_t> dims = columnDimensions(variableIndex,variables);
// Allocate return value, the combined factor, the augmented Ab matrix and other arrays
tic("Combine: set up empty");
shared_ptr combinedFactor(boost::make_shared<GaussianFactor>());
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());
combinedFactor->firstNonzeroBlocks_.resize(m);
Vector sigmas(m);
// Copy keys
combinedFactor->keys_.reserve(variables.size());
combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end());
toc("Combine: set up empty");
// Compute a row permutation that maintains a staircase pattern in the new combined factor.
tic("Combine: sort rows");
vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph);
toc("Combine: sort rows");
// 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.
// todo SL: smarter ignoring of zero factor variables (store first possible like above)
if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: ");
tic("Combine: copy rows");
#ifndef NDEBUG
size_t lastRowFirstVarpos = 0;
#endif
for(size_t row=0; row<m; ++row) {
const _RowSource& rowSource = rowSources[row];
assert(rowSource.factorI < factorGraph.size());
const size_t factorI = rowSource.factorI;
const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]);
const size_t factorRow = rowSource.factorRowI;
if(debug)
cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl;
// Copy rhs b and sigma
combinedFactor->getb()(row) = factor.getb()(factorRow);
sigmas(row) = factor.get_sigmas()(factorRow);
// Copy the row of A variable by variable, starting at the first nonzero variable.
copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables);
#ifndef NDEBUG
// Debug check, make sure the first column of nonzeros increases monotonically
if(row != 0)
assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos);
lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row];
#endif
}
toc("Combine: copy rows");
if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl;
if (constrained) {
combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas);
if (verbose) combinedFactor->model_->print("Just created Constraint ^");
} else {
combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas);
if (verbose) combinedFactor->model_->print("Just created Diagonal");
}
if(debug) combinedFactor->print("Combined factor: ");
combinedFactor->assertInvariants();
return combinedFactor;
}
///* 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_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
//// TODO: would be nicer if this function was split and are factorgraph methods
//static std::pair<size_t,bool> rowCount(const FactorGraph<GaussianFactor>& factorGraph,
// const vector<size_t>& factorIndices)
//{
// static const bool debug = false;
// tic("Combine: count sizes");
// size_t m = 0;
// bool constrained = false;
// BOOST_FOREACH(const size_t i, factorIndices)
// {
// assert(factorGraph[i] != NULL);
// assert(!factorGraph[i]->keys().empty());
// m += factorGraph[i]->numberOfRows();
// if (debug) cout << "Combining factor " << i << endl;
// if (debug) factorGraph[i]->print(" :");
// if (!constrained && factorGraph[i]->isConstrained()) {
// constrained = true;
// if (debug) std::cout << "Found a constraint!" << std::endl;
// }
// }
// toc("Combine: count sizes");
// return make_pair(m,constrained);
//}
//
//// Determine row and column counts and check if any noise models are constrained
//template<class STORAGE>
//static vector<size_t> columnDimensions(
// const GaussianVariableIndex<STORAGE>& variableIndex,
// const vector<Index>& variables)
//{
// tic("Combine: count dims");
// static const bool debug = false;
// vector<size_t> dims(variables.size() + 1);
// size_t n = 0;
// {
// size_t j = 0;
// BOOST_FOREACH(const Index& var, variables)
// {
// if (debug) cout << "Have variable " << var << endl;
// dims[j] = variableIndex.dim(var);
// n += dims[j];
// ++j;
// }
// dims[j] = 1;
// }
// toc("Combine: count dims");
// return dims;
//}
//
//// To do this, we merge-sort the rows so that the column indices of the first structural
//// non-zero in each row increase monotonically.
//vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorIndices,
// const FactorGraph<GaussianFactor>& factorGraph) {
// vector<_RowSource> rowSources;
// rowSources.reserve(m);
// size_t i1 = 0;
// BOOST_FOREACH(const size_t i2, factorIndices) {
// const GaussianFactor& factor(*factorGraph[i2]);
// size_t factorRowI = 0;
// assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows());
// BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) {
// Index firstNonzeroVar;
// firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos];
// rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI));
// ++ factorRowI;
// }
// assert(factorRowI == factor.numberOfRows());
// ++ i1;
// }
// assert(rowSources.size() == m);
// assert(i1 == factorIndices.size());
// sort(rowSources.begin(), rowSources.end());
// return rowSources;
//}
//
//void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row,
// const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions,
// const size_t factorRow, const size_t factorI, const vector<Index>& variables) {
// const static bool debug = false;
// const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow];
// std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos;
// std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
// combinedFactor->set_firstNonzeroBlocks(row, *varposIt);
// if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
// std::vector<Index>::const_iterator keyitend = factor.keys().end();
// while(keyit != keyitend) {
// const size_t varpos = *varposIt;
// assert(variables[varpos] == *keyit);
// GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos));
// const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit));
// ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
// ++ keyit;
// ++ varposIt;
// }
//}
//
//template<class STORAGE>
//GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
// const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
// const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
//
// // Debugging flags
// static const bool verbose = false;
// static const bool debug = false;
// if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl;
// assert(factorIndices.size() == variablePositions.size());
//
// // Determine row count and check if any noise models are constrained
// size_t m; bool constrained;
// boost::tie(m,constrained) = rowCount(factorGraph,factorIndices);
//
// // Determine column dimensions
// vector<size_t> dims = columnDimensions(variableIndex,variables);
//
// // Allocate return value, the combined factor, the augmented Ab matrix and other arrays
// tic("Combine: set up empty");
// shared_ptr combinedFactor(boost::make_shared<GaussianFactor>());
// 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());
// combinedFactor->firstNonzeroBlocks_.resize(m);
// Vector sigmas(m);
//
// // Copy keys
// combinedFactor->keys_.reserve(variables.size());
// combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end());
// toc("Combine: set up empty");
//
// // Compute a row permutation that maintains a staircase pattern in the new combined factor.
// tic("Combine: sort rows");
// vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph);
// toc("Combine: sort rows");
//
// // 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.
// // todo SL: smarter ignoring of zero factor variables (store first possible like above)
//
// if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: ");
//
// tic("Combine: copy rows");
//#ifndef NDEBUG
// size_t lastRowFirstVarpos = 0;
//#endif
// for(size_t row=0; row<m; ++row) {
//
// const _RowSource& rowSource = rowSources[row];
// assert(rowSource.factorI < factorGraph.size());
// const size_t factorI = rowSource.factorI;
// const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]);
// const size_t factorRow = rowSource.factorRowI;
//
// if(debug)
// cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl;
//
// // Copy rhs b and sigma
// combinedFactor->getb()(row) = factor.getb()(factorRow);
// sigmas(row) = factor.get_sigmas()(factorRow);
//
// // Copy the row of A variable by variable, starting at the first nonzero variable.
// copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables);
//
//#ifndef NDEBUG
// // Debug check, make sure the first column of nonzeros increases monotonically
// if(row != 0)
// assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos);
// lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row];
//#endif
// }
// toc("Combine: copy rows");
//
// if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl;
//
// if (constrained) {
// combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas);
// if (verbose) combinedFactor->model_->print("Just created Constraint ^");
// } else {
// combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas);
// if (verbose) combinedFactor->model_->print("Just created Diagonal");
// }
//
// if(debug) combinedFactor->print("Combined factor: ");
//
// combinedFactor->assertInvariants();
//
// return combinedFactor;
//}
/* ************************************************************************* */
// Helper functions for Combine

15
linear/GaussianFactor.h
View File

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

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
GaussianFactorGraph result = *this;
// for each of the variables, add a prior
for(Index var=0; var<variableIndex.size(); ++var) {
size_t dim = variableIndex.dim(var);
for(Index j=0; j<dimensions.size(); ++j) {
size_t dim = dimensions[j];
Matrix A = eye(dim);
Vector b = zero(dim);
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);
}
return result;
}
GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
return add_priors(sigma, GaussianVariableIndex<>(*this)) ;
}
} // namespace gtsam

117
linear/GaussianFactorGraph.h
View File

@ -39,7 +39,6 @@ namespace gtsam {
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr;
typedef GaussianBayesNet bayesnet_type;
typedef GaussianVariableIndex<> variableindex_type;
/**
* Default constructor
@ -152,122 +151,8 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
GaussianFactorGraph add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const;
GaussianFactorGraph add_priors(double sigma) const;
GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& dimensions) 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

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
* @brief
@ -13,23 +24,21 @@ namespace gtsam {
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) :
junctionTree_(new GaussianJunctionTree(factorGraph)) {}
Base(factorGraph) {}
/* ************************************************************************* */
BayesTree<GaussianConditional>::sharedClique GaussianMultifrontalSolver::eliminate() const {
return junctionTree_->eliminate();
BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate() const {
return Base::eliminate();
}
/* ************************************************************************* */
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 {
BayesTree<GaussianConditional> bayesTree;
bayesTree.insert(junctionTree_->eliminate());
return bayesTree.marginal(j);
return Base::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
* @brief
@ -7,6 +18,7 @@
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -38,11 +50,11 @@ namespace gtsam {
* typedef'ed in linear/GaussianBayesNet, on which this class calls
* optimize(...) to perform back-substitution.
*/
class GaussianMultifrontalSolver {
class GaussianMultifrontalSolver : GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> {
protected:
GaussianJunctionTree::shared_ptr junctionTree_;
typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base;
public:
@ -56,7 +68,7 @@ public:
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
BayesTree<GaussianConditional>::sharedClique eliminate() const;
BayesTree<GaussianConditional>::shared_ptr eliminate() const;
/**
* 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
* @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
* @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;
}
SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
SubgraphPreconditioner result = *this ;
result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
return result ;
}
// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
// SubgraphPreconditioner result = *this ;
// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
// return result ;
// }
/* ************************************************************************* */
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
* @param sigma Standard deviation of Gaussian
*/
SubgraphPreconditioner add_priors(double sigma) const;
// SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */
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));
}
/* ************************************************************************* */
TEST(GaussianFactor, Combine)
{
Matrix A00 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 0.0, 0.0, 0.0);
Vector s0 = Vector_(3, 0.0, 0.0, 0.0);
Matrix A10 = Matrix_(3,3,
0.0, -2.0, -4.0,
2.0, 0.0, 2.0,
0.0, 0.0, -10.0);
Matrix A11 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 10.0);
Vector b1 = Vector_(3, 6.0, 2.0, 0.0);
Vector s1 = Vector_(3, 1.0, 1.0, 1.0);
Matrix A20 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b2 = Vector_(3, 0.0, 0.0, 0.0);
Vector s2 = Vector_(3, 100.0, 100.0, 100.0);
GaussianFactorGraph gfg;
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, A20, b2, noiseModel::Diagonal::Sigmas(s2, true));
GaussianVariableIndex<> varindex(gfg);
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<vector<size_t> > variablePositions(3);
variablePositions[0].resize(1); variablePositions[0][0]=0;
variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1;
variablePositions[2].resize(1); variablePositions[2][0]=0;
GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions);
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A00, &A10, &A20);
Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3);
Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
CHECK(assert_equal(expected, actual));
}
///* ************************************************************************* */
//TEST(GaussianFactor, Combine)
//{
// Matrix A00 = Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0);
// Vector b0 = Vector_(3, 0.0, 0.0, 0.0);
// Vector s0 = Vector_(3, 0.0, 0.0, 0.0);
//
// Matrix A10 = Matrix_(3,3,
// 0.0, -2.0, -4.0,
// 2.0, 0.0, 2.0,
// 0.0, 0.0, -10.0);
// Matrix A11 = Matrix_(3,3,
// 2.0, 0.0, 0.0,
// 0.0, 2.0, 0.0,
// 0.0, 0.0, 10.0);
// Vector b1 = Vector_(3, 6.0, 2.0, 0.0);
// Vector s1 = Vector_(3, 1.0, 1.0, 1.0);
//
// Matrix A20 = Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0);
// Vector b2 = Vector_(3, 0.0, 0.0, 0.0);
// Vector s2 = Vector_(3, 100.0, 100.0, 100.0);
//
// GaussianFactorGraph gfg;
// 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, A20, b2, noiseModel::Diagonal::Sigmas(s2, true));
//
// GaussianVariableIndex varindex(gfg);
// 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<vector<size_t> > variablePositions(3);
// variablePositions[0].resize(1); variablePositions[0][0]=0;
// variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1;
// variablePositions[2].resize(1); variablePositions[2][0]=0;
//
// GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions);
//
// Matrix zero3x3 = zeros(3,3);
// Matrix A0 = gtsam::stack(3, &A00, &A10, &A20);
// Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3);
// Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
// Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
//
// GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
//
// CHECK(assert_equal(expected, actual));
//}
/* ************************************************************************* */
TEST(GaussianFactor, Combine2)
@ -712,23 +712,23 @@ TEST(GaussianFactor, permuteWithInverse)
GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual)));
GaussianVariableIndex<> actualIndex(actualFG);
VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse());
GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected)));
// GaussianVariableIndex<> expectedIndex(expectedFG);
// GaussianVariableIndex expectedIndex(expectedFG);
CHECK(assert_equal(expected, actual));
// // todo: fix this!!! VariableIndex should not hold slots
// 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(); }
// }
// 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(); }
// }
// 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);
// Compute the VariableIndex (column-wise index)
VariableIndex<> variableIndex(*symbolic);
VariableIndex variableIndex(*symbolic);
// Compute a fill-reducing ordering with COLAMD
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>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
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(
"NonlinearOptimizer constructor: graph = NULL");
if (!config) throw std::invalid_argument(
"NonlinearOptimizer constructor: config = NULL");
if (!ordering) throw std::invalid_argument(
"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>
VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const {
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();
}
@ -115,7 +115,7 @@ namespace gtsam {
if (verbosity >= Parameters::CONFIG)
newValues->print("newValues");
NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, ordering_, lambda_);
NonlinearOptimizer newOptimizer = newValues_(newValues);
if (verbosity >= Parameters::ERROR)
cout << "error: " << newOptimizer.error_ << endl;
@ -172,7 +172,7 @@ namespace gtsam {
cout << "trying lambda = " << lambda_ << endl;
// 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)
damped.print("damped");
@ -187,7 +187,7 @@ namespace gtsam {
// newValues->print("config");
// 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(lambdaMode >= Parameters::CAUTIOUS) {
@ -199,7 +199,7 @@ namespace gtsam {
// If we're cautious, see if the current lambda is better
// todo: include stopping criterion here?
if(lambdaMode == Parameters::CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_);
NonlinearOptimizer sameLambda(newValues_(newValues));
if(sameLambda.error_ <= next.error_)
return sameLambda;
}
@ -212,7 +212,7 @@ namespace gtsam {
// A more adventerous lambda was worse. If we're cautious, try the same lambda.
if(lambdaMode == Parameters::CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_);
NonlinearOptimizer sameLambda(newValues_(newValues));
if(sameLambda.error_ <= error_)
return sameLambda;
}
@ -223,9 +223,9 @@ namespace gtsam {
// TODO: can we avoid copying the config ?
if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) {
return NonlinearOptimizer(graph_, newValues, ordering_, lambda_);;
return NonlinearOptimizer(newValues_(newValues));
} else {
NonlinearOptimizer cautious(graph_, config_, ordering_, lambda_ * factor);
NonlinearOptimizer cautious(newLambda_(lambda_ * factor));
return cautious.try_lambda(linear, verbosity, factor, lambdaMode);
}
@ -249,13 +249,12 @@ namespace gtsam {
// linearize all factors once
boost::shared_ptr<L> linear = graph_->linearize(*config_, *ordering_);
NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_);
if (verbosity >= Parameters::LINEAR)
linear->print("linear");
// try lambda steps with successively larger lambda until we achieve descent
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
typedef boost::shared_ptr<const T> shared_values;
typedef boost::shared_ptr<const G> shared_graph;
typedef boost::shared_ptr<Ordering> shared_ordering;
//typedef boost::shared_ptr<const GS> shared_solver;
//typedef const GS solver;
typedef boost::shared_ptr<const Ordering> shared_ordering;
typedef NonlinearOptimizationParameters Parameters;
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
// These normally do not change
const shared_graph graph_;
@ -84,7 +85,7 @@ namespace gtsam {
// keep a values structure and its error
// These typically change once per iteration (in a functional way)
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_;
// the linear system solver
@ -94,10 +95,30 @@ namespace gtsam {
// TODO: red flag, should we have an LM class ?
const double lambda_;
// The dimensions of each linearized variable
const shared_dimensions dimensions_;
// Recursively try to do tempered Gauss-Newton steps until we succeed
NonlinearOptimizer try_lambda(const L& linear,
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:
/**
@ -106,19 +127,12 @@ namespace gtsam {
NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering,
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
*/
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
graph_(optimizer.graph_), config_(optimizer.config_),
error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_) {}
graph_(optimizer.graph_), config_(optimizer.config_), error_(optimizer.error_),
ordering_(optimizer.ordering_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {}
/**
* Return current error

4
tests/testGaussianFactorGraph.cpp
View File

@ -344,7 +344,7 @@ TEST( GaussianFactorGraph, add_priors )
{
Ordering ordering; ordering += "l1","x1","x2";
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);
Matrix A = eye(2);
Vector b = zero(2);
@ -460,7 +460,7 @@ TEST( GaussianFactorGraph, getOrdering)
{
Ordering original; original += "l1","x1","x2";
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 expected; expected += "l1","x2","x1";
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));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]];
// GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex<> varIndex(marginal);
// GaussianVariableIndex varIndex(marginal);
// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
// Permutation toFrontInverse(*toFront.inverse());
// varIndex.permute(toFront);

2
tests/testGaussianJunctionTree.cpp
View File

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