Created Concurrent Incremental Filter and Smoother
Stephen Williams
parent
663c598591
commit
f656e93202
|
|
@ -0,0 +1,410 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ConcurrentIncrementalFilter.cpp
|
||||
* @brief An iSAM2-based Filter that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
isam2_.print("");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol) const {
|
||||
const ConcurrentIncrementalFilter* filter = dynamic_cast<const ConcurrentIncrementalFilter*>(&rhs);
|
||||
return filter
|
||||
&& isam2_.equals(filter->isam2_)
|
||||
&& (currentSmootherSummarizationSlots_.size() == filter->currentSmootherSummarizationSlots_.size())
|
||||
&& std::equal(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), filter->currentSmootherSummarizationSlots_.begin())
|
||||
&& previousSmootherSummarization_.equals(filter->previousSmootherSummarization_)
|
||||
&& smootherShortcut_.equals(filter->smootherShortcut_)
|
||||
&& smootherFactors_.equals(filter->smootherFactors_)
|
||||
&& smootherValues_.equals(filter->smootherValues_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const boost::optional<FastList<Key> >& keysToMove) {
|
||||
|
||||
gttic(update);
|
||||
|
||||
// const bool debug = ISDEBUG("ConcurrentIncrementalFilter update");
|
||||
const bool debug = false;
|
||||
|
||||
if(debug) std::cout << "ConcurrentIncrementalFilter::update Begin" << std::endl;
|
||||
|
||||
// Create the return result meta-data
|
||||
Result result;
|
||||
|
||||
// We do not need to remove any factors at this time
|
||||
gtsam::FastVector<size_t> removedFactors;
|
||||
|
||||
// Generate ordering constraints that force the 'keys to move' to the end
|
||||
boost::optional<gtsam::FastMap<gtsam::Key,int> > orderingConstraints = boost::none;
|
||||
if(keysToMove && keysToMove->size() > 0) {
|
||||
orderingConstraints = gtsam::FastMap<gtsam::Key,int>();
|
||||
int group = 1;
|
||||
// Set all existing variables to Group1
|
||||
if(isam2_.getLinearizationPoint().size() > 0) {
|
||||
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
|
||||
orderingConstraints->operator[](key_value.key) = group;
|
||||
}
|
||||
++group;
|
||||
}
|
||||
// Assign new variables to the root
|
||||
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, newTheta) {
|
||||
orderingConstraints->operator[](key_value.key) = group;
|
||||
}
|
||||
// Set marginalizable variables to Group0
|
||||
BOOST_FOREACH(gtsam::Key key, *keysToMove){
|
||||
orderingConstraints->operator[](key) = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Create the set of linear keys that iSAM2 should hold constant
|
||||
// iSAM2 takes care of this for us; no need to specify additional noRelin keys
|
||||
boost::optional<gtsam::FastList<gtsam::Key> > noRelinKeys = boost::none;
|
||||
|
||||
// Mark additional keys between the 'keys to move' and the leaves
|
||||
boost::optional<FastList<Key> > additionalKeys = boost::none;
|
||||
if(keysToMove && keysToMove->size() > 0) {
|
||||
std::set<Key> markedKeys;
|
||||
BOOST_FOREACH(gtsam::Key key, *keysToMove) {
|
||||
if(isam2_.getLinearizationPoint().exists(key)) {
|
||||
Index index = isam2_.getOrdering().at(key);
|
||||
ISAM2Clique::shared_ptr clique = isam2_[index];
|
||||
GaussianConditional::const_iterator index_iter = clique->conditional()->begin();
|
||||
while(*index_iter != index) {
|
||||
markedKeys.insert(isam2_.getOrdering().key(*index_iter));
|
||||
++index_iter;
|
||||
}
|
||||
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) {
|
||||
RecursiveMarkAffectedKeys(index, child, isam2_.getOrdering(), markedKeys);
|
||||
}
|
||||
}
|
||||
}
|
||||
additionalKeys = FastList<Key>(markedKeys.begin(), markedKeys.end());
|
||||
}
|
||||
|
||||
// Update the system using iSAM2
|
||||
gttic(isam2);
|
||||
gtsam::ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
|
||||
gttoc(isam2);
|
||||
|
||||
if(keysToMove && keysToMove->size() > 0) {
|
||||
|
||||
gttic(cache_smoother_factors);
|
||||
// Find the set of factors that will be removed
|
||||
std::vector<size_t> removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
|
||||
// Cache these factors for later transmission to the smoother
|
||||
NonlinearFactorGraph removedFactors;
|
||||
BOOST_FOREACH(size_t slot, removedFactorSlots) {
|
||||
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
|
||||
if(factor) {
|
||||
smootherFactors_.push_back(factor);
|
||||
removedFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
// Cache removed values for later transmission to the smoother
|
||||
BOOST_FOREACH(Key key, *keysToMove) {
|
||||
smootherValues_.insert(key, isam2_.getLinearizationPoint().at(key));
|
||||
}
|
||||
gttoc(cache_smoother_factors);
|
||||
|
||||
gttic(marginalize);
|
||||
std::vector<size_t> marginalFactorsIndices;
|
||||
std::vector<size_t> deletedFactorsIndices;
|
||||
isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
|
||||
currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
|
||||
BOOST_FOREACH(size_t index, deletedFactorsIndices) {
|
||||
currentSmootherSummarizationSlots_.erase(std::remove(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), index), currentSmootherSummarizationSlots_.end());
|
||||
}
|
||||
gttoc(marginalize);
|
||||
|
||||
// Calculate a new shortcut
|
||||
updateShortcut(removedFactors);
|
||||
}
|
||||
|
||||
// Extract the ConcurrentIncrementalFilter::Result information
|
||||
result.iterations = 1;
|
||||
result.linearVariables = isam2_.getFixedVariables().size();
|
||||
result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables;
|
||||
result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate());
|
||||
|
||||
if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl;
|
||||
|
||||
gttoc(update);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::presync() {
|
||||
|
||||
gttic(presync);
|
||||
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
|
||||
|
||||
gttic(synchronize);
|
||||
// const bool debug = ISDEBUG("ConcurrentIncrementalFilter synchronize");
|
||||
const bool debug = false;
|
||||
|
||||
if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize Begin" << std::endl;
|
||||
|
||||
// Update the smoother summarization on the old separator
|
||||
previousSmootherSummarization_ = smootherSummarization;
|
||||
|
||||
// Find the set of new separator keys
|
||||
const FastSet<Key>& newSeparatorKeys = isam2_.getFixedVariables();
|
||||
|
||||
// Use the shortcut to calculate an updated marginal on the current separator
|
||||
// Combine just the shortcut and the previousSmootherSummarization
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(previousSmootherSummarization_);
|
||||
graph.push_back(smootherShortcut_);
|
||||
Values values;
|
||||
values.insert(smootherSummarizationValues);
|
||||
BOOST_FOREACH(Key key, newSeparatorKeys) {
|
||||
if(!values.exists(key)) {
|
||||
values.insert(key, isam2_.getLinearizationPoint().at(key));
|
||||
}
|
||||
}
|
||||
|
||||
// Force iSAM2 not to relinearize anything during this iteration
|
||||
FastList<Key> noRelinKeys;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
|
||||
noRelinKeys.push_back(key_value.key);
|
||||
}
|
||||
|
||||
// Calculate the summarized factor on just the new separator keys
|
||||
NonlinearFactorGraph currentSmootherSummarization = internal::calculateMarginalFactors(graph, values, newSeparatorKeys,
|
||||
isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR);
|
||||
|
||||
// Remove the old factors on the separator and insert the new ones
|
||||
FastVector<size_t> removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
|
||||
ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false);
|
||||
currentSmootherSummarizationSlots_ = result.newFactorsIndices;
|
||||
|
||||
// Set the previous smoother summarization to the current smoother summarization and clear the smoother shortcut
|
||||
previousSmootherSummarization_ = currentSmootherSummarization;
|
||||
smootherShortcut_.resize(0);
|
||||
|
||||
if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize End" << std::endl;
|
||||
|
||||
gttoc(synchronize);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) {
|
||||
|
||||
gttic(get_summarized_factors);
|
||||
|
||||
// Calculate the current filter summarization
|
||||
filterSummarization = calculateFilterSummarization();
|
||||
|
||||
// Copy the current separator values into the output
|
||||
BOOST_FOREACH(Key key, isam2_.getFixedVariables()) {
|
||||
filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key));
|
||||
}
|
||||
|
||||
gttoc(get_summarized_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) {
|
||||
|
||||
gttic(get_smoother_factors);
|
||||
|
||||
// Copy the previous calculated smoother summarization factors into the output
|
||||
smootherFactors.push_back(smootherFactors_);
|
||||
smootherValues.insert(smootherValues_);
|
||||
|
||||
gttoc(get_smoother_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::postsync() {
|
||||
|
||||
gttic(postsync);
|
||||
|
||||
// Clear out the factors and values that were just sent to the smoother
|
||||
smootherFactors_.resize(0);
|
||||
smootherValues_.clear();
|
||||
|
||||
gttoc(postsync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys) {
|
||||
|
||||
// Check if the separator keys of the current clique contain the specified key
|
||||
if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) {
|
||||
|
||||
// Mark the frontal keys of the current clique
|
||||
BOOST_FOREACH(Index idx, clique->conditional()->frontals()) {
|
||||
additionalKeys.insert(ordering.key(idx));
|
||||
}
|
||||
|
||||
// Recursively mark all of the children
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
|
||||
RecursiveMarkAffectedKeys(index, child, ordering, additionalKeys);
|
||||
}
|
||||
}
|
||||
// If the key was not found in the separator/parents, then none of its children can have it either
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore) {
|
||||
|
||||
// Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
|
||||
std::vector<size_t> removedFactorSlots;
|
||||
const VariableIndex& variableIndex = isam2.getVariableIndex();
|
||||
BOOST_FOREACH(Key key, keys) {
|
||||
const FastList<size_t>& slots = variableIndex[isam2.getOrdering().at(key)];
|
||||
removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
|
||||
}
|
||||
|
||||
// Sort and remove duplicates
|
||||
std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
|
||||
removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
|
||||
|
||||
// Remove any linear/marginal factor that made it into the set
|
||||
BOOST_FOREACH(size_t index, factorsToIgnore) {
|
||||
removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
|
||||
}
|
||||
|
||||
return removedFactorSlots;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
// TODO: Make this a static function
|
||||
void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) {
|
||||
|
||||
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
|
||||
FastSet<Key> shortcutKeys;
|
||||
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
|
||||
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
|
||||
if(factor) {
|
||||
shortcutKeys.insert(factor->begin(), factor->end());
|
||||
}
|
||||
}
|
||||
BOOST_FOREACH(Key key, previousSmootherSummarization_.keys()) {
|
||||
shortcutKeys.insert(key);
|
||||
}
|
||||
|
||||
// Combine the previous shortcut factor with all of the new factors being sent to the smoother
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(removedFactors);
|
||||
graph.push_back(smootherShortcut_);
|
||||
Values values;
|
||||
values.insert(smootherValues_);
|
||||
values.insert(isam2_.getLinearizationPoint());
|
||||
// Calculate the summarized factor on the shortcut keys
|
||||
smootherShortcut_ = internal::calculateMarginalFactors(graph, values, shortcutKeys,
|
||||
isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: Make this a static function
|
||||
NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() const {
|
||||
|
||||
// The filter summarization factors are the resulting marginal factors on the separator
|
||||
// variables that result from marginalizing out all of the other variables
|
||||
|
||||
// Find the set of current separator keys
|
||||
const FastSet<Key>& separatorKeys = isam2_.getFixedVariables();
|
||||
|
||||
// Find all cliques that contain any separator variables
|
||||
std::set<ISAM2Clique::shared_ptr> separatorCliques;
|
||||
BOOST_FOREACH(Key key, separatorKeys) {
|
||||
Index index = isam2_.getOrdering().at(key);
|
||||
ISAM2Clique::shared_ptr clique = isam2_[index];
|
||||
separatorCliques.insert( clique );
|
||||
}
|
||||
|
||||
// Create the set of clique keys
|
||||
std::vector<Index> cliqueIndices;
|
||||
std::vector<Key> cliqueKeys;
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
BOOST_FOREACH(Index index, clique->conditional()->frontals()) {
|
||||
cliqueIndices.push_back(index);
|
||||
cliqueKeys.push_back(isam2_.getOrdering().key(index));
|
||||
}
|
||||
}
|
||||
std::sort(cliqueIndices.begin(), cliqueIndices.end());
|
||||
std::sort(cliqueKeys.begin(), cliqueKeys.end());
|
||||
|
||||
// Gather all factors that involve only clique keys
|
||||
std::set<size_t> cliqueFactorSlots;
|
||||
BOOST_FOREACH(Index index, cliqueIndices) {
|
||||
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) {
|
||||
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
|
||||
if(factor) {
|
||||
std::set<Key> factorKeys(factor->begin(), factor->end());
|
||||
if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
|
||||
cliqueFactorSlots.insert(slot);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Remove any factor included in the current smoother summarization
|
||||
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
|
||||
cliqueFactorSlots.erase(slot);
|
||||
}
|
||||
|
||||
// Create a factor graph from the identified factors
|
||||
NonlinearFactorGraph graph;
|
||||
BOOST_FOREACH(size_t slot, cliqueFactorSlots) {
|
||||
graph.push_back(isam2_.getFactorsUnsafe().at(slot));
|
||||
}
|
||||
|
||||
// Find the set of children of the separator cliques
|
||||
std::set<ISAM2Clique::shared_ptr> childCliques;
|
||||
// Add all of the children
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
childCliques.insert(clique->children().begin(), clique->children().end());
|
||||
}
|
||||
// Remove any separator cliques that were added because they were children of other separator cliques
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
childCliques.erase(clique);
|
||||
}
|
||||
|
||||
// Augment the factor graph with cached factors from the children
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) {
|
||||
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint()));
|
||||
graph.push_back( factor );
|
||||
}
|
||||
|
||||
// Calculate the marginal factors on the separator
|
||||
NonlinearFactorGraph filterSummarization = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys,
|
||||
isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR);
|
||||
|
||||
return filterSummarization;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ConcurrentBatchFilter.h
|
||||
* @brief An iSAM2-based Filter that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An iSAM2-based Batch Filter that implements the Concurrent Filtering and Smoother interface.
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual ConcurrentFilter {
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<ConcurrentIncrementalFilter> shared_ptr;
|
||||
typedef ConcurrentFilter Base; ///< typedef for base class
|
||||
|
||||
/** Meta information returned about the update */
|
||||
struct Result {
|
||||
size_t iterations; ///< The number of optimizer iterations performed
|
||||
size_t nonlinearVariables; ///< The number of variables that can be relinearized
|
||||
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
double error; ///< The final factor graph error
|
||||
|
||||
/// Constructor
|
||||
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
|
||||
|
||||
/// Getter methods
|
||||
size_t getIterations() const { return iterations; }
|
||||
size_t getNonlinearVariables() const { return nonlinearVariables; }
|
||||
size_t getLinearVariables() const { return linearVariables; }
|
||||
double getError() const { return error; }
|
||||
};
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentIncrementalFilter(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentIncrementalFilter() {};
|
||||
|
||||
/** Implement a GTSAM standard 'print' function */
|
||||
virtual void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Check if two Concurrent Filters are equal */
|
||||
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
|
||||
|
||||
/** Access the current set of factors */
|
||||
const NonlinearFactorGraph& getFactors() const {
|
||||
return isam2_.getFactorsUnsafe();
|
||||
}
|
||||
|
||||
/** Access the current linearization point */
|
||||
const Values& getLinearizationPoint() const {
|
||||
return isam2_.getLinearizationPoint();
|
||||
}
|
||||
|
||||
/** Access the current ordering */
|
||||
const Ordering& getOrdering() const {
|
||||
return isam2_.getOrdering();
|
||||
}
|
||||
|
||||
/** Access the current set of deltas to the linearization point */
|
||||
const VectorValues& getDelta() const {
|
||||
return isam2_.getDelta();
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of all variables and return a full Values structure.
|
||||
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
|
||||
*/
|
||||
Values calculateEstimate() const {
|
||||
return isam2_.calculateEstimate();
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
* calling the no-argument version of calculateEstimate if only specific variables are needed.
|
||||
* @param key
|
||||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(Key key) const {
|
||||
return isam2_.calculateEstimate<VALUE>(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors and variables to the filter.
|
||||
*
|
||||
* Add new measurements, and optionally new variables, to the filter.
|
||||
* This runs a full update step of the derived filter algorithm
|
||||
*
|
||||
* @param newFactors The new factors to be added to the smoother
|
||||
* @param newTheta Initialization points for new variables to be added to the filter
|
||||
* You must include here all new variables occurring in newFactors that were not already
|
||||
* in the filter.
|
||||
* @param keysToMove An optional set of keys to move from the filter to the smoother
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
|
||||
const boost::optional<FastList<Key> >& keysToMove = boost::none);
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync();
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the filter branch summarization
|
||||
* needed by the smoother. Also, linearization points for the new root clique must be provided.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
* @param rootValues The linearization points of the root clique variables
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues);
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors being sent to the smoother from the filter. These
|
||||
* may be original nonlinear factors, or factors encoding a summarization of the filter information.
|
||||
* The specifics will be implementation-specific for a given filter.
|
||||
*
|
||||
* @param smootherFactors The new factors to be added to the smoother
|
||||
* @param smootherValues The linearization points of any new variables
|
||||
*/
|
||||
virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues);
|
||||
|
||||
/**
|
||||
* Apply the updated version of the smoother branch summarized factors.
|
||||
*
|
||||
* @param summarizedFactors An updated version of the smoother branch summarized factors
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues);
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void postsync();
|
||||
|
||||
protected:
|
||||
|
||||
ISAM2 isam2_; ///< The iSAM2 inference engine
|
||||
|
||||
// ???
|
||||
NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization
|
||||
std::vector<size_t> currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
|
||||
NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
|
||||
|
||||
// Storage for information to be sent to the smoother
|
||||
NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother
|
||||
Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
|
||||
|
||||
private:
|
||||
|
||||
/** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */
|
||||
static void RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys);
|
||||
|
||||
/** Find the set of iSAM2 factors adjacent to 'keys' */
|
||||
static std::vector<size_t> FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore);
|
||||
|
||||
/** Update the shortcut marginal between the current separator keys and the previous separator keys */
|
||||
// TODO: Make this a static function
|
||||
void updateShortcut(const NonlinearFactorGraph& removedFactors);
|
||||
|
||||
/** Calculate marginal factors on the current separator variables using just the information in the filter */
|
||||
// TODO: Make this a static function
|
||||
NonlinearFactorGraph calculateFilterSummarization() const;
|
||||
|
||||
}; // ConcurrentBatchFilter
|
||||
|
||||
/// Typedef for Matlab wrapping
|
||||
typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult;
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,253 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ConcurrentIncrementalSmoother.cpp
|
||||
* @brief An iSAM2-based Smoother that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
isam2_.print("");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double tol) const {
|
||||
const ConcurrentIncrementalSmoother* smoother = dynamic_cast<const ConcurrentIncrementalSmoother*>(&rhs);
|
||||
return smoother
|
||||
&& isam2_.equals(smoother->isam2_)
|
||||
&& smootherFactors_.equals(smoother->smootherFactors_)
|
||||
&& smootherValues_.equals(smoother->smootherValues_)
|
||||
&& filterSummarizationFactors_.equals(smoother->filterSummarizationFactors_)
|
||||
&& separatorValues_.equals(smoother->separatorValues_)
|
||||
&& (filterSummarizationSlots_.size() == smoother->filterSummarizationSlots_.size())
|
||||
&& std::equal(filterSummarizationSlots_.begin(), filterSummarizationSlots_.end(), smoother->filterSummarizationSlots_.begin())
|
||||
&& smootherSummarization_.equals(smoother->smootherSummarization_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) {
|
||||
|
||||
gttic(update);
|
||||
|
||||
// Create the return result meta-data
|
||||
Result result;
|
||||
|
||||
// Constrain the separator keys to remain in the root
|
||||
// Also, mark the separator keys as fixed linearization points
|
||||
FastMap<Key,int> constrainedKeys;
|
||||
FastList<Key> noRelinKeys;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
constrainedKeys[key_value.key] = 1;
|
||||
noRelinKeys.push_back(key_value.key);
|
||||
}
|
||||
|
||||
// Use iSAM2 to perform an update
|
||||
gtsam::ISAM2Result isam2Result;
|
||||
if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) {
|
||||
if(synchronizationUpdatesAvailable_) {
|
||||
// Augment any new factors/values with the cached data from the last synchronization
|
||||
NonlinearFactorGraph graph(newFactors);
|
||||
graph.push_back(smootherFactors_);
|
||||
graph.push_back(filterSummarizationFactors_);
|
||||
Values values(newTheta);
|
||||
// Unfortunately, we must be careful here, as some of the smoother values
|
||||
// and/or separator values may have been added previously
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) {
|
||||
if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
|
||||
values.insert(key_value.key, smootherValues_.at(key_value.key));
|
||||
}
|
||||
}
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
|
||||
values.insert(key_value.key, separatorValues_.at(key_value.key));
|
||||
}
|
||||
}
|
||||
|
||||
// Update the system using iSAM2
|
||||
isam2Result = isam2_.update(graph, values, filterSummarizationSlots_, constrainedKeys, noRelinKeys);
|
||||
|
||||
// Clear out the cache and update the filter summarization slots
|
||||
smootherFactors_.resize(0);
|
||||
smootherValues_.clear();
|
||||
filterSummarizationSlots_.clear();
|
||||
filterSummarizationSlots_.insert(filterSummarizationSlots_.end(),
|
||||
isam2Result.newFactorsIndices.end()-filterSummarizationFactors_.size(), isam2Result.newFactorsIndices.end());
|
||||
filterSummarizationFactors_.resize(0);
|
||||
synchronizationUpdatesAvailable_ = false;
|
||||
} else {
|
||||
// Update the system using iSAM2
|
||||
isam2Result = isam2_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys, noRelinKeys);
|
||||
}
|
||||
}
|
||||
|
||||
// Extract the ConcurrentIncrementalSmoother::Result information
|
||||
result.iterations = 1;
|
||||
result.linearVariables = separatorValues_.size();
|
||||
result.nonlinearVariables = isam2_.getLinearizationPoint().size() - separatorValues_.size();
|
||||
result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate());
|
||||
|
||||
// Calculate the marginal on the separator from the smoother factors
|
||||
if(separatorValues_.size() > 0) {
|
||||
gttic(presync);
|
||||
updateSmootherSummarization();
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
gttoc(update);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::presync() {
|
||||
|
||||
gttic(presync);
|
||||
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
|
||||
const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
|
||||
|
||||
gttic(synchronize);
|
||||
|
||||
// Store the new smoother factors and values for addition during the next update call
|
||||
smootherFactors_ = smootherFactors;
|
||||
smootherValues_ = smootherValues;
|
||||
|
||||
// Store the new filter summarization and separator, to be replaced during the next update call
|
||||
filterSummarizationFactors_ = summarizedFactors;
|
||||
separatorValues_ = separatorValues;
|
||||
|
||||
// Flag the next smoother update to include the synchronization data
|
||||
synchronizationUpdatesAvailable_ = true;
|
||||
|
||||
gttoc(synchronize);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::postsync() {
|
||||
|
||||
gttic(postsync);
|
||||
|
||||
gttoc(postsync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
|
||||
|
||||
// The smoother summarization factors are the resulting marginal factors on the separator
|
||||
// variables that result from marginalizing out all of the other variables
|
||||
// These marginal factors will be cached for later transmission to the filter using
|
||||
// linear container factors
|
||||
|
||||
// Find all cliques that contain any separator variables
|
||||
std::set<ISAM2Clique::shared_ptr> separatorCliques;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
Index index = isam2_.getOrdering().at(key_value.key);
|
||||
ISAM2Clique::shared_ptr clique = isam2_[index];
|
||||
separatorCliques.insert( clique );
|
||||
}
|
||||
|
||||
// Create the set of clique keys
|
||||
std::vector<Index> cliqueIndices;
|
||||
std::vector<Key> cliqueKeys;
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
BOOST_FOREACH(Index index, clique->conditional()->frontals()) {
|
||||
cliqueIndices.push_back(index);
|
||||
cliqueKeys.push_back(isam2_.getOrdering().key(index));
|
||||
}
|
||||
}
|
||||
std::sort(cliqueIndices.begin(), cliqueIndices.end());
|
||||
std::sort(cliqueKeys.begin(), cliqueKeys.end());
|
||||
|
||||
// Gather all factors that involve only clique keys
|
||||
std::set<size_t> cliqueFactorSlots;
|
||||
BOOST_FOREACH(Index index, cliqueIndices) {
|
||||
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) {
|
||||
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
|
||||
if(factor) {
|
||||
std::set<Key> factorKeys(factor->begin(), factor->end());
|
||||
if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
|
||||
cliqueFactorSlots.insert(slot);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Remove any factor included in the filter summarization
|
||||
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
|
||||
cliqueFactorSlots.erase(slot);
|
||||
}
|
||||
|
||||
// Create a factor graph from the identified factors
|
||||
NonlinearFactorGraph graph;
|
||||
BOOST_FOREACH(size_t slot, cliqueFactorSlots) {
|
||||
graph.push_back(isam2_.getFactorsUnsafe().at(slot));
|
||||
}
|
||||
|
||||
// Find the set of children of the separator cliques
|
||||
std::set<ISAM2Clique::shared_ptr> childCliques;
|
||||
// Add all of the children
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
childCliques.insert(clique->children().begin(), clique->children().end());
|
||||
}
|
||||
// Remove any separator cliques that were added because they were children of other separator cliques
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
|
||||
childCliques.erase(clique);
|
||||
}
|
||||
|
||||
// Augment the factor graph with cached factors from the children
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) {
|
||||
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint()));
|
||||
graph.push_back( factor );
|
||||
}
|
||||
|
||||
// Get the set of separator keys
|
||||
gtsam::FastSet<Key> separatorKeys;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
separatorKeys.insert(key_value.key);
|
||||
}
|
||||
|
||||
// Calculate the marginal factors on the separator
|
||||
smootherSummarization_ = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys,
|
||||
isam2_.params().factorization == ISAM2Params::CHOLESKY ? EliminateCholesky : EliminateQR);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,175 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ConcurrentIncrementalSmoother.h
|
||||
* @brief An iSAM2-based Smoother that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalSmoother : public virtual ConcurrentSmoother {
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<ConcurrentIncrementalSmoother> shared_ptr;
|
||||
typedef ConcurrentSmoother Base; ///< typedef for base class
|
||||
|
||||
/** Meta information returned about the update */
|
||||
struct Result {
|
||||
size_t iterations; ///< The number of optimizer iterations performed
|
||||
size_t nonlinearVariables; ///< The number of variables that can be relinearized
|
||||
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
double error; ///< The final factor graph error
|
||||
|
||||
/// Constructor
|
||||
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
|
||||
|
||||
/// Getter methods
|
||||
size_t getIterations() const { return iterations; }
|
||||
size_t getNonlinearVariables() const { return nonlinearVariables; }
|
||||
size_t getLinearVariables() const { return linearVariables; }
|
||||
double getError() const { return error; }
|
||||
};
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentIncrementalSmoother(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentIncrementalSmoother() {};
|
||||
|
||||
/** Implement a GTSAM standard 'print' function */
|
||||
virtual void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Check if two Concurrent Smoothers are equal */
|
||||
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
|
||||
|
||||
/** Access the current set of factors */
|
||||
const NonlinearFactorGraph& getFactors() const {
|
||||
return isam2_.getFactorsUnsafe();
|
||||
}
|
||||
|
||||
/** Access the current linearization point */
|
||||
const Values& getLinearizationPoint() const {
|
||||
return isam2_.getLinearizationPoint();
|
||||
}
|
||||
|
||||
/** Access the current ordering */
|
||||
const Ordering& getOrdering() const {
|
||||
return isam2_.getOrdering();
|
||||
}
|
||||
|
||||
/** Access the current set of deltas to the linearization point */
|
||||
const VectorValues& getDelta() const {
|
||||
return isam2_.getDelta();
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of all variables and return a full Values structure.
|
||||
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
|
||||
*/
|
||||
Values calculateEstimate() const {
|
||||
return isam2_.calculateEstimate();
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
* calling the no-argument version of calculateEstimate if only specific variables are needed.
|
||||
* @param key
|
||||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(Key key) const {
|
||||
return isam2_.calculateEstimate<VALUE>(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors and variables to the smoother.
|
||||
*
|
||||
* Add new measurements, and optionally new variables, to the smoother.
|
||||
* This runs a full step of the ISAM2 algorithm, relinearizing and updating
|
||||
* the solution as needed, according to the wildfire and relinearize
|
||||
* thresholds.
|
||||
*
|
||||
* @param newFactors The new factors to be added to the smoother
|
||||
* @param newTheta Initialization points for new variables to be added to the smoother
|
||||
* You must include here all new variables occuring in newFactors (which were not already
|
||||
* in the smoother). There must not be any variables here that do not occur in newFactors,
|
||||
* and additionally, variables that were already in the system must not be included here.
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values());
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync();
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the smoother branch summarization
|
||||
* needed by the filter.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
|
||||
|
||||
/**
|
||||
* Apply the new smoother factors sent by the filter, and the updated version of the filter
|
||||
* branch summarized factors.
|
||||
*
|
||||
* @param smootherFactors A set of new factors added to the smoother from the filter
|
||||
* @param smootherValues Linearization points for any new variables
|
||||
* @param summarizedFactors An updated version of the filter branch summarized factors
|
||||
* @param rootValues The linearization point of the root variables
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
|
||||
const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void postsync();
|
||||
|
||||
protected:
|
||||
|
||||
ISAM2 isam2_; ///< iSAM2 inference engine
|
||||
|
||||
// Storage for information received from the filter during the last synchronization
|
||||
NonlinearFactorGraph smootherFactors_; ///< New factors to be added to the smoother during the next update
|
||||
Values smootherValues_; ///< New variables to be added to the smoother during the next update
|
||||
NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update
|
||||
Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization.
|
||||
FastVector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
|
||||
bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet
|
||||
|
||||
// Storage for information to be sent to the filter
|
||||
NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
|
||||
|
||||
private:
|
||||
|
||||
/** Calculate the smoother marginal factors on the separator variables */
|
||||
void updateSmootherSummarization();
|
||||
|
||||
}; // ConcurrentBatchSmoother
|
||||
|
||||
/// Typedef for Matlab wrapping
|
||||
typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult;
|
||||
|
||||
}/// namespace gtsam
|
||||
Loading…
Reference in New Issue