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

Started re-implementing the synchronization functions for the Concurrent Filter

release/4.3a0
Stephen Williams 2013-04-11 12:43:42 +00:00
parent 634a4c5ef9
commit 04d595dec1
3 changed files with 130 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -105,10 +105,43 @@ void ConcurrentBatchFilter::presync() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
gttic(synchronize); gttic(synchronize);
// Remove the previous smoother summarization
removeFactors(smootherSummarizationSlots_);
// Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother,
// and all of the filter factors. This is the set of factors on the filter side since the smoother started
// its previous update cycle.
NonlinearFactorGraph filterGraph;
filterGraph.push_back(factors_);
filterGraph.push_back(smootherFactors_);
filterGraph.push_back(summarizedFactors);
Values filterValues;
filterValues.insert(theta_);
filterValues.insert(smootherValues_);
filterValues.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother
// Optimize this graph using a modified version of L-M
// TODO:
// Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out
// all of the filter variables that are not part of the new separator. This filter summarization will then be
// sent to the smoother.
Ordering filterOrdering;
std::vector<Key> filterKeys;
filterSummarization_ = marginalize(filterGraph, filterValues, filterOrdering, filterKeys, parameters_.getEliminationFunction());
// Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out
// all of the smoother variables that are not part of the new separator. This smoother summarization will be
// stored locally for use in the filter
Ordering smootherOrdering;
std::vector<Key> smootherKeys;
smootherSummarizationSlots_ = insertFactors( marginalize(filterGraph, filterValues, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) );
gttoc(synchronize); gttoc(synchronize);
} }
@ -173,7 +206,7 @@ std::vector<size_t> ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchFilter::removeFactors(const std::set<size_t>& slots) { void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors); gttic(remove_factors);
@ -345,24 +378,24 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Note: It is assumed the ordering already has these keys first // Note: It is assumed the ordering already has these keys first
{ {
// Use the variable Index to mark the factors that will be marginalized // Use the variable Index to mark the factors that will be marginalized
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
const gtsam::FastList<size_t>& slots = variableIndex_[ordering_.at(key)]; const FastList<size_t>& slots = variableIndex_[ordering_.at(key)];
removedFactorSlots.insert(slots.begin(), slots.end()); removedFactorSlots.insert(slots.begin(), slots.end());
} }
// Create the linear factor graph // Create the linear factor graph
gtsam::GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Construct an elimination tree to perform sparse elimination // Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex_) ); std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex_) );
// This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically
// Find the subset of nodes/keys that must be eliminated // Find the subset of nodes/keys that must be eliminated
std::set<gtsam::Index> indicesToEliminate; std::set<Index> indicesToEliminate;
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
indicesToEliminate.insert(ordering_.at(key)); indicesToEliminate.insert(ordering_.at(key));
} }
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key)));
} }
@ -370,12 +403,12 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Convert the marginal factors into Linear Container Factors // Convert the marginal factors into Linear Container Factors
// Add the marginal factor variables to the separator // Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(gtsam::Index index, indicesToEliminate) { BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
marginalFactors.push_back(marginalFactor); marginalFactors.push_back(marginalFactor);
// Add the keys associated with the marginal factor to the separator values // Add the keys associated with the marginal factor to the separator values
BOOST_FOREACH(gtsam::Key key, *marginalFactor) { BOOST_FOREACH(Key key, *marginalFactor) {
if(!separatorValues_.exists(key)) { if(!separatorValues_.exists(key)) {
separatorValues_.insert(key, theta_.at(key)); separatorValues_.insert(key, theta_.at(key));
} }
@ -403,7 +436,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
} }
// Add the linearization point of the moved variables to the smoother cache // Add the linearization point of the moved variables to the smoother cache
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
smootherValues_.insert(key, theta_.at(key)); smootherValues_.insert(key, theta_.at(key));
} }
} }
@ -411,10 +444,11 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Remove the marginalized variables and factors from the filter // Remove the marginalized variables and factors from the filter
{ {
// Remove marginalized factors from the factor graph // Remove marginalized factors from the factor graph
removeFactors(removedFactorSlots); std::vector<size_t> slots(removedFactorSlots.begin(), removedFactorSlots.end());
removeFactors(slots);
// Remove marginalized keys from values (and separator) // Remove marginalized keys from values (and separator)
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
theta_.erase(key); theta_.erase(key);
if(separatorValues_.exists(key)) { if(separatorValues_.exists(key)) {
separatorValues_.erase(key); separatorValues_.erase(key);
@ -423,8 +457,8 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Permute the ordering such that the removed keys are at the end. // Permute the ordering such that the removed keys are at the end.
// This is a prerequisite for removing them from several structures // This is a prerequisite for removing them from several structures
std::vector<gtsam::Index> toBack; std::vector<Index> toBack;
BOOST_FOREACH(gtsam::Key key, keysToMove) { BOOST_FOREACH(Key key, keysToMove) {
toBack.push_back(ordering_.at(key)); toBack.push_back(ordering_.at(key));
} }
Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size());
@ -439,6 +473,48 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
} }
} }
/* ************************************************************************* */
NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGraph& graph, const Values& values,
const Ordering& ordering, const std::vector<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) {
// This function returns marginal factors (in the form of LinearContainerFactors) that result from
// marginalizing out the selected variables.
// Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys')
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph);
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a forest. Only the top-most node/index of each tree needs to be eliminated; all of the children will be eliminated automatically
// Find the subset of nodes/keys that must be eliminated
std::set<Index> indicesToEliminate;
BOOST_FOREACH(Key key, marginalizeKeys) {
indicesToEliminate.insert(ordering.at(key));
}
BOOST_FOREACH(Key key, marginalizeKeys) {
EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering.at(key)));
}
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors
// Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
return marginalFactors;
}
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent, const KeyFormatter& keyFormatter) { const std::string& indent, const KeyFormatter& keyFormatter) {
@ -479,20 +555,20 @@ std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(cons
const size_t m = structure.nFactors(); const size_t m = structure.nFactors();
const size_t n = structure.size(); const size_t n = structure.size();
static const gtsam::Index none = std::numeric_limits<gtsam::Index>::max(); static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns // Allocate result parent vector and vector of last factor columns
std::vector<gtsam::Index> parents(n, none); std::vector<Index> parents(n, none);
std::vector<gtsam::Index> prevCol(m, none); std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do // for column j \in 1 to n do
for (gtsam::Index j = 0; j < n; j++) { for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do // for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, structure[j]) { BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) { if (prevCol[i] != none) {
gtsam::Index k = prevCol[i]; Index k = prevCol[i];
// find root r of the current tree that contains k // find root r of the current tree that contains k
gtsam::Index r = k; Index r = k;
while (parents[r] != none) while (parents[r] != none)
r = parents[r]; r = parents[r];
if (r != j) parents[r] = j; if (r != j) parents[r] = j;
@ -505,28 +581,28 @@ std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(cons
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatchFilter::EliminationForest::Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure) { std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure // Compute the tree structure
std::vector<gtsam::Index> parents(ComputeParents(structure)); std::vector<Index> parents(ComputeParents(structure));
// Number of variables // Number of variables
const size_t n = structure.size(); const size_t n = structure.size();
static const gtsam::Index none = std::numeric_limits<gtsam::Index>::max(); static const Index none = std::numeric_limits<Index>::max();
// Create tree structure // Create tree structure
std::vector<shared_ptr> trees(n); std::vector<shared_ptr> trees(n);
for (gtsam::Index k = 1; k <= n; k++) { for (Index k = 1; k <= n; k++) {
gtsam::Index j = n - k; // Start at the last variable and loop down to 0 Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationForest(j)); // Create a new node on this variable trees[j].reset(new EliminationForest(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]); trees[parents[j]]->add(trees[j]);
} }
// Hang factors in right places // Hang factors in right places
BOOST_FOREACH(const sharedFactor& factor, factorGraph) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) { if(factor && factor->size() > 0) {
gtsam::Index j = *std::min_element(factor->begin(), factor->end()); Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size()) if(j < structure.size())
trees[j]->add(factor); trees[j]->add(factor);
} }
@ -536,10 +612,10 @@ std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatc
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConcurrentBatchFilter::EliminationForest::sharedFactor ConcurrentBatchFilter::EliminationForest::eliminateRecursive(Eliminate function) { GaussianFactor::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space // Create the list of factors to be eliminated, initially empty, and reserve space
gtsam::GaussianFactorGraph factors; GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size()); factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node // Add all factors associated with the current node
@ -550,7 +626,7 @@ ConcurrentBatchFilter::EliminationForest::sharedFactor ConcurrentBatchFilter::El
factors.push_back(child->eliminateRecursive(function)); factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor // Combine all factors (from this node and from subtrees) into a joint factor
gtsam::GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second; return eliminated.second;
} }

38
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -157,7 +157,7 @@ protected:
* *
* @param summarizedFactors An updated version of the smoother branch summarized factors * @param summarizedFactors An updated version of the smoother branch summarized factors
*/ */
virtual void synchronize(const NonlinearFactorGraph& summarizedFactors); virtual void synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
/** /**
* Perform any required operations after the synchronization process finishes. * Perform any required operations after the synchronization process finishes.
@ -179,7 +179,7 @@ private:
* *
* @param slots The slots in the factor graph that should be deleted * @param slots The slots in the factor graph that should be deleted
*/ */
void removeFactors(const std::set<size_t>& slots); void removeFactors(const std::vector<size_t>& slots);
/** Use colamd to update into an efficient ordering */ /** Use colamd to update into an efficient ordering */
void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none); void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
@ -192,6 +192,12 @@ private:
*/ */
void marginalize(const FastList<Key>& keysToMove); void marginalize(const FastList<Key>& keysToMove);
/** Marginalize out the set of requested variables from the filter, caching them for the smoother
* This effectively moves the separator.
*/
static NonlinearFactorGraph marginalize(const NonlinearFactorGraph& graph, const Values& values,
const Ordering& ordering, const std::vector<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function = EliminateQR);
/** Print just the nonlinear keys in a nonlinear factor */ /** Print just the nonlinear keys in a nonlinear factor */
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -204,31 +210,31 @@ private:
class EliminationForest { class EliminationForest {
public: public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
typedef gtsam::GaussianFactor Factor; ///< The factor Type // typedef GaussianFactor Factor; ///< The factor Type
typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor // typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNet<Factor::ConditionalType> BayesNet; ///< The BayesNet // typedef BayesNet<Factor::ConditionalType> BayesNet; ///< The BayesNet
typedef gtsam::GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine // typedef GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine
private: private:
typedef gtsam::FastList<sharedFactor> Factors; typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef gtsam::FastList<shared_ptr> SubTrees; typedef FastList<shared_ptr> SubTrees;
typedef std::vector<Factor::ConditionalType::shared_ptr> Conditionals; typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
gtsam::Index key_; ///< index associated with root Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees SubTrees subTrees_; ///< sub-trees
/** default constructor, private, as you should use Create below */ /** default constructor, private, as you should use Create below */
EliminationForest(gtsam::Index key = 0) : key_(key) {} EliminationForest(Index key = 0) : key_(key) {}
/** /**
* Static internal function to build a vector of parent pointers using the * Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT. * algorithm of Gilbert et al., 2001, BIT.
*/ */
static std::vector<gtsam::Index> ComputeParents(const gtsam::VariableIndex& structure); static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */ /** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); } void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */ /** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); } void add(const shared_ptr& child) { subTrees_.push_back(child); }
@ -236,7 +242,7 @@ private:
public: public:
/** return the key associated with this tree node */ /** return the key associated with this tree node */
gtsam::Index key() const { return key_; } Index key() const { return key_; }
/** return the const reference of children */ /** return the const reference of children */
const SubTrees& children() const { return subTrees_; } const SubTrees& children() const { return subTrees_; }
@ -245,10 +251,10 @@ private:
const Factors& factors() const { return factors_; } const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */ /** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure); static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */ /** Recursive routine that eliminates the factors arranged in an elimination tree */
sharedFactor eliminateRecursive(Eliminate function); GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */ /** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree); static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -52,7 +52,7 @@ public:
void presync() { ConcurrentBatchFilter::presync(); }; void presync() { ConcurrentBatchFilter::presync(); };
void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { ConcurrentBatchFilter::getSummarizedFactors(summarizedFactors, rootValues); }; void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { ConcurrentBatchFilter::getSummarizedFactors(summarizedFactors, rootValues); };
void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { ConcurrentBatchFilter::getSmootherFactors(smootherFactors, smootherValues); }; void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { ConcurrentBatchFilter::getSmootherFactors(smootherFactors, smootherValues); };
void synchronize(const NonlinearFactorGraph& summarizedFactors) { ConcurrentBatchFilter::synchronize(summarizedFactors); }; void synchronize(const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) { ConcurrentBatchFilter::synchronize(summarizedFactors, separatorValues); };
void postsync() { ConcurrentBatchFilter::postsync(); }; void postsync() { ConcurrentBatchFilter::postsync(); };
}; };