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

Starting to clean up and refactor the Concurrent Filtering and Smoothing classes. Currently the synchronization is disables, as is the marginalization in the filter.

release/4.3a0
Stephen Williams 2013-04-09 21:24:05 +00:00
parent d6e0cae6f5
commit 0b5c07e543
5 changed files with 1606 additions and 1284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1803
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

File diff suppressed because it is too large Load Diff

148
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -19,10 +19,8 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include "ConcurrentFilteringAndSmoothing.h" #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <map>
#include <queue> #include <queue>
namespace gtsam { namespace gtsam {
@ -33,37 +31,61 @@ namespace gtsam {
class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter {
public: public:
typedef boost::shared_ptr<ConcurrentBatchFilter> shared_ptr;
typedef ConcurrentFilter Base; ///< typedef for base class typedef ConcurrentFilter Base; ///< typedef for base class
typedef std::map<Key, double> KeyTimestampMap; ///< Typedef for a Key-Timestamp map/database
typedef std::multimap<double, Key> TimestampKeyMap;///< Typedef for a Timestamp-Key map/database
/** /** Meta information returned about the update */
* Meta information returned about the update
*/
// TODO: Think of some more things to put here
struct Result { struct Result {
size_t iterations; ///< The number of optimizer iterations performed size_t iterations; ///< The number of optimizer iterations performed
size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization
size_t nonlinearVariables; ///< The number of variables that can be relinearized 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 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error double error; ///< The final factor graph error
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {};
}; };
/** Default constructor */ /** Default constructor */
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters, double lag) : parameters_(parameters), lag_(lag) {}; ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters) : parameters_(parameters) {};
/** Default destructor */ /** Default destructor */
virtual ~ConcurrentBatchFilter() {}; virtual ~ConcurrentBatchFilter() {};
// Implement a GTSAM standard 'print' function /** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two Concurrent Filters are equal */
bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return factors_;
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return theta_;
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return delta_;
}
/** Access the nonlinear variable index */
const VariableIndex& getVariableIndex() const {
return variableIndex_;
}
/** Compute the current best estimate of all variables and return a full Values structure. /** 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&). * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
*/ */
Values calculateEstimate() const { Values calculateEstimate() const {
return theta_; return getLinearizationPoint().retract(getDelta(), getOrdering());
} }
/** Compute the current best estimate of a single variable. This is generally faster than /** Compute the current best estimate of a single variable. This is generally faster than
@ -72,8 +94,10 @@ public:
* @return * @return
*/ */
template<class VALUE> template<class VALUE>
VALUE calculateEstimate(const Key key) const { VALUE calculateEstimate(Key key) const {
return theta_.at<VALUE>(key); const Index index = getOrdering().at(key);
const Vector delta = getDelta().at(index);
return getLinearizationPoint().at<VALUE>(key).retract(delta);
} }
/** /**
@ -86,30 +110,25 @@ public:
* @param newTheta Initialization points for new variables to be added to the filter * @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 * You must include here all new variables occurring in newFactors that were not already
* in the filter. * in the filter.
* @param keysToMove An optional set of keys to remove from the filter and
*/ */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const KeyTimestampMap& timestamps = KeyTimestampMap()); const boost::optional<FastList<Key> >& keysToMove = boost::none);
protected: protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Index> > FactorIndex;
LevenbergMarquardtParams parameters_; ///< LM parameters LevenbergMarquardtParams parameters_; ///< LM parameters
double lag_; ///< Time before keys are transitioned from the filter to the smoother NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
NonlinearFactorGraph graph_; ///< The graph of all the smoother factors Values theta_; ///< Current linearization point of all variables in the filter
Values theta_; ///< Current solution Ordering ordering_; ///< The current ordering used to calculate the linear deltas
TimestampKeyMap timestampKeyMap_; ///< The set of keys associated with each timestamp VectorValues delta_; ///< The current set of linear deltas from the linearization point
KeyTimestampMap keyTimestampMap_; ///< The timestamp associated with each key VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
std::vector<size_t> smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors
std::vector<size_t> smootherSummarizationSlots_; ///< The slots in graph for the last set of smoother summarized factors
Values separatorValues_;
// Storage for information to be sent to the smoother // Storage for information to be sent to the smoother
NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother
Values rootValues_; ///< The set of keys to be kept in the root and their linearization points
NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being 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 Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
@ -126,7 +145,7 @@ protected:
* @param summarizedFactors The summarized factors for the filter branch * @param summarizedFactors The summarized factors for the filter branch
* @param rootValues The linearization points of the root clique variables * @param rootValues The linearization points of the root clique variables
*/ */
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues); virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
/** /**
* Populate the provided containers with factors being sent to the smoother from the filter. These * Populate the provided containers with factors being sent to the smoother from the filter. These
@ -152,47 +171,34 @@ protected:
virtual void postsync(); virtual void postsync();
/** Augment the graph with a new factor
*
* @param factor The new factor to add to the graph
* @return The slot in the graph where the factor was inserted
*/
size_t insertFactor(const NonlinearFactor::shared_ptr& factor);
/** Remove a factor from the graph by slot index */
void removeFactor(size_t slot);
/** Remove the specified key from all data structures */
void removeKey(Key key);
/** Update the Timestamps associated with the keys */
void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
/** Find the most recent timestamp of the system */
double getCurrentTimestamp() const;
/** Find all of the keys associated with timestamps before the provided time */
std::set<Key> findKeysBefore(double timestamp) const;
/** Find all of the keys associated with timestamps after the provided time */
std::set<Key> findKeysAfter(double timestamp) const;
/** Find all of the nonlinear factors that contain any of the provided keys */
std::set<size_t> findFactorsWithAny(const std::set<Key>& keys) const;
/** Find all of the nonlinear factors that contain only the provided keys */
std::set<size_t> findFactorsWithOnly(const std::set<Key>& keys) const;
/** Create linearized factors from any factors remaining after marginalizing out the requested keys */
NonlinearFactor::shared_ptr marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set<Key>& remainingKeys) const;
std::set<gtsam::Key> findSeparatorKeys(const gtsam::NonlinearFactorGraph& graph) const;
gtsam::Ordering computeOrdering(const gtsam::NonlinearFactorGraph& graph) const;
private: private:
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = ""); /** Augment the graph with new factors
*
* @param factors The factors to add to the graph
* @return The slots in the graph where they were inserted
*/
std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
/** Remove factors from the graph by slot index
*
* @param slots The slots in the factor graph that should be deleted
* */
void removeFactors(const std::vector<size_t>& slots);
/** Use colamd to update into an efficient ordering */
void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
/** Use a modified version of L-M to update the linearization point and delta */
Result optimize();
/** Print just the nonlinear keys in a nonlinear factor */
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in a linear factor */
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
}; // ConcurrentBatchFilter }; // ConcurrentBatchFilter

801
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -18,38 +18,32 @@
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h> #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/inference/JunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <boost/lambda/lambda.hpp>
namespace gtsam { namespace gtsam {
///* ************************************************************************* */ /* ************************************************************************* */
//void ConcurrentBatchSmoother::SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent) { void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
// std::cout << indent << "P( "; std::cout << s;
// BOOST_FOREACH(Index index, clique->conditional()->frontals()){ std::cout << " Factors:" << std::endl;
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) {
// } PrintNonlinearFactor(factor, " ", keyFormatter);
// if(clique->conditional()->nrParents() > 0) { }
// std::cout << "| "; theta_.print("Values:\n");
// } }
// BOOST_FOREACH(Index index, clique->conditional()->parents()){
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
// }
// std::cout << ")" << std::endl;
//
// BOOST_FOREACH(const Clique& child, clique->children()) {
// SymbolicPrintTree(child, ordering, indent+" ");
// }
//}
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchSmoother::print(const std::string& s, bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) const {
const KeyFormatter& keyFormatter) const { const ConcurrentBatchSmoother* smoother = dynamic_cast<const ConcurrentBatchSmoother*>(&rhs);
std::cout << s; return smoother
graph_.print("Factors:\n"); && factors_.equals(smoother->factors_)
theta_.print("Values:\n"); && theta_.equals(smoother->theta_)
&& ordering_.equals(smoother->ordering_)
&& delta_.equals(smoother->delta_)
&& variableIndex_.equals(smoother->variableIndex_)
&& separatorValues_.equals(smoother->separatorValues_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -57,164 +51,217 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
gttic(update); gttic(update);
// Create result structure // Create the return result meta-data
Result result; Result result;
// Update all of the internal variables with the new information
gttic(augment_system); gttic(augment_system);
// Add the new factors to the graph
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
insertFactor(factor);
}
// Add the new variables to theta // Add the new variables to theta
theta_.insert(newTheta); theta_.insert(newTheta);
// Add new variables to the end of the ordering
std::vector<size_t> dims;
dims.reserve(newTheta.size());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
ordering_.push_back(key_value.key);
dims.push_back(key_value.value.dim());
}
// Augment Delta
delta_.append(dims);
for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) {
delta_[i].setZero();
}
// Add the new factors to the graph, updating the variable index
insertFactors(newFactors);
gttoc(augment_system); gttoc(augment_system);
// Optimize the graph, updating theta // Reorder the system to ensure efficient optimization (and marginalization) performance
gttic(optimize); gttic(reorder);
if(graph_.size() > 0){ reorder();
optimize(); gttoc(reorder);
// TODO: fill in the results structure properly // Optimize the factors using a modified version of L-M
result.iterations = 0; gttic(optimize);
result.nonlinearVariables = theta_.size() - separatorValues_.size(); if(factors_.size() > 0) {
result.linearVariables = separatorValues_.size(); result = optimize();
result.error = 0;
} }
gttoc(optimize); gttoc(optimize);
// Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these
// calculations while the filter is still running
gttic(presync); gttic(presync);
// Calculate and store the information passed up to the separator. This requires:
// 1) Calculate an ordering that forces the separator variables to be in the root
// 2) Eliminate all of the variables except the root. This produces one or more
// linear, marginal factors on the separator variables.
// 3) Convert the marginal factors into nonlinear ones using the 'LinearContainerFactor'
if(separatorValues_.size() > 0) {
// Force variables associated with root keys to keep the same linearization point
gttic(enforce_consistency);
Values linpoint;
linpoint.insert(theta_);
linpoint.insert(separatorValues_);
//linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n");
gttoc(enforce_consistency);
// Calculate a root-constrained ordering
gttic(compute_ordering);
std::map<Key, int> constraints;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
constraints[key_value.key] = 1;
}
Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints);
gttoc(compute_ordering);
// Create a Bayes Tree using iSAM2 cliques
gttic(create_bayes_tree);
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(*graph_.linearize(linpoint, ordering));
ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction());
BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
bayesTree.insert(root);
gttoc(create_bayes_tree);
//ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n");
std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl;
std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl;
//SymbolicPrintTree(root, ordering, " ");
// Extract the marginal factors from the smoother
// For any non-filter factor that involves a root variable,
// calculate its marginal on the root variables using the
// current linearization point.
// Find all of the smoother branches as the children of root cliques that are not also root cliques
gttic(find_smoother_branches);
std::set<ISAM2Clique::shared_ptr> rootCliques;
std::set<ISAM2Clique::shared_ptr> smootherBranches;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key));
if(clique) {
rootCliques.insert(clique);
smootherBranches.insert(clique->children().begin(), clique->children().end());
}
}
BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) {
smootherBranches.erase(rootClique);
}
gttoc(find_smoother_branches);
// Extract the cached factors on the root cliques from the smoother branches
gttic(extract_cached_factors);
GaussianFactorGraph cachedFactors;
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
cachedFactors.push_back(clique->cachedFactor());
}
gttoc(extract_cached_factors);
std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) {
std::cout << " g( ";
BOOST_FOREACH(Index index, factor->keys()) {
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
}
std::cout << ")" << std::endl;
}
// Marginalize out any additional (non-root) variables
gttic(marginalize_extra_variables);
// The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 }
Index minRootIndex = linpoint.size() - separatorValues_.size();
// Calculate the set of keys to be marginalized
FastSet<Index> cachedIndices = cachedFactors.keys();
std::vector<Index> marginalizeIndices;
std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex);
std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: ";
BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; }
std::cout << std::endl;
// If non-root-keys are present, marginalize them out
if(marginalizeIndices.size() > 0) {
// Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph
GaussianConditional::shared_ptr conditional;
boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction());
}
gttoc(marginalize_extra_variables);
std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) {
std::cout << " g( ";
BOOST_FOREACH(Index index, factor->keys()) {
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
}
std::cout << ")" << std::endl;
}
// Convert factors into 'Linearized' nonlinear factors
gttic(store_cached_factors);
smootherSummarization_.resize(0);
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) {
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(gaussianFactor, ordering, linpoint));
smootherSummarization_.push_back(factor);
}
gttoc(store_cached_factors);
std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) {
std::cout << " f( ";
BOOST_FOREACH(Key key, factor->keys()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
}
}
gttoc(presync); gttoc(presync);
gttoc(update); gttoc(update);
// gttic(update);
//
// // Create result structure
// Result result;
//
// gttic(augment_system);
// // Add the new factors to the graph
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
// insertFactor(factor);
// }
// // Add the new variables to theta
// theta_.insert(newTheta);
// gttoc(augment_system);
//
// // Optimize the graph, updating theta
// gttic(optimize);
// if(graph_.size() > 0){
// optimize();
//
// // TODO: fill in the results structure properly
// result.iterations = 0;
// result.nonlinearVariables = theta_.size() - separatorValues_.size();
// result.linearVariables = separatorValues_.size();
// result.error = 0;
// }
// gttoc(optimize);
//
// // Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these
// // calculations while the filter is still running
// gttic(presync);
// // Calculate and store the information passed up to the separator. This requires:
// // 1) Calculate an ordering that forces the separator variables to be in the root
// // 2) Eliminate all of the variables except the root. This produces one or more
// // linear, marginal factors on the separator variables.
// // 3) Convert the marginal factors into nonlinear ones using the 'LinearContainerFactor'
//
// if(separatorValues_.size() > 0) {
// // Force variables associated with root keys to keep the same linearization point
// gttic(enforce_consistency);
// Values linpoint;
// linpoint.insert(theta_);
// linpoint.insert(separatorValues_);
//
////linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n");
//
// gttoc(enforce_consistency);
//
// // Calculate a root-constrained ordering
// gttic(compute_ordering);
// std::map<Key, int> constraints;
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
// constraints[key_value.key] = 1;
// }
// Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints);
// gttoc(compute_ordering);
//
// // Create a Bayes Tree using iSAM2 cliques
// gttic(create_bayes_tree);
// JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(*graph_.linearize(linpoint, ordering));
// ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction());
// BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
// bayesTree.insert(root);
// gttoc(create_bayes_tree);
//
////ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n");
//std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl;
//std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl;
////SymbolicPrintTree(root, ordering, " ");
//
// // Extract the marginal factors from the smoother
// // For any non-filter factor that involves a root variable,
// // calculate its marginal on the root variables using the
// // current linearization point.
//
// // Find all of the smoother branches as the children of root cliques that are not also root cliques
// gttic(find_smoother_branches);
// std::set<ISAM2Clique::shared_ptr> rootCliques;
// std::set<ISAM2Clique::shared_ptr> smootherBranches;
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
// const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key));
// if(clique) {
// rootCliques.insert(clique);
// smootherBranches.insert(clique->children().begin(), clique->children().end());
// }
// }
// BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) {
// smootherBranches.erase(rootClique);
// }
// gttoc(find_smoother_branches);
//
// // Extract the cached factors on the root cliques from the smoother branches
// gttic(extract_cached_factors);
// GaussianFactorGraph cachedFactors;
// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
// cachedFactors.push_back(clique->cachedFactor());
// }
// gttoc(extract_cached_factors);
//
//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl;
//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) {
// std::cout << " g( ";
// BOOST_FOREACH(Index index, factor->keys()) {
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
// }
// std::cout << ")" << std::endl;
//}
//
// // Marginalize out any additional (non-root) variables
// gttic(marginalize_extra_variables);
// // The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 }
// Index minRootIndex = linpoint.size() - separatorValues_.size();
// // Calculate the set of keys to be marginalized
// FastSet<Index> cachedIndices = cachedFactors.keys();
// std::vector<Index> marginalizeIndices;
// std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex);
//
//std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: ";
//BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; }
//std::cout << std::endl;
//
// // If non-root-keys are present, marginalize them out
// if(marginalizeIndices.size() > 0) {
// // Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph
// GaussianConditional::shared_ptr conditional;
// boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction());
// }
// gttoc(marginalize_extra_variables);
//
//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl;
//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) {
// std::cout << " g( ";
// BOOST_FOREACH(Index index, factor->keys()) {
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
// }
// std::cout << ")" << std::endl;
//}
//
// // Convert factors into 'Linearized' nonlinear factors
// gttic(store_cached_factors);
// smootherSummarization_.resize(0);
// BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) {
// LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(gaussianFactor, ordering, linpoint));
// smootherSummarization_.push_back(factor);
// }
// gttoc(store_cached_factors);
//
//std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl;
//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) {
// std::cout << " f( ";
// BOOST_FOREACH(Key key, factor->keys()) {
// std::cout << DefaultKeyFormatter(key) << " ";
// }
// std::cout << ")" << std::endl;
//}
//
// }
// gttoc(presync);
//
// gttoc(update);
return result; return result;
} }
@ -223,7 +270,6 @@ void ConcurrentBatchSmoother::presync() {
gttic(presync); gttic(presync);
gttoc(presync); gttoc(presync);
} }
@ -245,23 +291,43 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
gttic(synchronize); gttic(synchronize);
// Remove the previous filter summarization from the graph // Remove the previous filter summarization from the graph
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { removeFactors(filterSummarizationSlots_);
removeFactor(slot);
}
filterSummarizationSlots_.clear();
// Insert the new filter summarized factors // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { std::vector<size_t> dims;
filterSummarizationSlots_.push_back(insertFactor(factor)); dims.reserve(smootherValues.size() + separatorValues.size());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) {
Values::iterator iter = theta_.find(key_value.key);
if(iter == theta_.end()) {
theta_.insert(key_value.key, key_value.value);
ordering_.push_back(key_value.key);
dims.push_back(key_value.value.dim());
} else {
iter->value = key_value.value;
}
}
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) {
Values::iterator iter = theta_.find(key_value.key);
if(iter == theta_.end()) {
theta_.insert(key_value.key, key_value.value);
ordering_.push_back(key_value.key);
dims.push_back(key_value.value.dim());
} else {
iter->value = key_value.value;
}
}
// Augment Delta
delta_.append(dims);
for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) {
delta_[i].setZero();
} }
// Insert the new smoother factors // Insert the new smoother factors
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors) { insertFactors(smootherFactors);
insertFactor(factor);
}
// Insert new linpoints into the values // Insert the new filter summarized factors
theta_.insert(smootherValues); filterSummarizationSlots_ = insertFactors(summarizedFactors);
// Update the list of root keys // Update the list of root keys
separatorValues_ = separatorValues; separatorValues_ = separatorValues;
@ -278,167 +344,175 @@ void ConcurrentBatchSmoother::postsync() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t ConcurrentBatchSmoother::insertFactor(const NonlinearFactor::shared_ptr& factor) { std::vector<size_t> ConcurrentBatchSmoother::insertFactors(const NonlinearFactorGraph& factors) {
gttic(insert_factors); gttic(insert_factors);
// create the output vector
std::vector<size_t> slots;
slots.reserve(factors.size());
// Insert the factor into an existing hole in the factor graph, if possible // Insert the factor into an existing hole in the factor graph, if possible
size_t slot; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
if(availableSlots_.size() > 0) { size_t slot;
slot = availableSlots_.front(); if(availableSlots_.size() > 0) {
availableSlots_.pop(); slot = availableSlots_.front();
graph_.replace(slot, factor); availableSlots_.pop();
} else { factors_.replace(slot, factor);
slot = graph_.size(); } else {
graph_.push_back(factor); slot = factors_.size();
} factors_.push_back(factor);
// Update the FactorIndex }
BOOST_FOREACH(Key key, *factor) { slots.push_back(slot);
factorIndex_[key].insert(slot);
} }
// Augment the Variable Index
variableIndex_.augment(*factors.symbolic(ordering_));
gttoc(insert_factors); gttoc(insert_factors);
return slot; return slots;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchSmoother::removeFactor(size_t slot) { void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors); gttic(remove_factors);
// Remove references to this factor from the FactorIndex // For each factor slot to delete...
BOOST_FOREACH(Key key, *(graph_.at(slot))) { SymbolicFactorGraph factors;
factorIndex_[key].erase(slot); BOOST_FOREACH(size_t slot, slots) {
// Create a symbolic version for the variable index
factors.push_back(factors_.at(slot)->symbolic(ordering_));
// Remove the factor from the graph
factors_.remove(slot);
// Mark the factor slot as available
availableSlots_.push(slot);
} }
// Remove this factor from the graph
graph_.remove(slot); // Remove references to this factor from the VariableIndex
// Mark the factor slot as avaiable variableIndex_.remove(slots, factors);
availableSlots_.push(slot);
gttoc(remove_factors); gttoc(remove_factors);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::set<size_t> ConcurrentBatchSmoother::findFactorsWithAny(const std::set<Key>& keys) const { void ConcurrentBatchSmoother::reorder() {
// Find the set of factor slots for each specified key
std::set<size_t> factorSlots; // Initialize all variables to group0
BOOST_FOREACH(Key key, keys) { std::vector<int> cmember(variableIndex_.size(), 0);
FactorIndex::const_iterator iter = factorIndex_.find(key);
if(iter != factorIndex_.end()) { // Set all of the separator keys to Group1
factorSlots.insert(iter->second.begin(), iter->second.end()); if(separatorValues_.size() > 0) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
cmember[ordering_.at(key_value.key)] = 1;
} }
} }
return factorSlots; // Generate the permutation
Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember);
// Permute the ordering, variable index, and deltas
ordering_.permuteInPlace(forwardPermutation);
variableIndex_.permuteInPlace(forwardPermutation);
delta_.permuteInPlace(forwardPermutation);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::set<size_t> ConcurrentBatchSmoother::findFactorsWithOnly(const std::set<Key>& keys) const { ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
// Find the set of factor slots with any of the provided keys
std::set<size_t> factorSlots = findFactorsWithAny(keys);
// Test each factor for non-specified keys
std::set<size_t>::iterator slot = factorSlots.begin();
while(slot != factorSlots.end()) {
const NonlinearFactor::shared_ptr& factor = graph_.at(*slot);
std::set<Key> factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted
if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) {
factorSlots.erase(slot++);
} else {
++slot;
}
}
return factorSlots; // Create output result structure
} Result result;
result.nonlinearVariables = theta_.size() - separatorValues_.size();
result.linearVariables = separatorValues_.size();
/* ************************************************************************* */ // Set optimization parameters
void ConcurrentBatchSmoother::optimize() { double lambda = parameters_.lambdaInitial;
double lambdaFactor = parameters_.lambdaFactor;
double lambdaUpperBound = parameters_.lambdaUpperBound;
double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound;
size_t maxIterations = parameters_.maxIterations;
double relativeErrorTol = parameters_.relativeErrorTol;
double absoluteErrorTol = parameters_.absoluteErrorTol;
double errorTol = parameters_.errorTol;
bool debug = ISDEBUG("ConcurrentBatchSmoother::optimize"); // Create a Values that holds the current evaluation point
Values evalpoint = theta_.retract(delta_, ordering_);
if(debug) std::cout << "ConcurrentBatchSmoother::optimize() Start" << std::endl; result.error = factors_.error(evalpoint);
// Calculate a good ordering
// TODO:
// Create a Values that holds the current evaluation point (simply use the linpoint initially, as a full delta is not available)
gtsam::Values evalpoint = theta_.retract(delta_, ordering_);
// Use a custom optimization loop so the linearization points can be controlled // Use a custom optimization loop so the linearization points can be controlled
double currentError = graph_.error(evalpoint);
double previousError; double previousError;
gtsam::VectorValues newDelta; VectorValues newDelta;
size_t iterations = 0;
double lambda = parameters_.lambdaInitial;
do { do {
previousError = currentError; previousError = result.error;
// Do next iteration // Do next iteration
gttic(optimizer_iteration); gttic(optimizer_iteration);
{ {
// Linearize graph around the linearization point // Linearize graph around the linearization point
gtsam::GaussianFactorGraph linearFactorGraph = *graph_.linearize(theta_, ordering_); GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while(true) { while(true) {
// Add prior factors at the current solution // Add prior factors at the current solution
gttic(damp); gttic(damp);
gtsam::GaussianFactorGraph dampedFactorGraph(linearFactorGraph); GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
{ {
// for each of the variables, add a prior at the current solution // for each of the variables, add a prior at the current solution
for(size_t j=0; j<delta_.size(); ++j) { for(size_t j=0; j<delta_.size(); ++j) {
gtsam::Matrix A = lambda * gtsam::eye(delta_[j].size()); Matrix A = lambda * eye(delta_[j].size());
gtsam::Vector b = lambda * delta_[j]; Vector b = lambda * delta_[j];
gtsam::SharedDiagonal model = gtsam::noiseModel::Unit::Create(delta_[j].size()); SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size());
gtsam::GaussianFactor::shared_ptr prior(new gtsam::JacobianFactor(j, A, b, model)); GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior); dampedFactorGraph.push_back(prior);
} }
} }
gttoc(damp); gttoc(damp);
result.lambdas++;
gttic(solve);
// Solve Damped Gaussian Factor Graph // Solve Damped Gaussian Factor Graph
newDelta = gtsam::GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
// update the evalpoint with the new delta // update the evalpoint with the new delta
gttic(retract);
evalpoint = theta_.retract(newDelta, ordering_); evalpoint = theta_.retract(newDelta, ordering_);
gttoc(retract); gttoc(solve);
// Evaluate the new error // Evaluate the new error
gttic(compute_error); gttic(compute_error);
double newError = graph_.error(evalpoint); double error = factors_.error(evalpoint);
gttoc(compute_error); gttoc(compute_error);
if(newError < currentError) { if(error < result.error) {
// Keep this change // Keep this change
// Update the error value // Update the error value
currentError = newError; result.error = error;
// Update the linearization point // Update the linearization point
theta_ = evalpoint; theta_ = evalpoint;
// Reset the deltas to zeros // Reset the deltas to zeros
delta_.setZero(); delta_.setZero();
// If 'enableConsistency' is turned on, put the linearization points and deltas back for specific variables // Put the linearization points and deltas back for specific variables
if(separatorValues_.size() > 0) { if(separatorValues_.size() > 0) {
theta_.update(separatorValues_); theta_.update(separatorValues_);
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
gtsam::Index index = ordering_.at(key_value.key); Index index = ordering_.at(key_value.key);
delta_.at(index) = newDelta.at(index); delta_.at(index) = newDelta.at(index);
} }
} }
// Decrease lambda for next time // Decrease lambda for next time
lambda /= parameters_.lambdaFactor; lambda /= lambdaFactor;
if(lambda < (0.5 / parameters_.lambdaUpperBound)) { if(lambda < lambdaLowerBound) {
lambda = (0.5 / parameters_.lambdaUpperBound); lambda = lambdaLowerBound;
} }
// End this lambda search iteration // End this lambda search iteration
break; break;
} else { } else {
// Reject this change // Reject this change
// Increase lambda and continue searching // Increase lambda and continue searching
lambda *= parameters_.lambdaFactor; lambda *= lambdaFactor;
if(lambda > parameters_.lambdaUpperBound) { if(lambda > lambdaUpperBound) {
// The maximum lambda has been used. Print a warning and end the search. // The maximum lambda has been used. Print a warning and end the search.
std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
break; break;
@ -448,90 +522,23 @@ void ConcurrentBatchSmoother::optimize() {
} }
gttoc(optimizer_iteration); gttoc(optimizer_iteration);
++iterations; result.iterations++;
} while(iterations < parameters_.maxIterations && } while(result.iterations < maxIterations &&
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
previousError, currentError, gtsam::NonlinearOptimizerParams::SILENT));
if(debug) std::cout << "ConcurrentBatchSmoother::optimize() Finish" << std::endl; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor::shared_ptr ConcurrentBatchSmoother::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set<Key>& keysToKeep, const Values& theta) const { void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) {
factor->print("Factor Before:\n");
// Sort the keys for this factor
std::set<Key> factorKeys;
BOOST_FOREACH(Key key, *factor) {
factorKeys.insert(key);
}
// Calculate the set of keys to marginalize
std::set<Key> marginalizeKeys;
std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
std::set<Key> remainingKeys;
std::set_intersection(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(remainingKeys, remainingKeys.end()));
//
if(marginalizeKeys.size() == 0) {
// No keys need to be marginalized out. Simply return the original factor.
return factor;
} else if(marginalizeKeys.size() == factor->size()) {
// All keys need to be marginalized out. Return an empty factor
return NonlinearFactor::shared_ptr();
} else {
// (0) Create an ordering with the remaining keys last
Ordering ordering;
BOOST_FOREACH(Key key, marginalizeKeys) {
ordering.push_back(key);
}
BOOST_FOREACH(Key key, remainingKeys) {
ordering.push_back(key);
}
ordering.print("Ordering:\n");
// (1) construct a linear factor graph
GaussianFactorGraph graph;
graph.push_back( factor->linearize(theta, ordering) );
graph.at(0)->print("Linear Factor Before:\n");
// (2) solve for the marginal factor
// Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables)
// and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2
std::vector<Index> variables;
BOOST_FOREACH(Key key, marginalizeKeys) {
variables.push_back(ordering.at(key));
}
// std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph> result = graph.eliminate(variables);
GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size());
result.first->print("Resulting Conditional:\n");
result.second->print("Resulting Linear Factor:\n");
// graph = result.second;
graph.replace(0, result.second);
// (3) convert the marginal factors into Linearized Factors
NonlinearFactor::shared_ptr marginalFactor;
assert(graph.size() <= 1);
if(graph.size() > 0) {
graph.at(0)->print("Linear Factor After:\n");
marginalFactor.reset(new LinearContainerFactor(graph.at(0), ordering, theta));
}
marginalFactor->print("Factor After:\n");
return marginalFactor;
}
}
/* ************************************************************************* */
void ConcurrentBatchSmoother::PrintNonlinearFactor(const gtsam::NonlinearFactor::shared_ptr& factor, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) {
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
if(boost::dynamic_pointer_cast<gtsam::LinearContainerFactor>(factor)) { if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( "; std::cout << "l( ";
} else { } else {
std::cout << "f( "; std::cout << "f( ";
} }
BOOST_FOREACH(gtsam::Key key, *factor) { BOOST_FOREACH(Key key, *factor) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -541,11 +548,11 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const gtsam::NonlinearFactor:
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
std::cout << "g( "; std::cout << "g( ";
BOOST_FOREACH(gtsam::Index index, *factor) { BOOST_FOREACH(Index index, *factor) {
std::cout << keyFormatter(ordering.key(index)) << " "; std::cout << keyFormatter(ordering.key(index)) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -554,15 +561,119 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::sha
} }
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//void ConcurrentBatchSmoother::PrintSingleClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { //std::set<size_t> ConcurrentBatchSmoother::findFactorsWithAny(const std::set<Key>& keys) const {
// // Find the set of factor slots for each specified key
// std::set<size_t> factorSlots;
// BOOST_FOREACH(Key key, keys) {
// FactorIndex::const_iterator iter = factorIndex_.find(key);
// if(iter != factorIndex_.end()) {
// factorSlots.insert(iter->second.begin(), iter->second.end());
// }
// }
//
// return factorSlots;
//}
//
///* ************************************************************************* */
//std::set<size_t> ConcurrentBatchSmoother::findFactorsWithOnly(const std::set<Key>& keys) const {
// // Find the set of factor slots with any of the provided keys
// std::set<size_t> factorSlots = findFactorsWithAny(keys);
// // Test each factor for non-specified keys
// std::set<size_t>::iterator slot = factorSlots.begin();
// while(slot != factorSlots.end()) {
// const NonlinearFactor::shared_ptr& factor = graph_.at(*slot);
// std::set<Key> factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted
// if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) {
// factorSlots.erase(slot++);
// } else {
// ++slot;
// }
// }
//
// return factorSlots;
//}
//
///* ************************************************************************* */
//NonlinearFactor::shared_ptr ConcurrentBatchSmoother::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set<Key>& keysToKeep, const Values& theta) const {
//
//factor->print("Factor Before:\n");
//
// // Sort the keys for this factor
// std::set<Key> factorKeys;
// BOOST_FOREACH(Key key, *factor) {
// factorKeys.insert(key);
// }
//
// // Calculate the set of keys to marginalize
// std::set<Key> marginalizeKeys;
// std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
// std::set<Key> remainingKeys;
// std::set_intersection(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(remainingKeys, remainingKeys.end()));
//
// //
// if(marginalizeKeys.size() == 0) {
// // No keys need to be marginalized out. Simply return the original factor.
// return factor;
// } else if(marginalizeKeys.size() == factor->size()) {
// // All keys need to be marginalized out. Return an empty factor
// return NonlinearFactor::shared_ptr();
// } else {
// // (0) Create an ordering with the remaining keys last
// Ordering ordering;
// BOOST_FOREACH(Key key, marginalizeKeys) {
// ordering.push_back(key);
// }
// BOOST_FOREACH(Key key, remainingKeys) {
// ordering.push_back(key);
// }
//ordering.print("Ordering:\n");
//
// // (1) construct a linear factor graph
// GaussianFactorGraph graph;
// graph.push_back( factor->linearize(theta, ordering) );
//graph.at(0)->print("Linear Factor Before:\n");
//
// // (2) solve for the marginal factor
// // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables)
// // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2
// std::vector<Index> variables;
// BOOST_FOREACH(Key key, marginalizeKeys) {
// variables.push_back(ordering.at(key));
// }
//// std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph> result = graph.eliminate(variables);
// GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size());
//result.first->print("Resulting Conditional:\n");
//result.second->print("Resulting Linear Factor:\n");
//// graph = result.second;
// graph.replace(0, result.second);
//
// // (3) convert the marginal factors into Linearized Factors
// NonlinearFactor::shared_ptr marginalFactor;
// assert(graph.size() <= 1);
// if(graph.size() > 0) {
//graph.at(0)->print("Linear Factor After:\n");
// marginalFactor.reset(new LinearContainerFactor(graph.at(0), ordering, theta));
// }
//marginalFactor->print("Factor After:\n");
// return marginalFactor;
// }
//}
///* ************************************************************************* */
//void ConcurrentBatchSmoother::PrintSingleClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
// std::cout << indent << "P( "; // std::cout << indent << "P( ";
// BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()){ // BOOST_FOREACH(Index index, clique->conditional()->frontals()){
// std::cout << keyFormatter(ordering.key(index)) << " "; // std::cout << keyFormatter(ordering.key(index)) << " ";
// } // }
// if(clique->conditional()->nrParents() > 0){ // if(clique->conditional()->nrParents() > 0){
// std::cout << "| "; // std::cout << "| ";
// BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()){ // BOOST_FOREACH(Index index, clique->conditional()->parents()){
// std::cout << keyFormatter(ordering.key(index)) << " "; // std::cout << keyFormatter(ordering.key(index)) << " ";
// } // }
// } // }
@ -570,19 +681,19 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::sha
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//void ConcurrentBatchSmoother::PrintRecursiveClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { //void ConcurrentBatchSmoother::PrintRecursiveClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
// //
// // Print this node // // Print this node
// PrintSingleClique(clique, ordering, indent, keyFormatter); // PrintSingleClique(clique, ordering, indent, keyFormatter);
// //
// // Print Children // // Print Children
// BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { // BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
// PrintRecursiveClique(child, ordering, indent+" ", keyFormatter); // PrintRecursiveClique(child, ordering, indent+" ", keyFormatter);
// } // }
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//void ConcurrentBatchSmoother::PrintBayesTree(const gtsam::ISAM2& bayesTree, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { //void ConcurrentBatchSmoother::PrintBayesTree(const ISAM2& bayesTree, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
// //
// std::cout << indent << "Bayes Tree:" << std::endl; // std::cout << indent << "Bayes Tree:" << std::endl;
// if (bayesTree.root().use_count() == 0) { // if (bayesTree.root().use_count() == 0) {

119
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -21,7 +21,6 @@
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h> #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <queue> #include <queue>
namespace gtsam { namespace gtsam {
@ -32,19 +31,17 @@ namespace gtsam {
class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
public: public:
typedef boost::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
typedef ConcurrentSmoother Base; ///< typedef for base class typedef ConcurrentSmoother Base; ///< typedef for base class
/** /** Meta information returned about the update */
* Meta information returned about the update
*/
// TODO: Think of some more things to put here
struct Result { struct Result {
size_t iterations; ///< The number of optimizer iterations performed size_t iterations; ///< The number of optimizer iterations performed
size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization
size_t nonlinearVariables; ///< The number of variables that can be relinearized 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 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error double error; ///< The final factor graph error
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {};
}; };
/** Default constructor */ /** Default constructor */
@ -53,14 +50,42 @@ public:
/** Default destructor */ /** Default destructor */
virtual ~ConcurrentBatchSmoother() {}; virtual ~ConcurrentBatchSmoother() {};
// Implement a GTSAM standard 'print' function /** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two Concurrent Smoothers are equal */
bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return factors_;
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return theta_;
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return delta_;
}
/** Access the nonlinear variable index */
const VariableIndex& getVariableIndex() const {
return variableIndex_;
}
/** Compute the current best estimate of all variables and return a full Values structure. /** 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&). * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
*/ */
Values calculateEstimate() const { Values calculateEstimate() const {
return theta_; return getLinearizationPoint().retract(getDelta(), getOrdering());
} }
/** Compute the current best estimate of a single variable. This is generally faster than /** Compute the current best estimate of a single variable. This is generally faster than
@ -69,8 +94,10 @@ public:
* @return * @return
*/ */
template<class VALUE> template<class VALUE>
VALUE calculateEstimate(const Key key) const { VALUE calculateEstimate(Key key) const {
return theta_.at<VALUE>(key); const Index index = getOrdering().at(key);
const Vector delta = getDelta().at(index);
return getLinearizationPoint().at<VALUE>(key).retract(delta);
} }
/** /**
@ -92,18 +119,17 @@ public:
protected: protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Index> > FactorIndex;
LevenbergMarquardtParams parameters_; ///< LM parameters LevenbergMarquardtParams parameters_; ///< LM parameters
NonlinearFactorGraph graph_; ///< The graph of all the smoother factors NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
Values theta_; ///< Current linearization point Values theta_; ///< Current linearization point of all variables in the smoother
Ordering ordering_; ///< The current ordering used to generate the deltas Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< Current set of offsets from the linearization point VectorValues delta_; ///< The current set of linear deltas from the linearization point
Values separatorValues_; ///< The set of keys to be kept in the root and their linearization points VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
std::vector<size_t> filterSummarizationSlots_; ///< The slots in graph for the last set of filter summarized factors std::vector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
// Storage for information to be sent to the filter
NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
/** /**
@ -130,7 +156,7 @@ protected:
* @param rootValues The linearization point of the root variables * @param rootValues The linearization point of the root variables
*/ */
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
const NonlinearFactorGraph& summarizedFactors, const Values& rootValues); const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
/** /**
* Perform any required operations after the synchronization process finishes. * Perform any required operations after the synchronization process finishes.
@ -138,48 +164,33 @@ protected:
*/ */
virtual void postsync(); virtual void postsync();
/** Augment the graph with a new factor /** Augment the graph with new factors
* *
* @param factors The factor to add to the graph * @param factors The factors to add to the graph
* @return The slot in the graph it was inserted into * @return The slots in the graph where they were inserted
*/ */
size_t insertFactor(const NonlinearFactor::shared_ptr& factor); std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
/** Remove a factor from the graph by slot index */ /** Remove factors from the graph by slot index
void removeFactor(size_t slot); *
* @param slots The slots in the factor graph that should be deleted
* */
void removeFactors(const std::vector<size_t>& slots);
/** Optimize the graph using a modified version of L-M */ /** Use colamd to update into an efficient ordering */
void optimize(); void reorder();
/** Find all of the nonlinear factors that contain any of the provided keys */ /** Use a modified version of L-M to update the linearization point and delta */
std::set<size_t> findFactorsWithAny(const std::set<Key>& keys) const; Result optimize();
/** Find all of the nonlinear factors that contain only the provided keys */
std::set<size_t> findFactorsWithOnly(const std::set<Key>& keys) const;
/** Create a linearized factor from the information remaining after marginalizing out the requested keys */
NonlinearFactor::shared_ptr marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set<Key>& keysToKeep, const Values& theta) const;
private: private:
/** Some printing functions for debugging */ /** Some printing functions for debugging */
static void PrintNonlinearFactor(const gtsam::NonlinearFactor::shared_ptr& factor, static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
static void PrintLinearFactor(const gtsam::GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering, static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// static void PrintSingleClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering,
// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
//
// static void PrintRecursiveClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering,
// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
//
// static void PrintBayesTree(const gtsam::ISAM2& bayesTree, const gtsam::Ordering& ordering,
// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
// typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
// static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "");
}; // ConcurrentBatchSmoother }; // ConcurrentBatchSmoother

19
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -22,6 +22,7 @@
#include <gtsam_unstable/base/dllexport.h> #include <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
namespace gtsam { namespace gtsam {
@ -35,6 +36,14 @@ void GTSAM_UNSTABLE_EXPORT synchronize(ConcurrentFilter& filter, ConcurrentSmoot
* The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture. * The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture.
*/ */
class GTSAM_UNSTABLE_EXPORT ConcurrentFilter { class GTSAM_UNSTABLE_EXPORT ConcurrentFilter {
public:
typedef boost::shared_ptr<ConcurrentFilter> shared_ptr;
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0;
protected: protected:
@ -89,7 +98,15 @@ protected:
/** /**
* The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture. * The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture.
*/ */
class ConcurrentSmoother { class GTSAM_UNSTABLE_EXPORT ConcurrentSmoother {
public:
typedef boost::shared_ptr<ConcurrentSmoother> shared_ptr;
/** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0;
protected: protected: