Starting to clean up and refactor the Concurrent Filtering and Smoothing classes. Currently the synchronization is disables, as is the marginalization in the filter.
Stephen Williams
parent
d6e0cae6f5
commit
0b5c07e543
File diff suppressed because it is too large
Load Diff
|
|
@ -19,10 +19,8 @@
|
|||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include "ConcurrentFilteringAndSmoothing.h"
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -33,37 +31,61 @@ namespace gtsam {
|
|||
class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter {
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<ConcurrentBatchFilter> shared_ptr;
|
||||
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
|
||||
*/
|
||||
// TODO: Think of some more things to put here
|
||||
/** Meta information returned about the update */
|
||||
struct Result {
|
||||
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 linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
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 */
|
||||
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters, double lag) : parameters_(parameters), lag_(lag) {};
|
||||
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters) : parameters_(parameters) {};
|
||||
|
||||
/** Default destructor */
|
||||
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;
|
||||
|
||||
/** 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.
|
||||
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
|
||||
*/
|
||||
Values calculateEstimate() const {
|
||||
return theta_;
|
||||
return getLinearizationPoint().retract(getDelta(), getOrdering());
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
|
|
@ -72,8 +94,10 @@ public:
|
|||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(const Key key) const {
|
||||
return theta_.at<VALUE>(key);
|
||||
VALUE calculateEstimate(Key key) const {
|
||||
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
|
||||
* 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 remove from the filter and
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
|
||||
const KeyTimestampMap& timestamps = KeyTimestampMap());
|
||||
const boost::optional<FastList<Key> >& keysToMove = boost::none);
|
||||
|
||||
protected:
|
||||
|
||||
/** A typedef defining an Key-Factor mapping **/
|
||||
typedef std::map<Key, std::set<Index> > FactorIndex;
|
||||
|
||||
LevenbergMarquardtParams parameters_; ///< LM parameters
|
||||
double lag_; ///< Time before keys are transitioned from the filter to the smoother
|
||||
NonlinearFactorGraph graph_; ///< The graph of all the smoother factors
|
||||
Values theta_; ///< Current solution
|
||||
TimestampKeyMap timestampKeyMap_; ///< The set of keys associated with each timestamp
|
||||
KeyTimestampMap keyTimestampMap_; ///< The timestamp associated with each key
|
||||
NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
|
||||
Values theta_; ///< Current linearization point of all variables in the filter
|
||||
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
|
||||
VectorValues delta_; ///< The current set of linear deltas from the linearization point
|
||||
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
|
||||
FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key
|
||||
|
||||
std::vector<size_t> smootherSummarizationSlots_; ///< The slots in graph for the last set of smoother summarized factors
|
||||
Values separatorValues_;
|
||||
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
|
||||
|
||||
// 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
|
||||
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
|
||||
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 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
|
||||
|
|
@ -152,47 +171,34 @@ protected:
|
|||
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:
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -18,38 +18,32 @@
|
|||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
///* ************************************************************************* */
|
||||
//void ConcurrentBatchSmoother::SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent) {
|
||||
// std::cout << indent << "P( ";
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// if(clique->conditional()->nrParents() > 0) {
|
||||
// std::cout << "| ";
|
||||
// }
|
||||
// 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, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
std::cout << " Factors:" << std::endl;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) {
|
||||
PrintNonlinearFactor(factor, " ", keyFormatter);
|
||||
}
|
||||
theta_.print("Values:\n");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::print(const std::string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
graph_.print("Factors:\n");
|
||||
theta_.print("Values:\n");
|
||||
bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) const {
|
||||
const ConcurrentBatchSmoother* smoother = dynamic_cast<const ConcurrentBatchSmoother*>(&rhs);
|
||||
return smoother
|
||||
&& factors_.equals(smoother->factors_)
|
||||
&& 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);
|
||||
|
||||
// Create result structure
|
||||
// Create the return result meta-data
|
||||
Result result;
|
||||
|
||||
// Update all of the internal variables with the new information
|
||||
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);
|
||||
// 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);
|
||||
|
||||
// Optimize the graph, updating theta
|
||||
gttic(optimize);
|
||||
if(graph_.size() > 0){
|
||||
optimize();
|
||||
// Reorder the system to ensure efficient optimization (and marginalization) performance
|
||||
gttic(reorder);
|
||||
reorder();
|
||||
gttoc(reorder);
|
||||
|
||||
// TODO: fill in the results structure properly
|
||||
result.iterations = 0;
|
||||
result.nonlinearVariables = theta_.size() - separatorValues_.size();
|
||||
result.linearVariables = separatorValues_.size();
|
||||
result.error = 0;
|
||||
// Optimize the factors using a modified version of L-M
|
||||
gttic(optimize);
|
||||
if(factors_.size() > 0) {
|
||||
result = 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);
|
||||
// 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);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -223,7 +270,6 @@ void ConcurrentBatchSmoother::presync() {
|
|||
|
||||
gttic(presync);
|
||||
|
||||
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
|
|
@ -245,23 +291,43 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
|
|||
gttic(synchronize);
|
||||
|
||||
// Remove the previous filter summarization from the graph
|
||||
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
|
||||
removeFactor(slot);
|
||||
}
|
||||
filterSummarizationSlots_.clear();
|
||||
removeFactors(filterSummarizationSlots_);
|
||||
|
||||
// Insert the new filter summarized factors
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) {
|
||||
filterSummarizationSlots_.push_back(insertFactor(factor));
|
||||
// Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
|
||||
std::vector<size_t> dims;
|
||||
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
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors) {
|
||||
insertFactor(factor);
|
||||
}
|
||||
insertFactors(smootherFactors);
|
||||
|
||||
// Insert new linpoints into the values
|
||||
theta_.insert(smootherValues);
|
||||
// Insert the new filter summarized factors
|
||||
filterSummarizationSlots_ = insertFactors(summarizedFactors);
|
||||
|
||||
// Update the list of root keys
|
||||
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);
|
||||
|
||||
// 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
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
|
||||
size_t slot;
|
||||
if(availableSlots_.size() > 0) {
|
||||
slot = availableSlots_.front();
|
||||
availableSlots_.pop();
|
||||
graph_.replace(slot, factor);
|
||||
factors_.replace(slot, factor);
|
||||
} else {
|
||||
slot = graph_.size();
|
||||
graph_.push_back(factor);
|
||||
slot = factors_.size();
|
||||
factors_.push_back(factor);
|
||||
}
|
||||
// Update the FactorIndex
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
factorIndex_[key].insert(slot);
|
||||
slots.push_back(slot);
|
||||
}
|
||||
|
||||
// Augment the Variable Index
|
||||
variableIndex_.augment(*factors.symbolic(ordering_));
|
||||
|
||||
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);
|
||||
|
||||
// Remove references to this factor from the FactorIndex
|
||||
BOOST_FOREACH(Key key, *(graph_.at(slot))) {
|
||||
factorIndex_[key].erase(slot);
|
||||
}
|
||||
// Remove this factor from the graph
|
||||
graph_.remove(slot);
|
||||
// Mark the factor slot as avaiable
|
||||
// For each factor slot to delete...
|
||||
SymbolicFactorGraph factors;
|
||||
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 references to this factor from the VariableIndex
|
||||
variableIndex_.remove(slots, factors);
|
||||
|
||||
gttoc(remove_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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());
|
||||
void ConcurrentBatchSmoother::reorder() {
|
||||
|
||||
// Initialize all variables to group0
|
||||
std::vector<int> cmember(variableIndex_.size(), 0);
|
||||
|
||||
// Set all of the separator keys to Group1
|
||||
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 {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
|
||||
|
||||
return factorSlots;
|
||||
}
|
||||
// Create output result structure
|
||||
Result result;
|
||||
result.nonlinearVariables = theta_.size() - separatorValues_.size();
|
||||
result.linearVariables = separatorValues_.size();
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::optimize() {
|
||||
// Set optimization parameters
|
||||
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");
|
||||
|
||||
if(debug) std::cout << "ConcurrentBatchSmoother::optimize() Start" << std::endl;
|
||||
|
||||
// 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_);
|
||||
// Create a Values that holds the current evaluation point
|
||||
Values evalpoint = theta_.retract(delta_, ordering_);
|
||||
result.error = factors_.error(evalpoint);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError = graph_.error(evalpoint);
|
||||
double previousError;
|
||||
gtsam::VectorValues newDelta;
|
||||
size_t iterations = 0;
|
||||
double lambda = parameters_.lambdaInitial;
|
||||
VectorValues newDelta;
|
||||
do {
|
||||
previousError = currentError;
|
||||
previousError = result.error;
|
||||
|
||||
// Do next iteration
|
||||
gttic(optimizer_iteration);
|
||||
{
|
||||
// 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
|
||||
while(true) {
|
||||
// Add prior factors at the current solution
|
||||
gttic(damp);
|
||||
gtsam::GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
|
||||
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
|
||||
dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
|
||||
{
|
||||
// for each of the variables, add a prior at the current solution
|
||||
for(size_t j=0; j<delta_.size(); ++j) {
|
||||
gtsam::Matrix A = lambda * gtsam::eye(delta_[j].size());
|
||||
gtsam::Vector b = lambda * delta_[j];
|
||||
gtsam::SharedDiagonal model = gtsam::noiseModel::Unit::Create(delta_[j].size());
|
||||
gtsam::GaussianFactor::shared_ptr prior(new gtsam::JacobianFactor(j, A, b, model));
|
||||
Matrix A = lambda * eye(delta_[j].size());
|
||||
Vector b = lambda * delta_[j];
|
||||
SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size());
|
||||
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
|
||||
dampedFactorGraph.push_back(prior);
|
||||
}
|
||||
}
|
||||
gttoc(damp);
|
||||
result.lambdas++;
|
||||
|
||||
gttic(solve);
|
||||
// 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
|
||||
gttic(retract);
|
||||
evalpoint = theta_.retract(newDelta, ordering_);
|
||||
gttoc(retract);
|
||||
gttoc(solve);
|
||||
|
||||
// Evaluate the new error
|
||||
gttic(compute_error);
|
||||
double newError = graph_.error(evalpoint);
|
||||
double error = factors_.error(evalpoint);
|
||||
gttoc(compute_error);
|
||||
|
||||
if(newError < currentError) {
|
||||
if(error < result.error) {
|
||||
// Keep this change
|
||||
// Update the error value
|
||||
currentError = newError;
|
||||
result.error = error;
|
||||
// Update the linearization point
|
||||
theta_ = evalpoint;
|
||||
// Reset the deltas to zeros
|
||||
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) {
|
||||
theta_.update(separatorValues_);
|
||||
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
gtsam::Index index = ordering_.at(key_value.key);
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
Index index = ordering_.at(key_value.key);
|
||||
delta_.at(index) = newDelta.at(index);
|
||||
}
|
||||
}
|
||||
// Decrease lambda for next time
|
||||
lambda /= parameters_.lambdaFactor;
|
||||
if(lambda < (0.5 / parameters_.lambdaUpperBound)) {
|
||||
lambda = (0.5 / parameters_.lambdaUpperBound);
|
||||
lambda /= lambdaFactor;
|
||||
if(lambda < lambdaLowerBound) {
|
||||
lambda = lambdaLowerBound;
|
||||
}
|
||||
// End this lambda search iteration
|
||||
break;
|
||||
} else {
|
||||
// Reject this change
|
||||
// Increase lambda and continue searching
|
||||
lambda *= parameters_.lambdaFactor;
|
||||
if(lambda > parameters_.lambdaUpperBound) {
|
||||
lambda *= lambdaFactor;
|
||||
if(lambda > lambdaUpperBound) {
|
||||
// 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;
|
||||
break;
|
||||
|
|
@ -448,90 +522,23 @@ void ConcurrentBatchSmoother::optimize() {
|
|||
}
|
||||
gttoc(optimizer_iteration);
|
||||
|
||||
++iterations;
|
||||
} while(iterations < parameters_.maxIterations &&
|
||||
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol,
|
||||
previousError, currentError, gtsam::NonlinearOptimizerParams::SILENT));
|
||||
result.iterations++;
|
||||
} while(result.iterations < maxIterations &&
|
||||
!checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, 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 {
|
||||
|
||||
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) {
|
||||
void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) {
|
||||
std::cout << indent;
|
||||
if(factor) {
|
||||
if(boost::dynamic_pointer_cast<gtsam::LinearContainerFactor>(factor)) {
|
||||
if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
|
||||
std::cout << "l( ";
|
||||
} else {
|
||||
std::cout << "f( ";
|
||||
}
|
||||
BOOST_FOREACH(gtsam::Key key, *factor) {
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
std::cout << keyFormatter(key) << " ";
|
||||
}
|
||||
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;
|
||||
if(factor) {
|
||||
std::cout << "g( ";
|
||||
BOOST_FOREACH(gtsam::Index index, *factor) {
|
||||
BOOST_FOREACH(Index index, *factor) {
|
||||
std::cout << keyFormatter(ordering.key(index)) << " ";
|
||||
}
|
||||
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( ";
|
||||
// BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()){
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
// std::cout << keyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// if(clique->conditional()->nrParents() > 0){
|
||||
// std::cout << "| ";
|
||||
// BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()){
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->parents()){
|
||||
// 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
|
||||
// PrintSingleClique(clique, ordering, indent, keyFormatter);
|
||||
//
|
||||
// // 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);
|
||||
// }
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//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;
|
||||
// if (bayesTree.root().use_count() == 0) {
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -32,19 +31,17 @@ namespace gtsam {
|
|||
class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
|
||||
typedef ConcurrentSmoother Base; ///< typedef for base class
|
||||
|
||||
/**
|
||||
* Meta information returned about the update
|
||||
*/
|
||||
// TODO: Think of some more things to put here
|
||||
/** Meta information returned about the update */
|
||||
struct Result {
|
||||
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 linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
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 */
|
||||
|
|
@ -53,14 +50,42 @@ public:
|
|||
/** Default destructor */
|
||||
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;
|
||||
|
||||
/** 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.
|
||||
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
|
||||
*/
|
||||
Values calculateEstimate() const {
|
||||
return theta_;
|
||||
return getLinearizationPoint().retract(getDelta(), getOrdering());
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
|
|
@ -69,8 +94,10 @@ public:
|
|||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(const Key key) const {
|
||||
return theta_.at<VALUE>(key);
|
||||
VALUE calculateEstimate(Key key) const {
|
||||
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:
|
||||
|
||||
/** A typedef defining an Key-Factor mapping **/
|
||||
typedef std::map<Key, std::set<Index> > FactorIndex;
|
||||
|
||||
LevenbergMarquardtParams parameters_; ///< LM parameters
|
||||
NonlinearFactorGraph graph_; ///< The graph of all the smoother factors
|
||||
Values theta_; ///< Current linearization point
|
||||
Ordering ordering_; ///< The current ordering used to generate the deltas
|
||||
VectorValues delta_; ///< Current set of offsets from the linearization point
|
||||
Values separatorValues_; ///< The set of keys to be kept in the root and their linearization points
|
||||
NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
|
||||
Values theta_; ///< Current linearization point of all variables in the smoother
|
||||
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
|
||||
VectorValues delta_; ///< The current set of linear deltas from the linearization point
|
||||
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
|
||||
FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key
|
||||
std::vector<size_t> filterSummarizationSlots_; ///< The slots in graph for the last set of filter summarized factors
|
||||
Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
|
||||
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
|
||||
|
||||
/**
|
||||
|
|
@ -130,7 +156,7 @@ protected:
|
|||
* @param rootValues The linearization point of the root variables
|
||||
*/
|
||||
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.
|
||||
|
|
@ -138,48 +164,33 @@ protected:
|
|||
*/
|
||||
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
|
||||
* @return The slot in the graph it was inserted into
|
||||
* @param factors The factors to add to the graph
|
||||
* @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 */
|
||||
void removeFactor(size_t slot);
|
||||
/** 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);
|
||||
|
||||
/** Optimize the graph using a modified version of L-M */
|
||||
void optimize();
|
||||
/** Use colamd to update into an efficient ordering */
|
||||
void reorder();
|
||||
|
||||
/** 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 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;
|
||||
/** Use a modified version of L-M to update the linearization point and delta */
|
||||
Result optimize();
|
||||
|
||||
private:
|
||||
/** Some printing functions for debugging */
|
||||
|
||||
static void PrintNonlinearFactor(const gtsam::NonlinearFactor::shared_ptr& factor,
|
||||
const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
|
||||
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
|
||||
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
static void PrintLinearFactor(const gtsam::GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering,
|
||||
const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::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 = "");
|
||||
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
|
||||
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
}; // ConcurrentBatchSmoother
|
||||
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
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.
|
||||
*/
|
||||
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:
|
||||
|
||||
|
|
@ -89,7 +98,15 @@ protected:
|
|||
/**
|
||||
* 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:
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue