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

Storing variable index in solver, saved between nonlinear iterations

release/4.3a0
Richard Roberts 2010-11-15 23:01:50 +00:00
parent a124ce132f
commit d6929d4409
20 changed files with 354 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/inference/EliminationTree-inl.h
View File

@ -4,6 +4,7 @@
* @author Frank Dellaert
* @created Oct 13, 2010
*/
#pragma once
#include <gtsam/base/timing.h>
#include <gtsam/inference/EliminationTree.h>

21
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -33,14 +33,31 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
junctionTree_(factorGraph) {}
structure_(new VariableIndex(factorGraph)), junctionTree_(new JUNCTIONTREE(factorGraph, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) :
structure_(new VariableIndex(*factorGraph)), junctionTree_(new JUNCTIONTREE(*factorGraph, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::GenericMultifrontalSolver(
const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) :
structure_(variableIndex), junctionTree_(new JUNCTIONTREE(*factorGraph, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
void GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
junctionTree_.reset(new JUNCTIONTREE(*factorGraph, *structure_));
}
/* ************************************************************************* */
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());
bayesTree->insert(junctionTree_->eliminate());
return bayesTree;
}

29
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -31,17 +31,40 @@ class GenericMultifrontalSolver {
protected:
// Column structure of the factor graph
VariableIndex::shared_ptr structure_;
// Junction tree that performs elimination.
JUNCTIONTREE junctionTree_;
typename JUNCTIONTREE::shared_ptr junctionTree_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
* Construct the solver for a factor graph. This builds the junction
* tree, which already does some of the work of elimination.
*/
GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph);
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which already does some of the work of elimination.
*/
GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericMultifrontalSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
* such as during relinearization or adjusting of noise models.
*/
void replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.

35
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -31,9 +31,30 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
structure_(factorGraph),
eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
factors_.push_back(factorGraph);
factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(new VariableIndex(factorGraph)),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) :
factors_(factorGraph), structure_(new VariableIndex(*factorGraph)),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) :
factors_(factorGraph), structure_(variableIndex),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
// Reset this shared pointer first to deallocate if possible - for big
// problems there may not be enough memory to store two copies.
eliminationTree_.reset();
factors_ = factorGraph;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
@ -47,23 +68,23 @@ template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::jointFactorGraph(const std::vector<Index>& js) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js));
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
Permutation::shared_ptr permutationInverse(permutation->inverse());
// Permute the factors - NOTE that this permutes the original factors, not
// copies. Other parts of the code may hold shared_ptr's to these factors so
// we must undo the permutation before returning.
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) {
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
if(factor)
factor->permuteWithInverse(*permutationInverse);
}
// Eliminate all variables
typename BayesNet<typename FACTOR::Conditional>::shared_ptr bayesNet(
EliminationTree<FACTOR>::Create(factors_)->eliminate());
EliminationTree<FACTOR>::Create(*factors_)->eliminate());
// Undo the permuation on the original factors and on the structure.
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) {
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
if(factor)
factor->permuteWithInverse(*permutation);
}

26
gtsam/inference/GenericSequentialSolver.h
View File

@ -32,10 +32,10 @@ class GenericSequentialSolver {
protected:
// Store the original factors for computing marginals
FactorGraph<FACTOR> factors_;
typename FactorGraph<FACTOR>::shared_ptr factors_;
// Column structure of the factor graph
VariableIndex structure_;
VariableIndex::shared_ptr structure_;
// Elimination tree that performs elimination.
typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;
@ -44,10 +44,30 @@ public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
* tree, which already does some of the work of elimination.
*/
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
* such as during relinearization or adjusting of noise models.
*/
void replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.

269
gtsam/inference/JunctionTree-inl.h
View File

@ -25,7 +25,7 @@
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/ClusterTree-inl.h>
#include <boost/foreach.hpp>
@ -35,129 +35,136 @@
namespace gtsam {
using namespace std;
/* ************************************************************************* */
template <class FG>
JunctionTree<FG>::JunctionTree(const FG& fg) {
tic("JT 1 constructor");
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
// -> SymbolicBayesNet -> SymbolicBayesTree
tic("JT 1.1 symbolic elimination");
SymbolicBayesNet::shared_ptr sbn = SymbolicSequentialSolver(fg).eliminate();
// SymbolicFactorGraph sfg(fg);
// SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg);
// assert(assert_equal(*sbn, *sbn_orig));
toc("JT 1.1 symbolic elimination");
tic("JT 1.2 symbolic BayesTree");
SymbolicBayesTree sbt(*sbn);
toc("JT 1.2 symbolic BayesTree");
// distribute factors
tic("JT 1.3 distributeFactors");
this->root_ = distributeFactors(fg, sbt.root());
toc("JT 1.3 distributeFactors");
toc("JT 1 constructor");
}
/* ************************************************************************* */
template<class FG>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors
// that involve this variable as their *lowest-ordered* variable. For each
// factor, it is the lowest-ordered variable of that factor that pulls the
// factor into elimination, after which all of the information in the
// factor is contained in the eliminated factors that are passed up the
// tree as elimination continues.
// Two stages - first build an array of the lowest-ordered variable in each
// factor and find the last variable to be eliminated.
vector<Index> lowestOrdered(fg.size());
Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) {
typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min == fg[i]->end())
lowestOrdered[i] = numeric_limits<Index>::max();
else {
lowestOrdered[i] = *min;
maxVar = std::max(maxVar, *min);
}
}
// Now add each factor to the list corresponding to its lowest-ordered
// variable.
vector<list<size_t, boost::fast_pool_allocator<size_t> > > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i);
// Now call the recursive distributeFactors
return distributeFactors(fg, targets, bayesClique);
}
using namespace std;
/* ************************************************************************* */
template<class FG>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg,
const std::vector<std::list<size_t,boost::fast_pool_allocator<size_t> > >& targets,
template <class FG>
void JunctionTree<FG>::construct(const FG& fg, const VariableIndex& variableIndex) {
tic("JT 1 constructor");
tic("JT 1.1 symbolic elimination");
SymbolicBayesNet::shared_ptr sbn = EliminationTree<IndexFactor>::Create(fg, variableIndex)->eliminate();
toc("JT 1.1 symbolic elimination");
tic("JT 1.2 symbolic BayesTree");
SymbolicBayesTree sbt(*sbn);
toc("JT 1.2 symbolic BayesTree");
// distribute factors
tic("JT 1.3 distributeFactors");
this->root_ = distributeFactors(fg, sbt.root());
toc("JT 1.3 distributeFactors");
toc("JT 1 constructor");
}
/* ************************************************************************* */
template <class FG>
JunctionTree<FG>::JunctionTree(const FG& fg) {
construct(fg, VariableIndex(fg));
}
/* ************************************************************************* */
template <class FG>
JunctionTree<FG>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
construct(fg, variableIndex);
}
/* ************************************************************************* */
template<class FG>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors
// that involve this variable as their *lowest-ordered* variable. For each
// factor, it is the lowest-ordered variable of that factor that pulls the
// factor into elimination, after which all of the information in the
// factor is contained in the eliminated factors that are passed up the
// tree as elimination continues.
// Two stages - first build an array of the lowest-ordered variable in each
// factor and find the last variable to be eliminated.
vector<Index> lowestOrdered(fg.size());
Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) {
typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min == fg[i]->end())
lowestOrdered[i] = numeric_limits<Index>::max();
else {
lowestOrdered[i] = *min;
maxVar = std::max(maxVar, *min);
}
}
// Now add each factor to the list corresponding to its lowest-ordered
// variable.
vector<list<size_t, boost::fast_pool_allocator<size_t> > > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i);
// Now call the recursive distributeFactors
return distributeFactors(fg, targets, bayesClique);
}
/* ************************************************************************* */
template<class FG>
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(const FG& fg,
const std::vector<std::list<size_t,boost::fast_pool_allocator<size_t> > >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) {
if(bayesClique) {
// create a new clique in the junction tree
list<Index> frontals = bayesClique->ordering();
sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
if(bayesClique) {
// create a new clique in the junction tree
list<Index> frontals = bayesClique->ordering();
sharedClique clique(new Clique(frontals.begin(), frontals.end(), bayesClique->separator_.begin(), bayesClique->separator_.end()));
// count the factors for this cluster to pre-allocate space
{
size_t nFactors = 0;
BOOST_FOREACH(const Index frontal, clique->frontal) {
// There may be less variables in "targets" than there really are if
// some of the highest-numbered variables do not pull in any factors.
if(frontal < targets.size())
nFactors += targets[frontal].size(); }
clique->reserve(nFactors);
}
// add the factors to this cluster
BOOST_FOREACH(const Index frontal, clique->frontal) {
if(frontal < targets.size()) {
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
clique->push_back(fg[factorI]); } } }
// count the factors for this cluster to pre-allocate space
{
size_t nFactors = 0;
BOOST_FOREACH(const Index frontal, clique->frontal) {
// There may be less variables in "targets" than there really are if
// some of the highest-numbered variables do not pull in any factors.
if(frontal < targets.size())
nFactors += targets[frontal].size(); }
clique->reserve(nFactors);
}
// add the factors to this cluster
BOOST_FOREACH(const Index frontal, clique->frontal) {
if(frontal < targets.size()) {
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
clique->push_back(fg[factorI]); } } }
// recursively call the children
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
sharedClique child = distributeFactors(fg, targets, bayesChild);
clique->addChild(child);
child->parent() = clique;
}
return clique;
} else
return sharedClique();
}
// recursively call the children
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
sharedClique child = distributeFactors(fg, targets, bayesChild);
clique->addChild(child);
child->parent() = clique;
}
return clique;
} else
return sharedClique();
}
/* ************************************************************************* */
template <class FG>
pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current) const {
/* ************************************************************************* */
template <class FG>
pair<typename JunctionTree<FG>::BayesTree::sharedClique, typename FG::sharedFactor>
JunctionTree<FG>::eliminateOneClique(const boost::shared_ptr<const Clique>& current) const {
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
fg.push_back(*current); // add the local factors
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
fg.push_back(*current); // add the local factors
// receive the factors from the child and its clique point
list<typename BayesTree::sharedClique> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
eliminateOneClique(child));
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> tree_factor(
eliminateOneClique(child));
children.push_back(tree_factor.first);
fg.push_back(tree_factor.second);
}
fg.push_back(tree_factor.second);
}
// eliminate the combined factors
// warning: fg is being eliminated in-place and will contain marginal afterwards
tic("JT 2.1 VariableSlots");
VariableSlots variableSlots(fg);
// eliminate the combined factors
// warning: fg is being eliminated in-place and will contain marginal afterwards
tic("JT 2.1 VariableSlots");
VariableSlots variableSlots(fg);
toc("JT 2.1 VariableSlots");
#ifndef NDEBUG
// Debug check that the keys found in the factors match the frontal and
@ -182,29 +189,29 @@ namespace gtsam {
assert(std::equal(jointFactor->begin(), jointFactor->end(), current->separator.begin()));
tic("JT 2.4 Update tree");
// create a new clique corresponding the combined factors
typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(*fragment));
new_clique->children_ = children;
// create a new clique corresponding the combined factors
typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(*fragment));
new_clique->children_ = children;
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children)
childRoot->parent_ = new_clique;
BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children)
childRoot->parent_ = new_clique;
toc("JT 2.4 Update tree");
return make_pair(new_clique, jointFactor);
}
return make_pair(new_clique, jointFactor);
}
/* ************************************************************************* */
template <class FG>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate() const {
if(this->root()) {
tic("JT 2 eliminate");
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root());
if (ret.second->size() != 0)
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
toc("JT 2 eliminate");
return ret.first;
} else
return typename BayesTree::sharedClique();
}
/* ************************************************************************* */
template <class FG>
typename JunctionTree<FG>::BayesTree::sharedClique JunctionTree<FG>::eliminate() const {
if(this->root()) {
tic("JT 2 eliminate");
pair<typename BayesTree::sharedClique, typename FG::sharedFactor> ret = this->eliminateOneClique(this->root());
if (ret.second->size() != 0)
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
toc("JT 2 eliminate");
return ret.first;
} else
return typename BayesTree::sharedClique();
}
} //namespace gtsam

17
gtsam/inference/JunctionTree.h
View File

@ -48,6 +48,8 @@ namespace gtsam {
typedef class BayesTree<typename FG::Factor::Conditional> BayesTree;
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
// And we will frequently refer to a symbolic Bayes tree
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
@ -64,14 +66,19 @@ namespace gtsam {
std::pair<typename BayesTree::sharedClique, typename FG::sharedFactor>
eliminateOneClique(const boost::shared_ptr<const Clique>& clique) const;
public:
// constructor
JunctionTree() {
}
// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
// constructor given a factor graph and the elimination ordering
public:
/** Default constructor */
JunctionTree() {}
/** Construct from a factor graph. Computes a variable index. */
JunctionTree(const FG& fg);
/** Construct from a factor graph and a pre-computed variable index. */
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
// eliminate the factors in the subgraphs
typename BayesTree::sharedClique eliminate() const;

1
gtsam/inference/tests/testJunctionTree.cpp
View File

@ -31,6 +31,7 @@ using namespace boost::assign;
#include <gtsam/inference/ClusterTree-inl.h>
#include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
using namespace gtsam;

18
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -146,24 +146,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
return fg;
}
/* ************************************************************************* */
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 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(j, A, b, model));
result.push_back(prior);
}
return result;
}
bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
//typedef sharedFactor F ;

7
gtsam/linear/GaussianFactorGraph.h
View File

@ -151,13 +151,6 @@ namespace gtsam {
*/
void combine(const GaussianFactorGraph &lfg);
/**
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& dimensions) const;
/**
* Split a Gaussian factor graph into two, according to M
* M keeps the vertex indices of edges of A1. The others belong to A2.

6
gtsam/linear/GaussianJunctionTree.h
View File

@ -43,11 +43,15 @@ namespace gtsam {
public :
/** Default constructor */
GaussianJunctionTree() : Base() {}
// constructor
/** Constructor from a factor graph. Builds a VariableIndex. */
GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {}
/** Construct from a factor graph and a pre-computed variable index. */
GaussianJunctionTree(const GaussianFactorGraph& fg, const VariableIndex& variableIndex) : Base(fg, variableIndex) {}
// optimize the linear graph
VectorValues optimize() const;
}; // GaussianJunctionTree

24
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -31,17 +31,23 @@ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<Gaussia
Base(factorGraph) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::shared_ptr
GaussianMultifrontalSolver::Create(const FactorGraph<GaussianFactor>& factorGraph) {
return shared_ptr(new GaussianMultifrontalSolver(factorGraph));
}
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) :
Base(factorGraph) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::shared_ptr
GaussianMultifrontalSolver::update(const FactorGraph<GaussianFactor>& factorGraph) const {
// We do not yet have code written to update the junction tree, so we just
// create a new solver.
return Create(factorGraph);
GaussianMultifrontalSolver::Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) {
return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex));
}
/* ************************************************************************* */
void GaussianMultifrontalSolver::replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) {
Base::replaceFactors(factorGraph);
}
/* ************************************************************************* */
@ -51,7 +57,7 @@ BayesTree<GaussianConditional>::shared_ptr GaussianMultifrontalSolver::eliminate
/* ************************************************************************* */
VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
return VectorValues::shared_ptr(new VectorValues(junctionTree_.optimize()));
return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize()));
}
/* ************************************************************************* */

21
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -54,15 +54,28 @@ public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
* tree, which already does some of the work of elimination.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph);
/**
* Named constructor that returns a shared_ptr. This builds the junction
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
static shared_ptr Create(const FactorGraph<GaussianFactor>& factorGraph);
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Return a new solver that solves the given factor graph, which must have
@ -71,7 +84,7 @@ public:
* used in cases where the numerical values of the linear problem change,
* e.g. during iterative nonlinear optimization.
*/
shared_ptr update(const FactorGraph<GaussianFactor>& factorGraph) const;
void replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree

22
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -31,15 +31,23 @@ GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFac
Base(factorGraph) {}
/* ************************************************************************* */
GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(const FactorGraph<GaussianFactor>& factorGraph) {
return shared_ptr(new GaussianSequentialSolver(factorGraph));
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) :
Base(factorGraph) {}
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) :
Base(factorGraph, variableIndex) {}
/* ************************************************************************* */
GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(
const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) {
return shared_ptr(new GaussianSequentialSolver(factorGraph, variableIndex));
}
/* ************************************************************************* */
GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::update(const FactorGraph<GaussianFactor>& factorGraph) const {
// We do not yet have code written to update the elimination tree, so we just
// create a new solver.
return Create(factorGraph);
void GaussianSequentialSolver::replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph) {
Base::replaceFactors(factorGraph);
}
/* ************************************************************************* */
@ -52,7 +60,7 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
static const bool debug = false;
if(debug) this->factors_.print("GaussianSequentialSolver, eliminating ");
if(debug) this->factors_->print("GaussianSequentialSolver, eliminating ");
if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree ");
// Eliminate using the elimination tree

19
gtsam/linear/GaussianSequentialSolver.h
View File

@ -61,15 +61,28 @@ public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
* tree, which already does some of the work of elimination.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph);
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
static shared_ptr Create(const FactorGraph<GaussianFactor>& factorGraph);
static shared_ptr Create(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex);
/**
* Return a new solver that solves the given factor graph, which must have
@ -78,7 +91,7 @@ public:
* used in cases where the numerical values of the linear problem change,
* e.g. during iterative nonlinear optimization.
*/
shared_ptr update(const FactorGraph<GaussianFactor>& factorGraph) const;
void replaceFactors(const FactorGraph<GaussianFactor>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree

8
gtsam/linear/SubgraphSolver-inl.h
View File

@ -27,14 +27,13 @@ using namespace std;
namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES>
typename SubgraphSolver<GRAPH,LINEAR,VALUES>::shared_ptr
SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
shared_linear Ab1 = boost::make_shared<LINEAR>(),
Ab2 = boost::make_shared<LINEAR>();
if (parameters_->verbosity()) cout << "split the graph ...";
graph.split(pairs_, *Ab1, *Ab2) ;
graph->split(pairs_, *Ab1, *Ab2) ;
if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
// // Add a HardConstraint to the root, otherwise the root will be singular
@ -48,8 +47,7 @@ SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
shared_preconditioner pc = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc, parameters_) ;
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
}
template<class GRAPH, class LINEAR, class VALUES>

4
gtsam/linear/SubgraphSolver.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters()):
IterativeSolver(parameters){ initialize(G,theta0); }
SubgraphSolver(const LINEAR &GFG) {
SubgraphSolver(const typename LINEAR::shared_ptr& GFG) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
@ -71,7 +71,7 @@ namespace gtsam {
sharedParameters parameters = boost::make_shared<Parameters>()) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc) {}
shared_ptr update(const LINEAR &graph) const ;
void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() const ;
shared_ordering ordering() const { return ordering_; }

21
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -115,7 +115,7 @@ namespace gtsam {
boost::shared_ptr<L> linearized = graph_->linearize(*values_, *ordering_);
shared_solver newSolver = solver_;
if(newSolver) newSolver = newSolver->update(*linearized);
if(newSolver) newSolver->replaceFactors(linearized);
else newSolver.reset(new S(*linearized));
VectorValues delta = *newSolver->optimize();
@ -209,11 +209,24 @@ namespace gtsam {
if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
// add prior-factors
L damped = linear.add_priors(1.0/sqrt(lambda), *dimensions_);
if (verbosity >= Parameters::DAMPED) damped.print("damped");
typename L::shared_ptr damped(new L(linear));
{
double sigma = 1.0 / sqrt(lambda);
damped->reserve(damped->size() + dimensions_->size());
// for each of the variables, add a prior
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);
GaussianFactor::shared_ptr prior(new GaussianFactor(j, A, b, model));
damped->push_back(prior);
}
}
if (verbosity >= Parameters::DAMPED) damped->print("damped");
// solve
if(solver_) solver_ = solver_->update(damped);
if(solver_) solver_->replaceFactors(damped);
else solver_.reset(new S(damped));
VectorValues delta = *solver_->optimize();

2
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -71,7 +71,7 @@ namespace gtsam {
typedef boost::shared_ptr<const T> shared_values;
typedef boost::shared_ptr<const G> shared_graph;
typedef boost::shared_ptr<const Ordering> shared_ordering;
typedef boost::shared_ptr<const GS> shared_solver;
typedef boost::shared_ptr<GS> shared_solver;
typedef NonlinearOptimizationParameters Parameters;
typedef boost::shared_ptr<const Parameters> shared_parameters ;

30
tests/testGaussianFactorGraph.cpp
View File

@ -339,21 +339,21 @@ TEST( GaussianFactorGraph, eliminateAll )
// CHECK(assert_equal(expected,actual,tol));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, add_priors )
{
Ordering ordering; ordering += "l1","x1","x2";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
Matrix A = eye(2);
Vector b = zero(2);
SharedDiagonal sigma = sharedSigma(2,3.0);
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["l1"],A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x1"],A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x2"],A,b,sigma)));
CHECK(assert_equal(expected,actual));
}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, add_priors )
//{
// Ordering ordering; ordering += "l1","x1","x2";
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// Matrix A = eye(2);
// Vector b = zero(2);
// SharedDiagonal sigma = sharedSigma(2,3.0);
// expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["l1"],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x1"],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor(ordering["x2"],A,b,sigma)));
// CHECK(assert_equal(expected,actual));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, copying )