diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp new file mode 100644 index 000000000..fbf7ac3e4 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -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 +#include +#include +#include + +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(&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 >& 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 removedFactors; + + // Generate ordering constraints that force the 'keys to move' to the end + boost::optional > orderingConstraints = boost::none; + if(keysToMove && keysToMove->size() > 0) { + orderingConstraints = gtsam::FastMap(); + 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 > noRelinKeys = boost::none; + + // Mark additional keys between the 'keys to move' and the leaves + boost::optional > additionalKeys = boost::none; + if(keysToMove && keysToMove->size() > 0) { + std::set 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(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 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 marginalFactorsIndices; + std::vector 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& 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 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 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& 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 ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore) { + + // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) + std::vector removedFactorSlots; + const VariableIndex& variableIndex = isam2.getVariableIndex(); + BOOST_FOREACH(Key key, keys) { + const FastList& 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 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& separatorKeys = isam2_.getFixedVariables(); + + // Find all cliques that contain any separator variables + std::set 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 cliqueIndices; + std::vector 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 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 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 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h new file mode 100644 index 000000000..bef94ef32 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -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 +#include + +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 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 + VALUE calculateEstimate(Key key) const { + return isam2_.calculateEstimate(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 >& 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 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& additionalKeys); + + /** Find the set of iSAM2 factors adjacent to 'keys' */ + static std::vector FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp new file mode 100644 index 000000000..9a7072375 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -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 +#include +#include +#include + +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(&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 constrainedKeys; + FastList 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(), 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 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 cliqueIndices; + std::vector 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 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 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 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 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 diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h new file mode 100644 index 000000000..4178d45ff --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -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 +#include + +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 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 + VALUE calculateEstimate(Key key) const { + return isam2_.calculateEstimate(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 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