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

Updated Concurrent Smoother for changes in the base class synchronization

release/4.3a0
Stephen Williams 2013-04-11 12:42:45 +00:00
parent 0a459549f8
commit 634a4c5ef9
3 changed files with 40 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -111,13 +111,16 @@ void ConcurrentBatchSmoother::presync() {
}
/* ************************************************************************* */
void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) {
void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) {
gttic(get_summarized_factors);
// Copy the previous calculated smoother summarization factors into the output
summarizedFactors.push_back(smootherSummarization_);
// Copy the separator values into the output
separatorValues.insert(separatorValues_);
gttoc(get_summarized_factors);
}
@ -376,14 +379,14 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
smootherSummarization_.resize(0);
// 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
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<gtsam::Index> indicesToEliminate;
std::set<Index> indicesToEliminate;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) {
indicesToEliminate.insert(ordering_.at(key_value.key));
}
@ -397,7 +400,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors and store
BOOST_FOREACH(gtsam::Index index, indicesToEliminate) {
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
smootherSummarization_.push_back(marginalFactor);
@ -443,20 +446,20 @@ std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(co
const size_t m = structure.nFactors();
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
std::vector<gtsam::Index> parents(n, none);
std::vector<gtsam::Index> prevCol(m, none);
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// 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
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
gtsam::Index k = prevCol[i];
Index k = prevCol[i];
// find root r of the current tree that contains k
gtsam::Index r = k;
Index r = k;
while (parents[r] != none)
r = parents[r];
if (r != j) parents[r] = j;
@ -469,28 +472,28 @@ std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(co
}
/* ************************************************************************* */
std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBatchSmoother::EliminationForest::Create(const gtsam::GaussianFactorGraph& factorGraph, const gtsam::VariableIndex& structure) {
std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<gtsam::Index> parents(ComputeParents(structure));
std::vector<Index> parents(ComputeParents(structure));
// Number of variables
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
std::vector<shared_ptr> trees(n);
for (gtsam::Index k = 1; k <= n; k++) {
gtsam::Index j = n - k; // Start at the last variable and loop down to 0
for (Index k = 1; k <= n; k++) {
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
if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]);
}
// Hang factors in right places
BOOST_FOREACH(const sharedFactor& factor, factorGraph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
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())
trees[j]->add(factor);
}
@ -500,10 +503,10 @@ std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBa
}
/* ************************************************************************* */
ConcurrentBatchSmoother::EliminationForest::sharedFactor ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(Eliminate function) {
GaussianFactor::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// 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());
// Add all factors associated with the current node
@ -514,7 +517,7 @@ ConcurrentBatchSmoother::EliminationForest::sharedFactor ConcurrentBatchSmoother
factors.push_back(child->eliminateRecursive(function));
// 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;
}

26
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -139,7 +139,7 @@ protected:
*
* @param summarizedFactors The summarized factors for the filter branch
*/
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors);
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
/**
* Apply the new smoother factors sent by the filter, and the updated version of the filter
@ -195,31 +195,27 @@ private:
class EliminationForest {
public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
typedef gtsam::GaussianFactor Factor; ///< The factor Type
typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNet<Factor::ConditionalType> BayesNet; ///< The BayesNet
typedef gtsam::GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine
private:
typedef gtsam::FastList<sharedFactor> Factors;
typedef gtsam::FastList<shared_ptr> SubTrees;
typedef std::vector<Factor::ConditionalType::shared_ptr> Conditionals;
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
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
SubTrees subTrees_; ///< sub-trees
/** 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
* 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 */
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 */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
@ -227,7 +223,7 @@ private:
public:
/** 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 */
const SubTrees& children() const { return subTrees_; }
@ -236,10 +232,10 @@ private:
const Factors& factors() const { return factors_; }
/** 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 */
sharedFactor eliminateRecursive(Eliminate function);
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);

10
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -54,8 +54,8 @@ public:
void presync() {
ConcurrentBatchSmoother::presync();
};
void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) {
ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors);
void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) {
ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors, separatorValues);
};
void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) {
ConcurrentBatchSmoother::synchronize(smootherFactors, smootherValues, summarizedFactors, rootValues);
@ -573,8 +573,9 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
// Perform the synchronization procedure
NonlinearFactorGraph actualSmootherSummarization;
Values actualSeparatorValues;
smoother.presync();
smoother.getSummarizedFactors(actualSmootherSummarization);
smoother.getSummarizedFactors(actualSmootherSummarization, actualSeparatorValues);
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
smoother.postsync();
@ -663,10 +664,11 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
// Now perform a second synchronization to test the smoother-calculated summarization
actualSmootherSummarization.resize(0);
actualSeparatorValues.clear();
smootherFactors.resize(0);
smootherValues.clear();
smoother.presync();
smoother.getSummarizedFactors(actualSmootherSummarization);
smoother.getSummarizedFactors(actualSmootherSummarization, actualSeparatorValues);
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
smoother.postsync();