Added Fixed Lag Smoother and Concurrent Filtering and Smoothing to gtsam_unstable
Stephen Williams
parent
bc16edd2ac
commit
6fef6cf7d5
|
|
@ -6,7 +6,7 @@ set (gtsam_unstable_subdirs
|
|||
discrete
|
||||
# linear
|
||||
dynamics
|
||||
# nonlinear
|
||||
nonlinear
|
||||
slam
|
||||
navigation
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,563 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file BatchFixedLagSmoother.cpp
|
||||
* @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2
|
||||
* interface. However, additional parameters, such as the smoother lag and the timestamp associated
|
||||
* with each variable are needed.
|
||||
*
|
||||
* @author Michael Kaess, Stephen Williams
|
||||
* @date Oct 14, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::string& label) {
|
||||
std::cout << label;
|
||||
BOOST_FOREACH(gtsam::Key key, keys) {
|
||||
std::cout << " " << gtsam::DefaultKeyFormatter(key);
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) {
|
||||
std::cout << "f(";
|
||||
BOOST_FOREACH(Index index, factor->keys()) {
|
||||
std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]";
|
||||
}
|
||||
std::cout << " )" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) {
|
||||
std::cout << label << std::endl;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
|
||||
PrintSymbolicFactor(factor, ordering);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) {
|
||||
std::cout << "f(";
|
||||
if(factor) {
|
||||
BOOST_FOREACH(Key key, factor->keys()) {
|
||||
std::cout << " " << gtsam::DefaultKeyFormatter(key);
|
||||
}
|
||||
} else {
|
||||
std::cout << " NULL";
|
||||
}
|
||||
std::cout << " )" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) {
|
||||
std::cout << label << std::endl;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) {
|
||||
PrintSymbolicFactor(factor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
BatchFixedLagSmoother::BatchFixedLagSmoother(const LevenbergMarquardtParams& params, double smootherLag, bool enforceConsistency) :
|
||||
parameters_(params), smootherLag_(smootherLag), enforceConsistency_(enforceConsistency) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool BatchFixedLagSmoother::equals(const BatchFixedLagSmoother& rhs, double tol) const {
|
||||
return factors_.equals(rhs.factors_, tol)
|
||||
&& theta_.equals(rhs.theta_, tol)
|
||||
&& std::fabs(smootherLag_ - rhs.smootherLag_) < tol
|
||||
&& std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix BatchFixedLagSmoother::marginalR(Key key) const {
|
||||
// TODO: Fix This
|
||||
// // Use iSAM2 object to get the marginal factor
|
||||
// GaussianFactor::shared_ptr marginalGaussian;
|
||||
// if(params().getFactorization() == "QR")
|
||||
// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateQR);
|
||||
// else if(params().getFactorization() == "CHOLESKY")
|
||||
// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateCholesky);
|
||||
// else
|
||||
// throw std::invalid_argument(boost::str(boost::format("Encountered unknown factorization type of '%s'. Known types are 'QR' and 'CHOLESKY'.") % params().getFactorization()));
|
||||
//
|
||||
// // Extract the information matrix
|
||||
// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalGaussian);
|
||||
// assert(marginalJacobian != 0);
|
||||
// return marginalJacobian->getA(marginalJacobian->begin());
|
||||
return Matrix();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix BatchFixedLagSmoother::marginalInformation(Key key) const {
|
||||
Matrix R(marginalR(key));
|
||||
return R.transpose() * R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const {
|
||||
Matrix Rinv(inverse(marginalR(key)));
|
||||
return Rinv * Rinv.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BatchFixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
|
||||
|
||||
const bool debug = ISDEBUG("BatchFixedLagSmoother update");
|
||||
if(debug) {
|
||||
std::cout << "BatchFixedLagSmoother::update() START" << std::endl;
|
||||
}
|
||||
|
||||
// Add the new factors
|
||||
updateFactors(newFactors);
|
||||
|
||||
// Add the new variables
|
||||
theta_.insert(newTheta);
|
||||
|
||||
// Update the Timestamps associated with the factor keys
|
||||
updateKeyTimestampMap(timestamps);
|
||||
|
||||
// Get current timestamp
|
||||
double current_timestamp = getCurrentTimestamp();
|
||||
if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl;
|
||||
|
||||
// Find the set of variables to be marginalized out
|
||||
std::set<Key> marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_);
|
||||
if(debug) {
|
||||
std::cout << "Marginalizable Keys: ";
|
||||
BOOST_FOREACH(Key key, marginalizableKeys) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
// Marginalize out these variables.
|
||||
// This removes any factors that touch marginalized variables and adds new linear(ized) factors to the graph
|
||||
marginalizeKeys(marginalizableKeys);
|
||||
|
||||
// Create the optimizer
|
||||
Values linpoint;
|
||||
linpoint.insert(theta_);
|
||||
if(enforceConsistency_ && linearizedKeys_.size() > 0) {
|
||||
linpoint.update(linearizedKeys_);
|
||||
}
|
||||
LevenbergMarquardtOptimizer optimizer(factors_, linpoint, parameters_);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError;
|
||||
do {
|
||||
// Do next iteration
|
||||
currentError = optimizer.error();
|
||||
optimizer.iterate();
|
||||
|
||||
// Force variables associated with linearized factors to keep the same linearization point
|
||||
if(enforceConsistency_ && linearizedKeys_.size() > 0) {
|
||||
// Put the old values of the linearized keys back into the optimizer state
|
||||
optimizer.state().values.update(linearizedKeys_);
|
||||
optimizer.state().error = factors_.error(optimizer.state().values);
|
||||
}
|
||||
|
||||
// Maybe show output
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues");
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl;
|
||||
} while(optimizer.iterations() < parameters_.maxIterations &&
|
||||
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol,
|
||||
parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity));
|
||||
|
||||
// Update the Values from the optimizer
|
||||
theta_ = optimizer.values();
|
||||
|
||||
// Create result structure
|
||||
Result result;
|
||||
result.iterations = optimizer.state().iterations;
|
||||
result.linearVariables = linearizedKeys_.size();
|
||||
result.nonlinearVariables = theta_.size() - linearizedKeys_.size();
|
||||
result.error = optimizer.state().error;
|
||||
|
||||
if(debug) {
|
||||
std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::updateFactors(const NonlinearFactorGraph& newFactors) {
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
|
||||
Index index;
|
||||
// Insert the factor into an existing hole in the factor graph, if possible
|
||||
if(availableSlots_.size() > 0) {
|
||||
index = availableSlots_.front();
|
||||
availableSlots_.pop();
|
||||
factors_.replace(index, factor);
|
||||
} else {
|
||||
index = factors_.size();
|
||||
factors_.push_back(factor);
|
||||
}
|
||||
// Update the FactorIndex
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
factorIndex_[key].insert(index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& deleteFactors) {
|
||||
BOOST_FOREACH(size_t slot, deleteFactors) {
|
||||
if(factors_.at(slot)) {
|
||||
// Remove references to this factor from the FactorIndex
|
||||
BOOST_FOREACH(Key key, *(factors_.at(slot))) {
|
||||
factorIndex_[key].erase(slot);
|
||||
}
|
||||
// Remove the factor from the factor graph
|
||||
factors_.remove(slot);
|
||||
// Add the factor's old slot to the list of available slots
|
||||
availableSlots_.push(slot);
|
||||
} else {
|
||||
// TODO: Throw an error??
|
||||
std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) {
|
||||
// Loop through each key and add/update it in the map
|
||||
BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) {
|
||||
// Check to see if this key already exists inthe database
|
||||
KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);
|
||||
|
||||
// If the key already exists
|
||||
if(keyIter != keyTimestampMap_.end()) {
|
||||
// Find the entry in the Timestamp-Key database
|
||||
std::pair<TimestampKeyMap::iterator,TimestampKeyMap::iterator> range = timestampKeyMap_.equal_range(keyIter->second);
|
||||
TimestampKeyMap::iterator timeIter = range.first;
|
||||
while(timeIter->second != key_timestamp.first) {
|
||||
++timeIter;
|
||||
}
|
||||
// remove the entry in the Timestamp-Key database
|
||||
timestampKeyMap_.erase(timeIter);
|
||||
// insert an entry at the new time
|
||||
timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
|
||||
// update the Key-Timestamp database
|
||||
keyIter->second = key_timestamp.second;
|
||||
} else {
|
||||
// Add the Key-Timestamp database
|
||||
keyTimestampMap_.insert(key_timestamp);
|
||||
// Add the key to the Timestamp-Key database
|
||||
timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double BatchFixedLagSmoother::getCurrentTimestamp() const {
|
||||
if(timestampKeyMap_.size() > 0) {
|
||||
return timestampKeyMap_.rbegin()->first;
|
||||
} else {
|
||||
return -std::numeric_limits<double>::max();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Key> BatchFixedLagSmoother::findKeysBefore(double timestamp) const {
|
||||
std::set<Key> keys;
|
||||
TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp);
|
||||
for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) {
|
||||
keys.insert(iter->second);
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
|
||||
|
||||
// bool debug = true;
|
||||
|
||||
// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() START" << std::endl;
|
||||
|
||||
// if(debug) PrintKeySet(keys, "Keys To Erase: ");
|
||||
|
||||
BOOST_FOREACH(Key key, keys) {
|
||||
// if(debug) std::cout << "Attempting to erase key " << DefaultKeyFormatter(key) << std::endl;
|
||||
|
||||
// Erase the key from the Timestamp->Key map
|
||||
double timestamp = keyTimestampMap_.at(key);
|
||||
|
||||
// if(debug) std::cout << "Timestamp associated with key: " << timestamp << std::endl;
|
||||
|
||||
TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp);
|
||||
while(iter != timestampKeyMap_.end() && iter->first == timestamp) {
|
||||
if(iter->second == key) {
|
||||
|
||||
// if(debug) {
|
||||
// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl;
|
||||
// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) {
|
||||
// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl;
|
||||
// }
|
||||
// }
|
||||
|
||||
timestampKeyMap_.erase(iter++);
|
||||
|
||||
// if(debug) {
|
||||
// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl;
|
||||
// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) {
|
||||
// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if(debug) std::cout << "Erased 1 entry from timestampKeyMap_" << std::endl;
|
||||
} else {
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
// Erase the key from the Key->Timestamp map
|
||||
size_t ret;
|
||||
ret = keyTimestampMap_.erase(key);
|
||||
// if(debug) std::cout << "Erased " << ret << " entries from keyTimestampMap_" << std::endl;
|
||||
|
||||
// Erase the key from the values
|
||||
theta_.erase(key);
|
||||
// if(debug) std::cout << "(Hopefully) Erased 1 entries from theta_" << std::endl;
|
||||
|
||||
// Erase the key from the factor index
|
||||
ret = factorIndex_.erase(key);
|
||||
// if(debug) std::cout << "Erased " << ret << " entries from factorIndex_" << std::endl;
|
||||
|
||||
// Erase the key from the set of linearized keys
|
||||
if(linearizedKeys_.exists(key)) {
|
||||
linearizedKeys_.erase(key);
|
||||
// if(debug) std::cout << "Erased 1 entry from linearizedKeys_" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() FINISH" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
struct FactorTree {
|
||||
std::set<Index> factors;
|
||||
std::set<Key> keys;
|
||||
|
||||
FactorTree(const std::set<Index>& factors, const NonlinearFactorGraph& allFactors) : factors(factors) {
|
||||
BOOST_FOREACH(const Index& factor, factors) {
|
||||
BOOST_FOREACH(Key key, *(allFactors.at(factor))) {
|
||||
keys.insert(key);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void push_back(const FactorTree& factorTree) {
|
||||
factors.insert(factorTree.factors.begin(), factorTree.factors.end());
|
||||
keys.insert(factorTree.keys.begin(), factorTree.keys.end());
|
||||
}
|
||||
|
||||
bool hasCommonKeys(Index factor, const NonlinearFactorGraph& allFactors) {
|
||||
const NonlinearFactor::shared_ptr& f = allFactors.at(factor);
|
||||
std::set<Key>::const_iterator iter = std::find_first_of(keys.begin(), keys.end(), f->begin(), f->end());
|
||||
return iter != keys.end();
|
||||
}
|
||||
|
||||
template <class ForwardIterator>
|
||||
bool hasCommonKeys(ForwardIterator first, ForwardIterator last, const NonlinearFactorGraph& allFactors) {
|
||||
for(ForwardIterator factor = first; factor != last; ++factor) {
|
||||
if(hasCommonKeys(*factor, allFactors))
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BatchFixedLagSmoother::marginalizeKeys(const std::set<Key>& marginalizableKeys) {
|
||||
|
||||
const bool debug = ISDEBUG("BatchFixedLagSmoother update");
|
||||
if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() START" << std::endl;
|
||||
|
||||
|
||||
// In order to marginalize out the selected variables, the factors involved in those variables
|
||||
// must be identified and removed from iSAM2. Also, the effect of those removed factors on the
|
||||
// remaining variables needs to be accounted for. This will be done with linear(ized) factors from
|
||||
// a partial clique marginalization (or from the iSAM2 cached factor if the entire clique is removed).
|
||||
// This function finds the set of factors to be removed and generates the linearized factors that
|
||||
// must be added.
|
||||
|
||||
if(marginalizableKeys.size() > 0) {
|
||||
|
||||
if(debug) PrintKeySet(marginalizableKeys, "Marginalizable Keys:");
|
||||
|
||||
// Find all of the factors associated with marginalizable variables. This set of factors may form a forest.
|
||||
typedef std::list<FactorTree> FactorForest;
|
||||
FactorForest factorForest;
|
||||
BOOST_FOREACH(Key key, marginalizableKeys) {
|
||||
|
||||
if(debug) std::cout << "Looking for factors involving key " << DefaultKeyFormatter(key) << std::endl;
|
||||
|
||||
// Get the factors associated with this variable
|
||||
const std::set<size_t>& factors = factorIndex_.at(key);
|
||||
|
||||
if(debug) { std::cout << "Found the following factors:" << std::endl; BOOST_FOREACH(size_t i, factors) { std::cout << " "; PrintSymbolicFactor(factors_.at(i)); } }
|
||||
|
||||
// Loop over existing factor trees, looking for common keys
|
||||
std::vector<FactorForest::iterator> commonTrees;
|
||||
for(FactorForest::iterator tree = factorForest.begin(); tree != factorForest.end(); ++tree) {
|
||||
if(tree->hasCommonKeys(factors.begin(), factors.end(), factors_)) {
|
||||
commonTrees.push_back(tree);
|
||||
}
|
||||
}
|
||||
|
||||
if(debug) std::cout << "Found " << commonTrees.size() << " common trees." << std::endl;
|
||||
|
||||
if(commonTrees.size() == 0) {
|
||||
// No common trees were found. Create a new one.
|
||||
factorForest.push_back(FactorTree(factors, factors_));
|
||||
|
||||
if(debug) std::cout << "Created a new tree." << std::endl;
|
||||
|
||||
} else {
|
||||
// Extract the last common tree
|
||||
FactorForest::iterator commonTree = commonTrees.back();
|
||||
commonTrees.pop_back();
|
||||
// Merge the current factors into this tree
|
||||
commonTree->push_back(FactorTree(factors, factors_));
|
||||
// Merge all other common trees into this one, deleting the other trees from the forest.
|
||||
BOOST_FOREACH(FactorForest::iterator& tree, commonTrees) {
|
||||
commonTree->push_back(*tree);
|
||||
factorForest.erase(tree);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(debug) std::cout << "Found " << factorForest.size() << " factor trees in the set of removed factors." << std::endl;
|
||||
|
||||
// For each tree in the forest:
|
||||
// (0) construct an ordering for the tree
|
||||
// (1) construct a linear factor graph
|
||||
// (2) solve for the marginal factors
|
||||
// (3) convert the marginal factors into Linearized Factors
|
||||
// (4) remove the marginalized factors from the graph
|
||||
// (5) add the factors in this tree to the graph
|
||||
BOOST_FOREACH(const FactorTree& factorTree, factorForest) {
|
||||
// (0) construct an ordering for this tree
|
||||
// The ordering should place the marginalizable keys first, then the remaining keys
|
||||
Ordering ordering;
|
||||
|
||||
std::set<Key> marginalizableTreeKeys;
|
||||
std::set_intersection(factorTree.keys.begin(), factorTree.keys.end(),
|
||||
marginalizableKeys.begin(), marginalizableKeys.end(),
|
||||
std::inserter(marginalizableTreeKeys, marginalizableTreeKeys.end()));
|
||||
|
||||
std::set<Key> remainingTreeKeys;
|
||||
std::set_difference(factorTree.keys.begin(), factorTree.keys.end(),
|
||||
marginalizableTreeKeys.begin(), marginalizableTreeKeys.end(),
|
||||
std::inserter(remainingTreeKeys, remainingTreeKeys.end()));
|
||||
|
||||
// TODO: It may be worthwhile to use CCOLAMD here. (but maybe not???)
|
||||
BOOST_FOREACH(Key key, marginalizableTreeKeys) {
|
||||
ordering.push_back(key);
|
||||
}
|
||||
BOOST_FOREACH(Key key, remainingTreeKeys) {
|
||||
ordering.push_back(key);
|
||||
}
|
||||
|
||||
// (1) construct a linear factor graph
|
||||
GaussianFactorGraph graph;
|
||||
BOOST_FOREACH(size_t factor, factorTree.factors) {
|
||||
graph.push_back( factors_.at(factor)->linearize(theta_, ordering) );
|
||||
}
|
||||
|
||||
if(debug) PrintSymbolicGraph(graph, ordering, "Factor Tree:");
|
||||
|
||||
// (2) solve for the marginal factors
|
||||
// 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, marginalizableTreeKeys) {
|
||||
variables.push_back(ordering.at(key));
|
||||
}
|
||||
std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph> result = graph.eliminate(variables);
|
||||
graph = result.second;
|
||||
|
||||
if(debug) PrintSymbolicGraph(graph, ordering, "Factors on Remaining Variables:");
|
||||
|
||||
// (3) convert the marginal factors into Linearized Factors
|
||||
NonlinearFactorGraph newFactors;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, graph) {
|
||||
// These factors are all generated from BayesNet conditionals. They should all be Jacobians.
|
||||
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(gaussianFactor);
|
||||
assert(jacobianFactor);
|
||||
LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, getLinearizationPoint()));
|
||||
// add it to the new factor set
|
||||
newFactors.push_back(factor);
|
||||
}
|
||||
|
||||
if(debug) std::cout << "1" << std::endl;
|
||||
|
||||
// (4) remove the marginalized factors from the graph
|
||||
removeFactors(factorTree.factors);
|
||||
|
||||
if(debug) std::cout << "2" << std::endl;
|
||||
|
||||
// (5) add the factors in this tree to the main set of factors
|
||||
updateFactors(newFactors);
|
||||
|
||||
if(debug) std::cout << "3" << std::endl;
|
||||
|
||||
// (6) add the keys involved in the linear(ized) factors to the linearizedKey list
|
||||
FastSet<Key> linearizedKeys = newFactors.keys();
|
||||
BOOST_FOREACH(Key key, linearizedKeys) {
|
||||
if(!linearizedKeys_.exists(key)) {
|
||||
linearizedKeys_.insert(key, theta_.at(key));
|
||||
}
|
||||
}
|
||||
|
||||
if(debug) std::cout << "4" << std::endl;
|
||||
}
|
||||
|
||||
if(debug) std::cout << "5" << std::endl;
|
||||
|
||||
// Store the final estimate of each marginalized key
|
||||
BOOST_FOREACH(Key key, marginalizableKeys) {
|
||||
thetaFinal_.insert(key, theta_.at(key));
|
||||
}
|
||||
|
||||
// Remove the marginalized keys from the smoother data structures
|
||||
eraseKeys(marginalizableKeys);
|
||||
|
||||
if(debug) std::cout << "6" << std::endl;
|
||||
}
|
||||
|
||||
if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,227 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file BatchFixedLagSmoother.h
|
||||
* @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2
|
||||
* interface. However, additional parameters, such as the smoother lag and the timestamp associated
|
||||
* with each variable are needed.
|
||||
*
|
||||
* @author Michael Kaess, Stephen Williams
|
||||
* @date Oct 14, 2012
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class BatchFixedLagSmoother {
|
||||
|
||||
public:
|
||||
|
||||
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
|
||||
typedef boost::shared_ptr<BatchFixedLagSmoother> shared_ptr;
|
||||
|
||||
/// Typedef for a Key-Timestamp map/database
|
||||
typedef std::map<Key, double> KeyTimestampMap;
|
||||
typedef std::multimap<double, Key> TimestampKeyMap;
|
||||
|
||||
/**
|
||||
* Meta information returned about the update
|
||||
*/
|
||||
// TODO: Think of some more things to put here
|
||||
struct Result {
|
||||
size_t iterations; ///< The number of optimizer iterations performed
|
||||
size_t nonlinearVariables; ///< The number of variables that can be relinearized
|
||||
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
double error; ///< The final factor graph error
|
||||
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
|
||||
};
|
||||
|
||||
|
||||
/** default constructor */
|
||||
BatchFixedLagSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), double smootherLag = 0.0, bool enforceConsistency = false);
|
||||
|
||||
/** destructor */
|
||||
virtual ~BatchFixedLagSmoother() { };
|
||||
|
||||
// TODO: Create a better 'print'
|
||||
/** Print the factor for debugging and testing (implementing Testable) */
|
||||
virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Check if two IncrementalFixedLagSmoother Objects are equal */
|
||||
virtual bool equals(const BatchFixedLagSmoother& rhs, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* Add new factors, updating the solution and relinearizing as needed.
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const KeyTimestampMap& timestamps = KeyTimestampMap());
|
||||
|
||||
/** Access the current set of timestamps associated with each variable */
|
||||
const KeyTimestampMap& getTimestamps() const {
|
||||
return keyTimestampMap_;
|
||||
}
|
||||
|
||||
/** Access the current linearization point */
|
||||
const Values& getLinearizationPoint() const {
|
||||
return theta_;
|
||||
}
|
||||
|
||||
/** Compute an estimate from the incomplete linear delta computed during the last update.
|
||||
* This delta is incomplete because it was not updated below wildfire_threshold. If only
|
||||
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
|
||||
*/
|
||||
Values calculateEstimate() const {
|
||||
//return theta_.retract(delta_, ordering_);
|
||||
return theta_;
|
||||
}
|
||||
|
||||
/** Compute an estimate for a single variable using its incomplete linear delta computed
|
||||
* during the last update. This is faster than calling the no-argument version of
|
||||
* calculateEstimate, which operates on all variables.
|
||||
* @param key
|
||||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(Key key) const {
|
||||
//const Index index = ordering_.at(key);
|
||||
//const SubVector delta = delta_.at(index);
|
||||
//return theta_.at<VALUE>(key).retract(delta);
|
||||
return theta_.at<VALUE>(key);
|
||||
}
|
||||
|
||||
/** Compute an estimate using a complete delta computed by a full back-substitution.
|
||||
*/
|
||||
Values calculateBestEstimate() const {
|
||||
return calculateEstimate();
|
||||
}
|
||||
|
||||
/** Return the final/best estimate of each variable encountered, including those that have been marginalized out previously.
|
||||
*/
|
||||
Values calculateFinalEstimate() const {
|
||||
Values final(thetaFinal_);
|
||||
final.insert(theta_);
|
||||
return final;
|
||||
}
|
||||
|
||||
|
||||
/** Access the set of nonlinear factors */
|
||||
const NonlinearFactorGraph& getFactorsUnsafe() const {
|
||||
return factors_;
|
||||
}
|
||||
|
||||
/** return the current set of iSAM2 parameters */
|
||||
const LevenbergMarquardtParams& params() const {
|
||||
return parameters_;
|
||||
}
|
||||
|
||||
/** return the current smoother lag */
|
||||
double smootherLag() const {
|
||||
return smootherLag_;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** return the marginal square root information matrix associated with a specific variable */
|
||||
Matrix marginalR(Key key) const;
|
||||
|
||||
/** return the marginal information matrix associated with a specific variable */
|
||||
Matrix marginalInformation(Key key) const;
|
||||
|
||||
/** return the marginal covariance associated with a specific variable */
|
||||
Matrix marginalCovariance(Key key) const;
|
||||
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
/** The nonlinear factors **/
|
||||
NonlinearFactorGraph factors_;
|
||||
|
||||
/** The current linearization point **/
|
||||
Values theta_;
|
||||
|
||||
/** The final estimate of each variable **/
|
||||
Values thetaFinal_;
|
||||
|
||||
/** The L-M optimization parameters **/
|
||||
LevenbergMarquardtParams parameters_;
|
||||
|
||||
/** The length of the smoother lag. Any variable older than this amount will be marginalized out. */
|
||||
double smootherLag_;
|
||||
|
||||
/** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
|
||||
* linearization point of all variables involved in linearized/marginal factors at the edge of the
|
||||
* smoothing window. This idea is from ??? TODO: Look up paper reference **/
|
||||
bool enforceConsistency_;
|
||||
|
||||
/** The current timestamp associated with each tracked key */
|
||||
TimestampKeyMap timestampKeyMap_;
|
||||
KeyTimestampMap keyTimestampMap_;
|
||||
|
||||
/** A typedef defining an Key-Factor mapping **/
|
||||
typedef std::map<Key, std::set<Index> > FactorIndex;
|
||||
|
||||
/** A cross-reference structure to allow efficient factor lookups by key **/
|
||||
FactorIndex factorIndex_;
|
||||
|
||||
/** The set of keys involved in current linearized factors. These keys should not be relinearized. **/
|
||||
//std::set<Key> linearizedKeys_;
|
||||
Values linearizedKeys_;
|
||||
|
||||
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
|
||||
std::queue<size_t> availableSlots_;
|
||||
|
||||
|
||||
/** Augment the list of factors with a set of new factors */
|
||||
void updateFactors(const NonlinearFactorGraph& newFactors);
|
||||
|
||||
/** Remove factors from the list of factors by slot index */
|
||||
void removeFactors(const std::set<size_t>& deleteFactors);
|
||||
|
||||
/** 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;
|
||||
|
||||
/** Erase any keys associated with timestamps before the provided time */
|
||||
void eraseKeys(const std::set<Key>& keys);
|
||||
|
||||
/** Marginalize out selected variables */
|
||||
void marginalizeKeys(const std::set<Key>& marginalizableKeys);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
|
||||
|
||||
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
|
||||
|
||||
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label);
|
||||
|
||||
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
|
||||
|
||||
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
|
||||
|
||||
}; // BatchFixedLagSmoother
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,916 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentBatchFilter.cpp
|
||||
* @brief A Levenberg-Marquardt Batch Filter that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
||||
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::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 ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s;
|
||||
graph_.print("Factors:\n");
|
||||
theta_.print("Values:\n");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
|
||||
|
||||
gttic(update);
|
||||
|
||||
// Create the return result meta-data
|
||||
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);
|
||||
// Update the Timestamps associated with the factor keys
|
||||
updateKeyTimestampMap(timestamps);
|
||||
gttoc(augment_system);
|
||||
|
||||
// Optimize the graph, updating theta
|
||||
gttic(optimize);
|
||||
if(graph_.size() > 0) {
|
||||
// Create an L-M optimizer
|
||||
Values linpoint;
|
||||
linpoint.insert(theta_);
|
||||
if(separatorValues_.size() > 0) {
|
||||
linpoint.update(separatorValues_);
|
||||
}
|
||||
LevenbergMarquardtOptimizer optimizer(graph_, linpoint, parameters_);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError;
|
||||
do {
|
||||
// Do next iteration
|
||||
gttic(optimizer_iteration);
|
||||
currentError = optimizer.error();
|
||||
optimizer.iterate();
|
||||
gttoc(optimizer_iteration);
|
||||
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
gttic(enforce_consistency);
|
||||
if(separatorValues_.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(separatorValues_);
|
||||
optimizer.state().error = graph_.error(optimizer.state().values);
|
||||
}
|
||||
gttoc(enforce_consistency);
|
||||
|
||||
// Maybe show output
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues");
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl;
|
||||
} while(optimizer.iterations() < parameters_.maxIterations &&
|
||||
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol,
|
||||
parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity));
|
||||
|
||||
// Update the Values from the optimizer
|
||||
theta_ = optimizer.values();
|
||||
|
||||
result.iterations = optimizer.state().iterations;
|
||||
result.nonlinearVariables = theta_.size() - separatorValues_.size();
|
||||
result.linearVariables = separatorValues_.size();
|
||||
result.error = optimizer.state().error;
|
||||
}
|
||||
gttoc(optimize);
|
||||
|
||||
gttoc(update);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::presync() {
|
||||
|
||||
gttic(presync);
|
||||
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) {
|
||||
gttic(synchronize);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Begin" << std::endl;
|
||||
|
||||
//// Check that solution before marginalization matches solution after marginalization
|
||||
//Values solutionBefore;
|
||||
//solutionBefore.insert(theta_);
|
||||
//solutionBefore.update(smootherSeparatorValues_);
|
||||
//Ordering orderingBefore = *graph_.orderingCOLAMD(solutionBefore);
|
||||
//GaussianFactorGraph lingraphBefore = *graph_.linearize(solutionBefore, orderingBefore);
|
||||
//GaussianMultifrontalSolver gmsBefore(lingraphBefore, true);
|
||||
//VectorValues deltaBefore = *gmsBefore.optimize();
|
||||
//solutionBefore = solutionBefore.retract(deltaBefore, orderingBefore);
|
||||
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Input Smoother Summarization:" << std::endl;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) {
|
||||
std::cout << " f( ";
|
||||
BOOST_FOREACH(Key key, factor->keys()) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
// At this step, we need to update the summarized factors representing the smoother,
|
||||
// then update the solution by re-eliminating the nonlinear graph. In the process, we
|
||||
// will create a Bayes Tree and form a new root clique. We will let CCOLAMD define
|
||||
// the ordering, constraining the most recent variables (variables > lag) to the
|
||||
// top of the tree.
|
||||
//
|
||||
// Once formed, we then need to determine which branches will be sent to the smoother,
|
||||
// which branches constitute the filter, and which clique(s) form the root.
|
||||
//
|
||||
// Ideally, all of the old variables will be in the smoother. But, depending on the
|
||||
// factors and the ordering, there may be some cliques with recent and old variables.
|
||||
// So, we define the smoother branches as those remaining after removing any clique
|
||||
// with any recent variables.
|
||||
//
|
||||
// The root should be the smallest set of variables that separate the smoother from
|
||||
// the rest of the graph. Thus, the root will be the parents of all the smoother
|
||||
// branches (plus the path to the root to ensure a connected, full-rank system).
|
||||
//
|
||||
// The filter branches are then defined as the children of the root that are not
|
||||
// smoother branches.
|
||||
|
||||
|
||||
//std::cout << "Previous Graph:" << std::endl;
|
||||
//for(size_t i = 0; i < graph_.size(); ++i) {
|
||||
// if(graph_[i]) {
|
||||
// std::cout << " f" << i << "( ";
|
||||
// BOOST_FOREACH(Key key, graph_[i]->keys()) {
|
||||
// std::cout << DefaultKeyFormatter(key) << " ";
|
||||
// }
|
||||
// std::cout << ")" << std::endl;
|
||||
// }
|
||||
//}
|
||||
|
||||
// TODO: Temporarily remove smoother effects
|
||||
// Remove the previous smoother summarization from the graph
|
||||
BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) {
|
||||
removeFactor(slot);
|
||||
}
|
||||
smootherSummarizationSlots_.clear();
|
||||
|
||||
std::cout << "Removed Previous Smoother Summarization:" << std::endl;
|
||||
for(size_t i = 0; i < graph_.size(); ++i) {
|
||||
if(graph_[i]) {
|
||||
std::cout << " f" << i << "( ";
|
||||
BOOST_FOREACH(Key key, graph_[i]->keys()) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Temporarily remove smoother effects
|
||||
// Insert the updated smoother summarized factors
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) {
|
||||
smootherSummarizationSlots_.push_back(insertFactor(factor));
|
||||
}
|
||||
|
||||
std::cout << "Added New Smoother Summarization:" << std::endl;
|
||||
for(size_t i = 0; i < graph_.size(); ++i) {
|
||||
if(graph_[i]) {
|
||||
std::cout << " f" << i << "( ";
|
||||
BOOST_FOREACH(Key key, graph_[i]->keys()) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Temporarily remove smoother effects
|
||||
// Now that the smoother summarization has been updated, re-optimize the filter
|
||||
update();
|
||||
|
||||
|
||||
// The filter should now be optimal (up to the linearization point of the separator variables)
|
||||
// Calculate a new separator, a new filter summarization, a new smoother summarization, and the
|
||||
// set of factors to send to the smoother
|
||||
|
||||
|
||||
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
// TODO: This may be too many variables. It may only require that the smoother separator keys be kept constant.
|
||||
gttic(enforce_consistency);
|
||||
Values linpoint = theta_;
|
||||
if(separatorValues_.size() > 0) {
|
||||
linpoint.update(separatorValues_);
|
||||
}
|
||||
gttoc(enforce_consistency);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Old Smoother Separator Keys: ";
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Old Root Keys: ";
|
||||
//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) {
|
||||
// std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
//}
|
||||
//std::cout << std::endl;
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) All Keys: ";
|
||||
//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linpoint) {
|
||||
// std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
//}
|
||||
//std::cout << std::endl;
|
||||
|
||||
|
||||
// Calculate an ordering that puts variables newer than LAG at the top
|
||||
gttic(compute_ordering);
|
||||
std::set<Key> recentKeys = findKeysAfter(getCurrentTimestamp() - lag_);
|
||||
std::map<Key, int> constraints;
|
||||
BOOST_FOREACH(Key key, recentKeys) {
|
||||
constraints[key] = 1;
|
||||
}
|
||||
Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints);
|
||||
gttoc(compute_ordering);
|
||||
|
||||
//typedef std::map<Key, int> Constraints;
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Constraints: ";
|
||||
//BOOST_FOREACH(const Constraints::value_type key_group, constraints) {
|
||||
// std::cout << DefaultKeyFormatter(key_group.first) << " ";
|
||||
//}
|
||||
//std::cout << std::endl;
|
||||
|
||||
//ordering.print("ConcurrentBatchFilter::synchronize(*) Ordering:\n");
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Create Bayes Tree:" << std::endl;
|
||||
//SymbolicPrintTree(root, ordering, " ");
|
||||
|
||||
// Find all of the filter cliques, and the smoother branches
|
||||
// The smoother branches are defined as the children filter cliques that are not also filter cliques
|
||||
gttic(identify_smoother_branches);
|
||||
std::set<ISAM2Clique::shared_ptr> filterCliques;
|
||||
std::set<ISAM2Clique::shared_ptr> smootherBranches;
|
||||
BOOST_FOREACH(Key key, recentKeys) {
|
||||
Index index = ordering.at(key);
|
||||
const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index);
|
||||
if(clique) {
|
||||
// Add all of the child-cliques to the smoother branches
|
||||
filterCliques.insert(clique);
|
||||
smootherBranches.insert(clique->children().begin(), clique->children().end());
|
||||
}
|
||||
}
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& filterClique, filterCliques) {
|
||||
smootherBranches.erase(filterClique);
|
||||
}
|
||||
gttoc(identify_smoother_branches);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Branches:" << std::endl;
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
|
||||
std::cout << " 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;
|
||||
}
|
||||
|
||||
// Find all of the frontal keys in the smoother branches
|
||||
gttic(find_smoother_keys);
|
||||
std::set<Key> smootherKeys;
|
||||
std::queue<ISAM2Clique::shared_ptr> smootherCliques;
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
|
||||
smootherCliques.push(clique);
|
||||
}
|
||||
while(smootherCliques.size() > 0) {
|
||||
// Extract the frontal keys from the next clique
|
||||
BOOST_FOREACH(Index index, smootherCliques.front()->conditional()->frontals()) {
|
||||
smootherKeys.insert(ordering.key(index));
|
||||
}
|
||||
// Add any children to the queue
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, smootherCliques.front()->children()) {
|
||||
smootherCliques.push(child);
|
||||
}
|
||||
// Remove this clique from the queue
|
||||
smootherCliques.pop();
|
||||
}
|
||||
// Store the current linearization point for future transmission to the smoother (skip any variables in the previous root)
|
||||
smootherValues_.clear();
|
||||
BOOST_FOREACH(Key key, smootherKeys) {
|
||||
smootherValues_.insert(key, linpoint.at(key));
|
||||
}
|
||||
gttoc(find_smoother_keys);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Keys: ";
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) {
|
||||
std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// Find the set of smoother separator keys
|
||||
// The separator keys are defined as the set of parents to current branches
|
||||
// + the parents of previous branches that are not new smoother keys
|
||||
gttic(update_smoother_separator);
|
||||
BOOST_FOREACH(Key key, smootherKeys) {
|
||||
if(separatorValues_.exists(key)) {
|
||||
separatorValues_.erase(key);
|
||||
}
|
||||
}
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
|
||||
BOOST_FOREACH(Index index, clique->conditional()->parents()) {
|
||||
Key key = ordering.key(index);
|
||||
if(!separatorValues_.exists(key))
|
||||
separatorValues_.insert(key, linpoint.at(key));
|
||||
}
|
||||
}
|
||||
gttoc(update_smoother_separator);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) New Smoother Separator Keys: ";
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// Calculate the new smoother summarization.
|
||||
// This is potentially a combination of the previous summarization and cached factors from new smoother branches
|
||||
gttic(extract_smoother_summarization);
|
||||
// Add in previous summarization, marginalizing out non-smoother-separator keys
|
||||
std::set<Key> smootherSeparatorKeys;
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
|
||||
smootherSeparatorKeys.insert(key_value.key);
|
||||
}
|
||||
NonlinearFactorGraph smootherSummarization;
|
||||
BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) {
|
||||
if(smootherBranches.size() == 0) {
|
||||
graph_.at(slot)->print("Summarized Factor Before Marginalization:\n");
|
||||
NonlinearFactor::shared_ptr marginalFactor = marginalizeKeysFromFactor(graph_.at(slot), smootherSeparatorKeys);
|
||||
if(marginalFactor) {
|
||||
smootherSummarization.push_back(marginalFactor);
|
||||
}
|
||||
if(marginalFactor) {
|
||||
marginalFactor->print("Summarized Factor After Marginalization:\n");
|
||||
} else {
|
||||
std::cout << "Summarized Factor After Marginalization:\n{NULL}" << std::endl;
|
||||
}
|
||||
}
|
||||
removeFactor(slot);
|
||||
}
|
||||
smootherSummarizationSlots_.clear();
|
||||
// Insert cached factors for any smoother branches
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
|
||||
clique->cachedFactor()->print("Summarized Cached Factor:\n");
|
||||
LinearizedGaussianFactor::shared_ptr factor;
|
||||
if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<JacobianFactor>(clique->cachedFactor()))
|
||||
factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint));
|
||||
else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<HessianFactor>(clique->cachedFactor()))
|
||||
factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint));
|
||||
else
|
||||
throw std::invalid_argument("In ConcurrentBatchFilter::process(...), cached factor is neither a JacobianFactor nor a HessianFactor");
|
||||
factor->print("Summarized Linearized Factor:\n");
|
||||
smootherSummarization.push_back(factor);
|
||||
}
|
||||
gttoc(extract_smoother_summarization);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Summarization:" << std::endl;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization) {
|
||||
if(boost::dynamic_pointer_cast<LinearizedGaussianFactor>(factor))
|
||||
std::cout << " L( ";
|
||||
else
|
||||
std::cout << " f( ";
|
||||
BOOST_FOREACH(Key key, factor->keys()) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
// Find all of the factors that contain at least one smoother key
|
||||
// These nonlinear factors will be sent to the smoother
|
||||
gttic(find_smoother_factors);
|
||||
std::set<size_t> smootherFactorSlots = findFactorsWithAny(smootherKeys);
|
||||
// Convert the set of slots into a set of factors
|
||||
smootherFactors_.resize(0);
|
||||
BOOST_FOREACH(size_t slot, smootherFactorSlots) {
|
||||
smootherFactors_.push_back(graph_.at(slot));
|
||||
}
|
||||
gttoc(find_smoother_factors);
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Factors:" << std::endl;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors_) {
|
||||
if(boost::dynamic_pointer_cast<LinearizedGaussianFactor>(factor))
|
||||
std::cout << " L( ";
|
||||
else
|
||||
std::cout << " f( ";
|
||||
BOOST_FOREACH(Key key, factor->keys()) {
|
||||
std::cout << DefaultKeyFormatter(key) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
// Find the Root Cliques
|
||||
// The root is defined as the parents of the smoother separator + the path to the root
|
||||
gttic(identify_root_cliques);
|
||||
std::set<ISAM2Clique::shared_ptr> rootCliques;
|
||||
BOOST_FOREACH(Key key, smootherSeparatorKeys) {
|
||||
Index index = ordering.at(key);
|
||||
const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index);
|
||||
if(clique) {
|
||||
rootCliques.insert(clique);
|
||||
}
|
||||
}
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) {
|
||||
ISAM2Clique::shared_ptr rootClique = clique;
|
||||
while(!rootClique->isRoot()) {
|
||||
rootClique = rootClique->parent();
|
||||
rootCliques.insert(rootClique);
|
||||
}
|
||||
}
|
||||
std::set<Key> rootKeys;
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) {
|
||||
BOOST_FOREACH(Index index, clique->conditional()->frontals()) {
|
||||
rootKeys.insert(ordering.key(index));
|
||||
}
|
||||
}
|
||||
gttoc(identify_root_cliques);
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Root Cliques:" << std::endl;
|
||||
//BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) {
|
||||
//std::cout << " 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;
|
||||
//}
|
||||
|
||||
// Update the keys and linearization points of the new root
|
||||
gttic(update_root_values);
|
||||
rootValues_.clear();
|
||||
BOOST_FOREACH(Key key, rootKeys) {
|
||||
rootValues_.insert(key, linpoint.at(key));
|
||||
}
|
||||
gttoc(update_root_values);
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) New Root Keys: ";
|
||||
//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) {
|
||||
// std::cout << DefaultKeyFormatter(key_value.key) << " ";
|
||||
//}
|
||||
//std::cout << std::endl;
|
||||
|
||||
// Identify the filter branches
|
||||
// The filter branches are the children of the root that are not smoother branches.
|
||||
gttic(identify_filter_branches);
|
||||
std::set<ISAM2Clique::shared_ptr> filterBranches;
|
||||
// Find the children of the root
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) {
|
||||
filterBranches.insert(clique->children().begin(), clique->children().end());
|
||||
}
|
||||
// Remove any root cliques that made it into the filter branches
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) {
|
||||
filterBranches.erase(clique);
|
||||
}
|
||||
// Remove the smoother branches
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) {
|
||||
filterBranches.erase(clique);
|
||||
}
|
||||
gttoc(identify_filter_branches);
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Filter Branches:" << std::endl;
|
||||
//BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, filterBranches) {
|
||||
// std::cout << " 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;
|
||||
//}
|
||||
|
||||
// Extract cached factors from the filter branches and store for future transmission to the smoother
|
||||
gttic(extract_filter_summarization);
|
||||
filterSummarization_.resize(0);
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, filterBranches) {
|
||||
LinearizedGaussianFactor::shared_ptr factor;
|
||||
if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<JacobianFactor>(clique->cachedFactor()))
|
||||
factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint));
|
||||
else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<HessianFactor>(clique->cachedFactor()))
|
||||
factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint));
|
||||
else
|
||||
throw std::invalid_argument("In ConcurrentBatchFilter::process(...), cached factor is neither a JacobianFactor nor a HessianFactor");
|
||||
filterSummarization_.push_back(factor);
|
||||
}
|
||||
gttoc(extract_filter_summarization);
|
||||
|
||||
// Find all factors that contain only root clique variables
|
||||
// This information is not contained in either the smoother
|
||||
// summarization or the filter summarization. This must be sent
|
||||
// to the smoother as well. Including it as part of the
|
||||
// root/filter summarization
|
||||
gttic(extract_root_summarization);
|
||||
std::set<size_t> rootFactorSlots = findFactorsWithOnly(rootKeys);
|
||||
BOOST_FOREACH(size_t slot, rootFactorSlots) {
|
||||
filterSummarization_.push_back(graph_.at(slot));
|
||||
}
|
||||
gttoc(extract_root_summarization);
|
||||
|
||||
//std::cout << "ConcurrentBatchFilter::synchronize(*) Filter Summarization:" << std::endl;
|
||||
//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, filterSummarization_) {
|
||||
// if(boost::dynamic_pointer_cast<LinearizedGaussianFactor>(factor))
|
||||
// std::cout << " L( ";
|
||||
// else
|
||||
// std::cout << " f( ";
|
||||
// BOOST_FOREACH(Key key, factor->keys()) {
|
||||
// std::cout << DefaultKeyFormatter(key) << " ";
|
||||
// }
|
||||
// std::cout << ")" << std::endl;
|
||||
//}
|
||||
|
||||
gttic(purge_smoother_information);
|
||||
// Remove nonlinear factors being sent to the smoother
|
||||
BOOST_FOREACH(size_t slot, smootherFactorSlots) {
|
||||
removeFactor(slot);
|
||||
}
|
||||
|
||||
// Remove the nonlinear keys being sent to the smoother
|
||||
BOOST_FOREACH(Key key, smootherKeys) {
|
||||
removeKey(key);
|
||||
}
|
||||
|
||||
// Add all of the smoother summarization factors to the filter, keeping track of their locations for later removal
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization) {
|
||||
smootherSummarizationSlots_.push_back(insertFactor(factor));
|
||||
}
|
||||
gttoc(purge_smoother_information);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// // Check that solution before marginalization matches solution after marginalization
|
||||
// Values solutionAfter;
|
||||
// solutionAfter.insert(theta_);
|
||||
// solutionAfter.update(smootherSeparatorValues_);
|
||||
// Ordering orderingAfter = *graph_.orderingCOLAMD(solutionAfter);
|
||||
// GaussianFactorGraph lingraphAfter = *graph_.linearize(solutionAfter, orderingAfter);
|
||||
// GaussianMultifrontalSolver gmsAfter(lingraphAfter, true);
|
||||
// VectorValues deltaAfter = *gmsAfter.optimize();
|
||||
// solutionAfter = solutionAfter.retract(deltaAfter, orderingAfter);
|
||||
//
|
||||
// BOOST_FOREACH(Key key, smootherKeys) {
|
||||
// solutionBefore.erase(key);
|
||||
// }
|
||||
//
|
||||
//std::cout << "Solution before marginalization matches solution after marginalization: " << solutionBefore.equals(solutionAfter, 1.0e-9) << std::endl;
|
||||
|
||||
std::cout << "ConcurrentBatchFilter::synchronize(*) End" << std::endl;
|
||||
|
||||
gttoc(synchronize);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) {
|
||||
|
||||
gttic(get_summarized_factors);
|
||||
|
||||
// Copy the previous calculated smoother summarization factors into the output
|
||||
summarizedFactors.push_back(filterSummarization_);
|
||||
rootValues.insert(rootValues_);
|
||||
|
||||
gttic(get_summarized_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) {
|
||||
|
||||
gttic(get_smoother_factors);
|
||||
|
||||
// Copy the previous calculated smoother summarization factors into the output
|
||||
smootherFactors.push_back(smootherFactors_);
|
||||
smootherValues.insert(smootherValues_);
|
||||
|
||||
gttic(get_smoother_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::postsync() {
|
||||
|
||||
gttic(postsync);
|
||||
|
||||
gttoc(postsync);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
size_t ConcurrentBatchFilter::insertFactor(const NonlinearFactor::shared_ptr& factor) {
|
||||
|
||||
gttic(insert_factors);
|
||||
|
||||
// Insert the factor into an existing hole in the factor graph, if possible
|
||||
size_t slot;
|
||||
if(availableSlots_.size() > 0) {
|
||||
slot = availableSlots_.front();
|
||||
availableSlots_.pop();
|
||||
graph_.replace(slot, factor);
|
||||
} else {
|
||||
slot = graph_.size();
|
||||
graph_.push_back(factor);
|
||||
}
|
||||
// Update the FactorIndex
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
factorIndex_[key].insert(slot);
|
||||
}
|
||||
|
||||
gttoc(insert_factors);
|
||||
|
||||
return slot;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::removeFactor(size_t slot) {
|
||||
|
||||
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
|
||||
availableSlots_.push(slot);
|
||||
|
||||
gttoc(remove_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::removeKey(Key key) {
|
||||
|
||||
gttic(remove_keys);
|
||||
|
||||
// Erase the key from the Timestamp->Key map
|
||||
double timestamp = keyTimestampMap_.at(key);
|
||||
|
||||
TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp);
|
||||
while(iter != timestampKeyMap_.end() && iter->first == timestamp) {
|
||||
if(iter->second == key) {
|
||||
timestampKeyMap_.erase(iter++);
|
||||
} else {
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
// Erase the key from the Key->Timestamp map
|
||||
keyTimestampMap_.erase(key);
|
||||
|
||||
// Erase the key from the values
|
||||
theta_.erase(key);
|
||||
|
||||
// Erase the key from the set of linearized keys
|
||||
// TODO: Should this ever even occur?
|
||||
if(rootValues_.exists(key)) {
|
||||
rootValues_.erase(key);
|
||||
}
|
||||
|
||||
gttoc(remove_keys);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchFilter::updateKeyTimestampMap(const KeyTimestampMap& timestamps) {
|
||||
|
||||
gttic(update_key_timestamp_map);
|
||||
|
||||
// Loop through each key and add/update it in the map
|
||||
BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) {
|
||||
// Check to see if this key already exists inthe database
|
||||
KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);
|
||||
|
||||
// If the key already exists
|
||||
if(keyIter != keyTimestampMap_.end()) {
|
||||
// Find the entry in the Timestamp-Key database
|
||||
std::pair<TimestampKeyMap::iterator,TimestampKeyMap::iterator> range = timestampKeyMap_.equal_range(keyIter->second);
|
||||
TimestampKeyMap::iterator timeIter = range.first;
|
||||
while(timeIter->second != key_timestamp.first) {
|
||||
++timeIter;
|
||||
}
|
||||
// remove the entry in the Timestamp-Key database
|
||||
timestampKeyMap_.erase(timeIter);
|
||||
// insert an entry at the new time
|
||||
timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
|
||||
// update the Key-Timestamp database
|
||||
keyIter->second = key_timestamp.second;
|
||||
} else {
|
||||
// Add the Key-Timestamp database
|
||||
keyTimestampMap_.insert(key_timestamp);
|
||||
// Add the key to the Timestamp-Key database
|
||||
timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first));
|
||||
}
|
||||
}
|
||||
|
||||
gttoc(update_key_timestamp_map);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double ConcurrentBatchFilter::getCurrentTimestamp() const {
|
||||
|
||||
gttic(get_current_timestamp);
|
||||
|
||||
double timestamp;
|
||||
if(timestampKeyMap_.size() > 0) {
|
||||
timestamp = timestampKeyMap_.rbegin()->first;
|
||||
} else {
|
||||
timestamp = -std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
gttoc(get_current_timestamp);
|
||||
|
||||
return timestamp;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Key> ConcurrentBatchFilter::findKeysBefore(double timestamp) const {
|
||||
|
||||
gttic(find_keys_before);
|
||||
|
||||
std::set<Key> keys;
|
||||
TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp);
|
||||
for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) {
|
||||
keys.insert(iter->second);
|
||||
}
|
||||
|
||||
gttoc(find_keys_before);
|
||||
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Key> ConcurrentBatchFilter::findKeysAfter(double timestamp) const {
|
||||
|
||||
gttic(find_keys_after);
|
||||
|
||||
std::set<Key> keys;
|
||||
TimestampKeyMap::const_iterator begin = timestampKeyMap_.lower_bound(timestamp);
|
||||
for(TimestampKeyMap::const_iterator iter = begin; iter != timestampKeyMap_.end(); ++iter) {
|
||||
keys.insert(iter->second);
|
||||
}
|
||||
|
||||
gttoc(find_keys_after);
|
||||
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<size_t> ConcurrentBatchFilter::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> ConcurrentBatchFilter::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 ConcurrentBatchFilter::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set<Key>& remainingKeys) const {
|
||||
|
||||
const bool debug = false;
|
||||
if(debug) std::cout << "ConcurrentBatchFilter::marginalizeKeys() START" << std::endl;
|
||||
|
||||
LinearizedGaussianFactor::shared_ptr linearizedFactor = boost::dynamic_pointer_cast<LinearizedGaussianFactor>(factor);
|
||||
assert(linearizedFactor);
|
||||
|
||||
// Sort the keys for this factor
|
||||
std::set<Key> factorKeys;
|
||||
BOOST_FOREACH(Key key, *linearizedFactor) {
|
||||
factorKeys.insert(key);
|
||||
}
|
||||
|
||||
// Calculate the set of keys to marginalize
|
||||
std::set<Key> marginalizeKeys;
|
||||
std::set_difference(factorKeys.begin(), factorKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
|
||||
|
||||
//
|
||||
if(marginalizeKeys.size() == 0) {
|
||||
// No keys need to be marginalized out. Simply return the original factor.
|
||||
return linearizedFactor;
|
||||
} else if(marginalizeKeys.size() == linearizedFactor->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);
|
||||
}
|
||||
|
||||
// (1) construct a linear factor graph
|
||||
GaussianFactorGraph graph;
|
||||
graph.push_back( linearizedFactor->linearize(linearizedFactor->linearizationPoint(), ordering) );
|
||||
|
||||
// (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);
|
||||
graph = result.second;
|
||||
|
||||
// (3) convert the marginal factors into Linearized Factors
|
||||
NonlinearFactor::shared_ptr marginalFactor;
|
||||
assert(graph.size() <= 1);
|
||||
if(graph.size() > 0) {
|
||||
// These factors are all generated from BayesNet conditionals. They should all be Jacobians.
|
||||
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(graph.at(0));
|
||||
assert(jacobianFactor);
|
||||
marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, linearizedFactor->linearizationPoint()));
|
||||
}
|
||||
return marginalFactor;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentBatchFilter.h
|
||||
* @brief A Levenberg-Marquardt Batch Filter that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include "ConcurrentFilteringAndSmoothing.h"
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface.
|
||||
*/
|
||||
class ConcurrentBatchFilter : public ConcurrentFilter {
|
||||
|
||||
public:
|
||||
|
||||
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
|
||||
struct Result {
|
||||
size_t iterations; ///< The number of optimizer iterations performed
|
||||
size_t nonlinearVariables; ///< The number of variables that can be relinearized
|
||||
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
double error; ///< The final factor graph error
|
||||
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
|
||||
};
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters, double lag) : parameters_(parameters), lag_(lag) {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentBatchFilter() {};
|
||||
|
||||
// Implement a GTSAM standard 'print' function
|
||||
void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** 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_;
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
* calling the no-argument version of calculateEstimate if only specific variables are needed.
|
||||
* @param key
|
||||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(const Key key) const {
|
||||
return theta_.at<VALUE>(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors and variables to the filter.
|
||||
*
|
||||
* Add new measurements, and optionally new variables, to the filter.
|
||||
* This runs a full update step of the derived filter algorithm
|
||||
*
|
||||
* @param newFactors The new factors to be added to the smoother
|
||||
* @param newTheta Initialization points for new variables to be added to the filter
|
||||
* You must include here all new variables occurring in newFactors that were not already
|
||||
* in the filter.
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
|
||||
const KeyTimestampMap& timestamps = KeyTimestampMap());
|
||||
|
||||
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
|
||||
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_;
|
||||
|
||||
// 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
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync();
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the filter branch summarization
|
||||
* needed by the smoother. Also, linearization points for the new root clique must be provided.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
* @param rootValues The linearization points of the root clique variables
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues);
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors being sent to the smoother from the filter. These
|
||||
* may be original nonlinear factors, or factors encoding a summarization of the filter information.
|
||||
* The specifics will be implementation-specific for a given filter.
|
||||
*
|
||||
* @param smootherFactors The new factors to be added to the smoother
|
||||
* @param smootherValues The linearization points of any new variables
|
||||
*/
|
||||
virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues);
|
||||
|
||||
/**
|
||||
* Apply the updated version of the smoother branch summarized factors.
|
||||
*
|
||||
* @param summarizedFactors An updated version of the smoother branch summarized factors
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& summarizedFactors);
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
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;
|
||||
|
||||
private:
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "");
|
||||
|
||||
}; // ConcurrentBatchFilter
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,470 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentBatchSmoother.cpp
|
||||
* @brief A Levenberg-Marquardt Batch Smoother that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/base/timing.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;
|
||||
graph_.print("Factors:\n");
|
||||
theta_.print("Values:\n");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) {
|
||||
|
||||
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){
|
||||
// Create an L-M optimizer
|
||||
Values linpoint;
|
||||
linpoint.insert(theta_);
|
||||
if(rootValues_.size() > 0) {
|
||||
linpoint.insert(rootValues_);
|
||||
}
|
||||
LevenbergMarquardtOptimizer optimizer(graph_, linpoint, parameters_);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError;
|
||||
do {
|
||||
// Do next iteration
|
||||
gttic(optimizer_iteration);
|
||||
currentError = optimizer.error();
|
||||
optimizer.iterate();
|
||||
gttoc(optimizer_iteration);
|
||||
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
gttic(enforce_consistency);
|
||||
if(rootValues_.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(rootValues_);
|
||||
optimizer.state().error = graph_.error(optimizer.state().values);
|
||||
}
|
||||
gttoc(enforce_consistency);
|
||||
|
||||
// Maybe show output
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues");
|
||||
if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl;
|
||||
} while(optimizer.iterations() < parameters_.maxIterations &&
|
||||
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol,
|
||||
parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity));
|
||||
|
||||
// Update theta from the optimizer, then remove root variables
|
||||
theta_ = optimizer.values();
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) {
|
||||
theta_.erase(key_value.key);
|
||||
}
|
||||
|
||||
result.iterations = optimizer.state().iterations;
|
||||
result.nonlinearVariables = theta_.size();
|
||||
result.linearVariables = rootValues_.size();
|
||||
result.error = optimizer.state().error;
|
||||
}
|
||||
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 root clique. This requires:
|
||||
// 1) Calculate an ordering that forces the rootKey variables to be in the root
|
||||
// 2) Perform an elimination, constructing a Bayes Tree from the currnet
|
||||
// variable values. This elimination will use the iSAM2 version of a clique so
|
||||
// that cached factors are stored
|
||||
// 3) Verify the root's cached factors involve only root keys; all others should
|
||||
// be marginalized
|
||||
// 4) Convert cached factors into 'Linearized' nonlinear factors
|
||||
|
||||
if(rootValues_.size() > 0) {
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
gttic(enforce_consistency);
|
||||
Values linpoint;
|
||||
linpoint.insert(theta_);
|
||||
linpoint.insert(rootValues_);
|
||||
|
||||
//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, rootValues_) {
|
||||
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, rootValues_) { 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, rootValues_) {
|
||||
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() - rootValues_.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) {
|
||||
LinearizedGaussianFactor::shared_ptr factor;
|
||||
if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<JacobianFactor>(gaussianFactor))
|
||||
factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint));
|
||||
else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast<HessianFactor>(gaussianFactor))
|
||||
factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint));
|
||||
else
|
||||
throw std::invalid_argument("In ConcurrentBatchSmoother::presync(...), cached factor is neither a JacobianFactor nor a HessianFactor");
|
||||
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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::presync() {
|
||||
|
||||
gttic(presync);
|
||||
|
||||
|
||||
gttoc(presync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) {
|
||||
|
||||
gttic(get_summarized_factors);
|
||||
|
||||
// Copy the previous calculated smoother summarization factors into the output
|
||||
summarizedFactors.push_back(smootherSummarization_);
|
||||
|
||||
gttic(get_summarized_factors);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
|
||||
const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) {
|
||||
|
||||
gttic(synchronize);
|
||||
|
||||
// Remove the previous filter summarization from the graph
|
||||
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
|
||||
removeFactor(slot);
|
||||
}
|
||||
filterSummarizationSlots_.clear();
|
||||
|
||||
// Insert the new filter summarized factors
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) {
|
||||
filterSummarizationSlots_.push_back(insertFactor(factor));
|
||||
}
|
||||
|
||||
// Insert the new smoother factors
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors) {
|
||||
insertFactor(factor);
|
||||
}
|
||||
|
||||
// Insert new linpoints into the values
|
||||
theta_.insert(smootherValues);
|
||||
|
||||
// Update the list of root keys
|
||||
rootValues_ = rootValues;
|
||||
|
||||
gttoc(synchronize);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::postsync() {
|
||||
|
||||
gttic(postsync);
|
||||
|
||||
gttoc(postsync);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
size_t ConcurrentBatchSmoother::insertFactor(const NonlinearFactor::shared_ptr& factor) {
|
||||
|
||||
gttic(insert_factor);
|
||||
|
||||
// Insert the factor into an existing hole in the factor graph, if possible
|
||||
size_t slot;
|
||||
if(availableSlots_.size() > 0) {
|
||||
slot = availableSlots_.front();
|
||||
availableSlots_.pop();
|
||||
graph_.replace(slot, factor);
|
||||
} else {
|
||||
slot = graph_.size();
|
||||
graph_.push_back(factor);
|
||||
}
|
||||
// Update the FactorIndex
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
factorIndex_[key].insert(slot);
|
||||
}
|
||||
|
||||
gttoc(insert_factors);
|
||||
|
||||
return slot;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ConcurrentBatchSmoother::removeFactor(size_t slot) {
|
||||
|
||||
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
|
||||
availableSlots_.push(slot);
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
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");
|
||||
// These factors are all generated from BayesNet conditionals. They should all be Jacobians.
|
||||
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(graph.at(0));
|
||||
assert(jacobianFactor);
|
||||
marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, theta));
|
||||
}
|
||||
marginalFactor->print("Factor After:\n");
|
||||
return marginalFactor;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,164 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentBatchSmoother.h
|
||||
* @brief A Levenberg-Marquardt Batch Smoother that implements the
|
||||
* Concurrent Filtering and Smoothing interface.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include "ConcurrentFilteringAndSmoothing.h"
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
|
||||
*/
|
||||
class ConcurrentBatchSmoother : public ConcurrentSmoother {
|
||||
|
||||
public:
|
||||
|
||||
typedef ConcurrentSmoother Base; ///< typedef for base class
|
||||
|
||||
/**
|
||||
* Meta information returned about the update
|
||||
*/
|
||||
// TODO: Think of some more things to put here
|
||||
struct Result {
|
||||
size_t iterations; ///< The number of optimizer iterations performed
|
||||
size_t nonlinearVariables; ///< The number of variables that can be relinearized
|
||||
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
|
||||
double error; ///< The final factor graph error
|
||||
Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
|
||||
};
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentBatchSmoother(const LevenbergMarquardtParams& parameters) : parameters_(parameters) {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentBatchSmoother() {};
|
||||
|
||||
// Implement a GTSAM standard 'print' function
|
||||
void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** 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_;
|
||||
}
|
||||
|
||||
/** Compute the current best estimate of a single variable. This is generally faster than
|
||||
* calling the no-argument version of calculateEstimate if only specific variables are needed.
|
||||
* @param key
|
||||
* @return
|
||||
*/
|
||||
template<class VALUE>
|
||||
VALUE calculateEstimate(const Key key) const {
|
||||
return theta_.at<VALUE>(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors and variables to the smoother.
|
||||
*
|
||||
* Add new measurements, and optionally new variables, to the smoother.
|
||||
* This runs a full step of the ISAM2 algorithm, relinearizing and updating
|
||||
* the solution as needed, according to the wildfire and relinearize
|
||||
* thresholds.
|
||||
*
|
||||
* @param newFactors The new factors to be added to the smoother
|
||||
* @param newTheta Initialization points for new variables to be added to the smoother
|
||||
* You must include here all new variables occuring in newFactors (which were not already
|
||||
* in the smoother). There must not be any variables here that do not occur in newFactors,
|
||||
* and additionally, variables that were already in the system must not be included here.
|
||||
*/
|
||||
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values());
|
||||
|
||||
|
||||
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 solution
|
||||
Values rootValues_; ///< The set of keys to be kept in the root and their linearization points
|
||||
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
|
||||
NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync();
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the smoother branch summarization
|
||||
* needed by the filter.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors);
|
||||
|
||||
/**
|
||||
* Apply the new smoother factors sent by the filter, and the updated version of the filter
|
||||
* branch summarized factors.
|
||||
*
|
||||
* @param smootherFactors A set of new factors added to the smoother from the filter
|
||||
* @param smootherValues Linearization points for any new variables
|
||||
* @param summarizedFactors An updated version of the filter branch summarized factors
|
||||
* @param rootValues The linearization point of the root variables
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
|
||||
const NonlinearFactorGraph& summarizedFactors, const Values& rootValues);
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void postsync();
|
||||
|
||||
/** Augment the graph with a new factor
|
||||
*
|
||||
* @param factors The factor to add to the graph
|
||||
* @return The slot in the graph it was inserted into
|
||||
*/
|
||||
size_t insertFactor(const NonlinearFactor::shared_ptr& factor);
|
||||
|
||||
/** Remove a factor from the graph by slot index */
|
||||
void removeFactor(size_t slot);
|
||||
|
||||
/** 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;
|
||||
|
||||
private:
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "");
|
||||
|
||||
}; // ConcurrentBatchSmoother
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentFilteringAndSmoothing.cpp
|
||||
* @brief Base classes for the 'filter' and 'smoother' portion of the Concurrent
|
||||
* Filtering and Smoothing architecture, as well as an external synchronization
|
||||
* function. These classes act as an interface only.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#include "ConcurrentFilteringAndSmoothing.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) {
|
||||
|
||||
NonlinearFactorGraph smootherFactors, filterSumarization, smootherSummarization;
|
||||
Values smootherValues, rootValues;
|
||||
|
||||
// Call the pre-sync functions of the filter and smoother
|
||||
filter.presync();
|
||||
smoother.presync();
|
||||
|
||||
// Get the updates from the smoother and apply them to the filter
|
||||
smoother.getSummarizedFactors(smootherSummarization);
|
||||
filter.synchronize(smootherSummarization);
|
||||
|
||||
// Get the updates from the filter and apply them to the smoother
|
||||
filter.getSmootherFactors(smootherFactors, smootherValues);
|
||||
filter.getSummarizedFactors(filterSumarization, rootValues);
|
||||
smoother.synchronize(smootherFactors, smootherValues, filterSumarization, rootValues);
|
||||
|
||||
// Call the post-sync functions of the filter and smoother
|
||||
filter.postsync();
|
||||
smoother.postsync();
|
||||
}
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ConcurrentFilteringAndSmoothing.h
|
||||
* @brief Base classes for the 'filter' and 'smoother' portion of the Concurrent
|
||||
* Filtering and Smoothing architecture, as well as an external synchronization
|
||||
* function. These classes act as an interface only.
|
||||
* @author Stephen Williams
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declare the Filter and Smoother classes for the 'synchronize' function
|
||||
class ConcurrentFilter;
|
||||
class ConcurrentSmoother;
|
||||
|
||||
void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
|
||||
|
||||
/**
|
||||
* The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture.
|
||||
*/
|
||||
class ConcurrentFilter {
|
||||
|
||||
protected:
|
||||
|
||||
friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentFilter() {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentFilter() {};
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync() {};
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the filter branch summarization
|
||||
* needed by the smoother. Also, linearization points for the new root clique must be provided.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
* @param rootValues The linearization points of the root clique variables
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) = 0;
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors being sent to the smoother from the filter. These
|
||||
* may be original nonlinear factors, or factors encoding a summarization of the filter information.
|
||||
* The specifics will be implementation-specific for a given filter.
|
||||
*
|
||||
* @param smootherFactors The new factors to be added to the smoother
|
||||
* @param smootherValues The linearization points of any new variables
|
||||
*/
|
||||
virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) = 0;
|
||||
|
||||
/**
|
||||
* Apply the updated version of the smoother branch summarized factors.
|
||||
*
|
||||
* @param summarizedFactors An updated version of the smoother branch summarized factors
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& summarizedFactors) = 0;
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void postsync() {};
|
||||
|
||||
}; // ConcurrentFilter
|
||||
|
||||
/**
|
||||
* The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture.
|
||||
*/
|
||||
class ConcurrentSmoother {
|
||||
|
||||
protected:
|
||||
|
||||
friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentSmoother() {};
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ConcurrentSmoother() {};
|
||||
|
||||
/**
|
||||
* Perform any required operations before the synchronization process starts.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void presync() {};
|
||||
|
||||
/**
|
||||
* Populate the provided containers with factors that constitute the smoother branch summarization
|
||||
* needed by the filter.
|
||||
*
|
||||
* @param summarizedFactors The summarized factors for the filter branch
|
||||
*/
|
||||
virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) = 0;
|
||||
|
||||
/**
|
||||
* Apply the new smoother factors sent by the filter, and the updated version of the filter
|
||||
* branch summarized factors.
|
||||
*
|
||||
* @param smootherFactors A set of new factors added to the smoother from the filter
|
||||
* @param smootherValues Linearization points for any new variables
|
||||
* @param summarizedFactors An updated version of the filter branch summarized factors
|
||||
* @param rootValues The linearization point of the root variables
|
||||
*/
|
||||
virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
|
||||
const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) = 0;
|
||||
|
||||
/**
|
||||
* Perform any required operations after the synchronization process finishes.
|
||||
* Called by 'synchronize'
|
||||
*/
|
||||
virtual void postsync() {};
|
||||
|
||||
}; // ConcurrentSmoother
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,281 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LinearizedFactor.cpp
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) {
|
||||
// Extract the keys and linearization points
|
||||
BOOST_FOREACH(const Index& idx, gaussian->keys()) {
|
||||
// find full symbol
|
||||
if (idx < ordering.size()) {
|
||||
Key key = ordering.key(idx);
|
||||
|
||||
// extract linearization point
|
||||
assert(lin_points.exists(key));
|
||||
this->lin_points_.insert(key, lin_points.at(key));
|
||||
|
||||
// store keys
|
||||
this->keys_.push_back(key);
|
||||
} else {
|
||||
throw std::runtime_error("LinearizedGaussianFactor: could not find index in decoder!");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
|
||||
const Ordering& ordering, const Values& lin_points)
|
||||
: Base(jacobian, ordering, lin_points), Ab_(matrix_) {
|
||||
|
||||
// Get the Ab matrix from the Jacobian factor, with any covariance baked in
|
||||
AbMatrix fullMatrix = jacobian->matrix_augmented(true);
|
||||
|
||||
// Create the dims array
|
||||
size_t dims[jacobian->size() + 1];
|
||||
size_t index = 0;
|
||||
for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
|
||||
dims[index++] = jacobian->getDim(iter);
|
||||
}
|
||||
dims[index] = 1;
|
||||
|
||||
// Update the BlockInfo accessor
|
||||
BlockAb Ab(fullMatrix, dims, dims+jacobian->size()+1);
|
||||
Ab.swap(Ab_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
|
||||
std::cout << s << std::endl;
|
||||
|
||||
std::cout << "Nonlinear Keys: ";
|
||||
BOOST_FOREACH(const Key& key, this->keys())
|
||||
std::cout << keyFormatter(key) << " ";
|
||||
std::cout << std::endl;
|
||||
|
||||
for(const_iterator key=begin(); key!=end(); ++key)
|
||||
std::cout << boost::format("A[%1%]=\n")%keyFormatter(*key) << A(*key) << std::endl;
|
||||
std::cout << "b=\n" << b() << std::endl;
|
||||
|
||||
lin_points_.print("Linearization Point: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LinearizedJacobianFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
if (e) {
|
||||
|
||||
Matrix thisMatrix = this->Ab_.range(0, Ab_.nBlocks());
|
||||
Matrix rhsMatrix = e->Ab_.range(0, Ab_.nBlocks());
|
||||
|
||||
return Base::equals(expected, tol)
|
||||
&& lin_points_.equals(e->lin_points_, tol)
|
||||
&& equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double LinearizedJacobianFactor::error(const Values& c) const {
|
||||
Vector errorVector = error_vector(c);
|
||||
return 0.5 * errorVector.dot(errorVector);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||
|
||||
// Create the 'terms' data structure for the Jacobian constructor
|
||||
std::vector<std::pair<Index, Matrix> > terms;
|
||||
BOOST_FOREACH(Key key, keys()) {
|
||||
terms.push_back(std::make_pair(ordering[key], this->A(key)));
|
||||
}
|
||||
|
||||
// compute rhs
|
||||
Vector b = -error_vector(c);
|
||||
|
||||
return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
|
||||
|
||||
Vector errorVector = -b();
|
||||
|
||||
BOOST_FOREACH(Key key, this->keys()) {
|
||||
const Value& newPt = c.at(key);
|
||||
const Value& linPt = lin_points_.at(key);
|
||||
Vector d = linPt.localCoordinates_(newPt);
|
||||
const constABlock A = this->A(key);
|
||||
errorVector += A * d;
|
||||
}
|
||||
|
||||
return errorVector;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
|
||||
const Ordering& ordering, const Values& lin_points)
|
||||
: Base(hessian, ordering, lin_points), info_(matrix_) {
|
||||
|
||||
// Copy the augmented matrix holding G, g, and f
|
||||
Matrix fullMatrix = hessian->info();
|
||||
|
||||
// Create the dims array
|
||||
size_t dims[hessian->size() + 1];
|
||||
size_t index = 0;
|
||||
for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) {
|
||||
dims[index++] = hessian->getDim(iter);
|
||||
}
|
||||
dims[index] = 1;
|
||||
|
||||
// Update the BlockInfo accessor
|
||||
BlockInfo infoMatrix(fullMatrix, dims, dims+hessian->size()+1);
|
||||
infoMatrix.swap(info_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
|
||||
std::cout << s << std::endl;
|
||||
|
||||
std::cout << "Nonlinear Keys: ";
|
||||
BOOST_FOREACH(const Key& key, this->keys())
|
||||
std::cout << keyFormatter(key) << " ";
|
||||
std::cout << std::endl;
|
||||
|
||||
gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
|
||||
|
||||
lin_points_.print("Linearization Point: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
if (e) {
|
||||
|
||||
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
|
||||
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
|
||||
Matrix rhsMatrix = e->info_.full().selfadjointView<Eigen::Upper>();
|
||||
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
|
||||
|
||||
return Base::equals(expected, tol)
|
||||
&& lin_points_.equals(e->lin_points_, tol)
|
||||
&& equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double LinearizedHessianFactor::error(const Values& c) const {
|
||||
|
||||
// Construct an error vector in key-order from the Values
|
||||
Vector dx = zero(dim());
|
||||
size_t index = 0;
|
||||
for(unsigned int i = 0; i < this->size(); ++i){
|
||||
Key key = this->keys()[i];
|
||||
const Value& newPt = c.at(key);
|
||||
const Value& linPt = lin_points_.at(key);
|
||||
dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
|
||||
index += linPt.dim();
|
||||
}
|
||||
|
||||
// error 0.5*(f - 2*x'*g + x'*G*x)
|
||||
double f = constantTerm();
|
||||
double xtg = dx.dot(linearTerm());
|
||||
double xGx = dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
|
||||
|
||||
return 0.5 * (f - 2.0 * xtg + xGx);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||
|
||||
// Use the ordering to convert the keys into indices;
|
||||
std::vector<Index> js;
|
||||
BOOST_FOREACH(Key key, this->keys()){
|
||||
js.push_back(ordering.at(key));
|
||||
}
|
||||
|
||||
// Make a copy of the info matrix
|
||||
Matrix newMatrix;
|
||||
SymmetricBlockView<Matrix> newInfo(newMatrix);
|
||||
newInfo.assignNoalias(info_);
|
||||
|
||||
// Construct an error vector in key-order from the Values
|
||||
Vector dx = zero(dim());
|
||||
size_t index = 0;
|
||||
for(unsigned int i = 0; i < this->size(); ++i){
|
||||
Key key = this->keys()[i];
|
||||
const Value& newPt = c.at(key);
|
||||
const Value& linPt = lin_points_.at(key);
|
||||
dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
|
||||
index += linPt.dim();
|
||||
}
|
||||
|
||||
// f2 = f1 - 2*dx'*g1 + dx'*G1*dx
|
||||
//newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
|
||||
double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
|
||||
|
||||
// g2 = g1 - G1*dx
|
||||
//newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
|
||||
Vector g = linearTerm() - squaredTerm().selfadjointView<Eigen::Upper>() * dx;
|
||||
std::vector<Vector> gs;
|
||||
for(size_t i = 0; i < info_.nBlocks()-1; ++i) {
|
||||
gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i)));
|
||||
}
|
||||
|
||||
// G2 = G1
|
||||
// Do Nothing
|
||||
std::vector<Matrix> Gs;
|
||||
for(size_t i = 0; i < info_.nBlocks()-1; ++i) {
|
||||
for(size_t j = i; j < info_.nBlocks()-1; ++j) {
|
||||
Gs.push_back(info_(i,j));
|
||||
}
|
||||
}
|
||||
|
||||
// Create a Hessian Factor from the modified info matrix
|
||||
//return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
|
||||
return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, Gs, gs, f));
|
||||
}
|
||||
|
||||
} // \namespace aspn
|
||||
|
|
@ -0,0 +1,289 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LinearizedFactor.h
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/base/blockMatrices.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A base factor class for the Jacobian and Hessian linearized factors
|
||||
*/
|
||||
class LinearizedGaussianFactor : public NonlinearFactor {
|
||||
public:
|
||||
/** base type */
|
||||
typedef NonlinearFactor Base;
|
||||
typedef LinearizedGaussianFactor This;
|
||||
|
||||
/** shared pointer for convenience */
|
||||
typedef boost::shared_ptr<LinearizedGaussianFactor> shared_ptr;
|
||||
|
||||
protected:
|
||||
|
||||
/** linearization points for error calculation */
|
||||
Values lin_points_;
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor for serialization */
|
||||
LinearizedGaussianFactor() {};
|
||||
|
||||
/**
|
||||
* @param gaussian: A jacobian or hessian factor
|
||||
* @param ordering: The full ordering used to linearize this factor
|
||||
* @param lin_points: The linearization points for, at least, the variables used by this factor
|
||||
*/
|
||||
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points);
|
||||
|
||||
virtual ~LinearizedGaussianFactor() {};
|
||||
|
||||
// access functions
|
||||
const Values& linearizationPoint() const { return lin_points_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("LinearizedGaussianFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(lin_points_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* A factor that takes a linear, Jacobian factor and inserts it into
|
||||
* a nonlinear graph.
|
||||
*/
|
||||
class LinearizedJacobianFactor : public LinearizedGaussianFactor {
|
||||
|
||||
public:
|
||||
/** base type */
|
||||
typedef LinearizedGaussianFactor Base;
|
||||
typedef LinearizedJacobianFactor This;
|
||||
|
||||
/** shared pointer for convenience */
|
||||
typedef boost::shared_ptr<LinearizedJacobianFactor> shared_ptr;
|
||||
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
typedef BlockAb::Block ABlock;
|
||||
typedef BlockAb::constBlock constABlock;
|
||||
typedef BlockAb::Column BVector;
|
||||
typedef BlockAb::constColumn constBVector;
|
||||
|
||||
protected:
|
||||
|
||||
// // store components of a jacobian factor
|
||||
// typedef std::map<Key, Matrix> KeyMatrixMap;
|
||||
// KeyMatrixMap matrices_;
|
||||
// Vector b_;
|
||||
|
||||
AbMatrix matrix_; // the full matrix corresponding to the factor
|
||||
BlockAb Ab_; // the block view of the full matrix
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor for serialization */
|
||||
LinearizedJacobianFactor();
|
||||
|
||||
/**
|
||||
* @param jacobian: A jacobian factor
|
||||
* @param ordering: The ordering used to linearize this factor
|
||||
* @param lin_points: The linearization points for, at least, the variables used by this factor
|
||||
*/
|
||||
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
|
||||
const Ordering& ordering, const Values& lin_points);
|
||||
|
||||
virtual ~LinearizedJacobianFactor() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
// Testable
|
||||
|
||||
/** print function */
|
||||
virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals function with optional tolerance */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
// access functions
|
||||
const constBVector b() const { return Ab_.column(size(), 0); }
|
||||
const constABlock A() const { return Ab_.range(0, size()); };
|
||||
const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }
|
||||
|
||||
/** get the dimension of the factor (number of rows on linearization) */
|
||||
size_t dim() const { return Ab_.rows(); };
|
||||
|
||||
/** Calculate the error of the factor */
|
||||
double error(const Values& c) const;
|
||||
|
||||
/**
|
||||
* linearize to a GaussianFactor
|
||||
* Reimplemented from NoiseModelFactor to directly copy out the
|
||||
* matrices and only update the RHS b with an updated residual
|
||||
*/
|
||||
boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& c, const Ordering& ordering) const;
|
||||
|
||||
/** (A*x-b) */
|
||||
Vector error_vector(const Values& c) const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("LinearizedJacobianFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(Ab_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* A factor that takes a linear, Hessian factor and inserts it into
|
||||
* a nonlinear graph.
|
||||
*/
|
||||
class LinearizedHessianFactor : public LinearizedGaussianFactor {
|
||||
|
||||
public:
|
||||
/** base type */
|
||||
typedef LinearizedGaussianFactor Base;
|
||||
typedef LinearizedHessianFactor This;
|
||||
|
||||
/** shared pointer for convenience */
|
||||
typedef boost::shared_ptr<LinearizedHessianFactor> shared_ptr;
|
||||
|
||||
/** hessian block data types */
|
||||
typedef Matrix InfoMatrix; ///< The full augmented Hessian
|
||||
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
|
||||
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
|
||||
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
|
||||
typedef BlockInfo::Column Column; ///< A column containing the linear term h
|
||||
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
|
||||
|
||||
protected:
|
||||
|
||||
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
|
||||
BlockInfo info_; ///< The block view of the full information matrix.
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor for serialization */
|
||||
LinearizedHessianFactor();
|
||||
|
||||
/**
|
||||
* Use this constructor with the ordering used to linearize the graph
|
||||
* @param hessian: A hessian factor
|
||||
* @param ordering: The ordering used to linearize this factor
|
||||
* @param lin_points: The linearization points for, at least, the variables used by this factor
|
||||
*/
|
||||
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
|
||||
const Ordering& ordering, const Values& lin_points);
|
||||
|
||||
virtual ~LinearizedHessianFactor() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
// Testable
|
||||
|
||||
/** print function */
|
||||
virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals function with optional tolerance */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** Return the constant term \f$ f \f$ as described above
|
||||
* @return The constant term \f$ f \f$
|
||||
*/
|
||||
double constantTerm() const { return info_(this->size(), this->size())(0,0); };
|
||||
|
||||
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
|
||||
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
|
||||
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||
* @return The linear term \f$ g \f$ */
|
||||
constColumn linearTerm(const_iterator j) const { return info_.column(j - this->begin(), this->size(), 0); }
|
||||
|
||||
/** Return the complete linear term \f$ g \f$ as described above.
|
||||
* @return The linear term \f$ g \f$ */
|
||||
constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); };
|
||||
|
||||
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
|
||||
* squared term \f$ H \f$, no data is copied. See HessianFactor class documentation
|
||||
* above to explain that only the upper-triangular part of the information matrix is stored
|
||||
* and returned by this function.
|
||||
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
|
||||
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
|
||||
* use, for example, begin() + 2 to get the 3rd variable in this factor.
|
||||
* @return A view of the requested block, not a copy.
|
||||
*/
|
||||
constBlock squaredTerm(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
|
||||
|
||||
/** Return the <emph>upper-triangular part</emph> of the full squared term, as described above.
|
||||
* See HessianFactor class documentation above to explain that only the
|
||||
* upper-triangular part of the information matrix is stored and returned by this function.
|
||||
*/
|
||||
constBlock squaredTerm() const { return info_.range(0, this->size(), 0, this->size()); }
|
||||
|
||||
|
||||
/** get the dimension of the factor (number of rows on linearization) */
|
||||
size_t dim() const { return matrix_.rows() - 1; };
|
||||
|
||||
/** Calculate the error of the factor */
|
||||
double error(const Values& c) const;
|
||||
|
||||
/**
|
||||
* linearize to a GaussianFactor
|
||||
* Reimplemented from NoiseModelFactor to directly copy out the
|
||||
* matrices and only update the RHS b with an updated residual
|
||||
*/
|
||||
boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& c, const Ordering& ordering) const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("LinearizedHessianFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
ar & BOOST_SERIALIZATION_NVP(info_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
} // \namespace aspn
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testIncrementalFixedLagSmoother.cpp
|
||||
* @brief Unit tests for the Incremental Fixed-Lag Smoother
|
||||
* @author Stephen Williams (swilliams8@gatech.edu)
|
||||
* @date May 23, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) {
|
||||
|
||||
Ordering ordering = *fullgraph.orderingCOLAMD(fullinit);
|
||||
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
|
||||
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
|
||||
VectorValues delta = optimize(gbn);
|
||||
Values fullfinal = fullinit.retract(delta, ordering);
|
||||
|
||||
Point2 expected = fullfinal.at<Point2>(key);
|
||||
Point2 actual = smoother.calculateEstimate<Point2>(key);
|
||||
|
||||
return assert_equal(expected, actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( BatchFixedLagSmoother, Example )
|
||||
{
|
||||
// Test the BatchFixedLagSmoother in a pure linear environment. Thus, full optimization and
|
||||
// the BatchFixedLagSmoother should be identical (even with the linearized approximations at
|
||||
// the end of the smoothing lag)
|
||||
|
||||
SETDEBUG("BatchFixedLagSmoother update", false);
|
||||
|
||||
// Set up parameters
|
||||
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
|
||||
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
|
||||
|
||||
// Create a Fixed-Lag Smoother
|
||||
typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps;
|
||||
BatchFixedLagSmoother smoother(LevenbergMarquardtParams(), 7.0);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullinit;
|
||||
NonlinearFactorGraph fullgraph;
|
||||
|
||||
|
||||
|
||||
// i keeps track of the time step
|
||||
size_t i = 0;
|
||||
|
||||
// Add a prior at time 0 and update the HMF
|
||||
{
|
||||
Key key0(0);
|
||||
|
||||
NonlinearFactorGraph newFactors;
|
||||
Values newValues;
|
||||
Timestamps newTimestamps;
|
||||
|
||||
newFactors.add(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
|
||||
newValues.insert(key0, Point2(0.01, 0.01));
|
||||
newTimestamps[key0] = 0.0;
|
||||
|
||||
fullgraph.push_back(newFactors);
|
||||
fullinit.insert(newValues);
|
||||
|
||||
// Update the smoother
|
||||
smoother.update(newFactors, newValues, newTimestamps);
|
||||
|
||||
// Check
|
||||
CHECK(check_smoother(fullgraph, fullinit, smoother, key0));
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
// Add odometry from time 0 to time 5
|
||||
while(i <= 5) {
|
||||
Key key1(i-1);
|
||||
Key key2(i);
|
||||
|
||||
NonlinearFactorGraph newFactors;
|
||||
Values newValues;
|
||||
Timestamps newTimestamps;
|
||||
|
||||
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
|
||||
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
|
||||
newTimestamps[key2] = double(i);
|
||||
|
||||
fullgraph.push_back(newFactors);
|
||||
fullinit.insert(newValues);
|
||||
|
||||
// Update the smoother
|
||||
smoother.update(newFactors, newValues, newTimestamps);
|
||||
|
||||
// Check
|
||||
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
// Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM
|
||||
{
|
||||
// Add the odometry factor to the HMF
|
||||
Key key1(i-1);
|
||||
Key key2(i);
|
||||
|
||||
NonlinearFactorGraph newFactors;
|
||||
Values newValues;
|
||||
Timestamps newTimestamps;
|
||||
|
||||
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
|
||||
newFactors.add(BetweenFactor<Point2>(Key(2), Key(5), Point2(3.5, 0.0), loopNoise));
|
||||
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
|
||||
newTimestamps[key2] = double(i);
|
||||
|
||||
fullgraph.push_back(newFactors);
|
||||
fullinit.insert(newValues);
|
||||
|
||||
// Update the smoother
|
||||
smoother.update(newFactors, newValues, newTimestamps);
|
||||
|
||||
// Check
|
||||
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
// Add odometry from time 6 to time 15
|
||||
while(i <= 15) {
|
||||
Key key1(i-1);
|
||||
Key key2(i);
|
||||
|
||||
NonlinearFactorGraph newFactors;
|
||||
Values newValues;
|
||||
Timestamps newTimestamps;
|
||||
|
||||
newFactors.add(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
|
||||
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
|
||||
newTimestamps[key2] = double(i);
|
||||
|
||||
fullgraph.push_back(newFactors);
|
||||
fullinit.insert(newValues);
|
||||
|
||||
// Update the smoother
|
||||
smoother.update(newFactors, newValues, newTimestamps);
|
||||
|
||||
// Check
|
||||
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,783 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testConcurrentBatchFilter.cpp
|
||||
* @brief Unit tests for the Concurrent Batch Filter
|
||||
* @author Stephen Williams (swilliams8@gatech.edu)
|
||||
* @date Jan 5, 2013
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
|
||||
const Pose3 poseInitial;
|
||||
const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
|
||||
const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
|
||||
|
||||
// Set up noise models for the factors
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
|
||||
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
|
||||
|
||||
// Create a derived class to allow testing protected member functions
|
||||
class ConcurrentBatchFilterTester : public ConcurrentBatchFilter {
|
||||
public:
|
||||
ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters, double lag) : ConcurrentBatchFilter(parameters, lag) { };
|
||||
virtual ~ConcurrentBatchFilterTester() { };
|
||||
|
||||
// Add accessors to the protected members
|
||||
void presync() { ConcurrentBatchFilter::presync(); };
|
||||
void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { ConcurrentBatchFilter::getSummarizedFactors(summarizedFactors, rootValues); };
|
||||
void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { ConcurrentBatchFilter::getSmootherFactors(smootherFactors, smootherValues); };
|
||||
void synchronize(const NonlinearFactorGraph& summarizedFactors) { ConcurrentBatchFilter::synchronize(summarizedFactors); };
|
||||
void postsync() { ConcurrentBatchFilter::postsync(); };
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGraph& actual, const Values& theta, double tol = 1e-9) {
|
||||
|
||||
FastSet<Key> expectedKeys = expected.keys();
|
||||
FastSet<Key> actualKeys = actual.keys();
|
||||
|
||||
// Verify the set of keys in both graphs are the same
|
||||
if(!std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin()))
|
||||
return false;
|
||||
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
BOOST_FOREACH(Key key, expectedKeys) {
|
||||
ordering.push_back(key);
|
||||
}
|
||||
|
||||
// Linearize each factor graph
|
||||
GaussianFactorGraph expectedGaussian;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) {
|
||||
if(factor)
|
||||
expectedGaussian.push_back( factor->linearize(theta, ordering) );
|
||||
}
|
||||
GaussianFactorGraph actualGaussian;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) {
|
||||
if(factor)
|
||||
actualGaussian.push_back( factor->linearize(theta, ordering) );
|
||||
}
|
||||
|
||||
// Convert linear factor graph into a dense Hessian
|
||||
Matrix expectedHessian = expectedGaussian.augmentedHessian();
|
||||
Matrix actualHessian = actualGaussian.augmentedHessian();
|
||||
|
||||
// Zero out the lower-right entry. This corresponds to a constant in the optimization,
|
||||
// which does not affect the result. Further, in conversions between Jacobians and Hessians,
|
||||
// this term is ignored.
|
||||
expectedHessian(expectedHessian.rows()-1, expectedHessian.cols()-1) = 0.0;
|
||||
actualHessian(actualHessian.rows()-1, actualHessian.cols()-1) = 0.0;
|
||||
|
||||
// Compare Hessians
|
||||
return assert_equal(expectedHessian, actualHessian, tol);
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFilter::KeyTimestampMap& timestamps, size_t index1 = 0, size_t index2 = 1) {
|
||||
|
||||
// Calculate all poses
|
||||
Pose3 poses[20];
|
||||
poses[0] = poseInitial;
|
||||
for(size_t index = 1; index < 20; ++index) {
|
||||
poses[index] = poses[index-1].compose(poseOdometry);
|
||||
}
|
||||
|
||||
// Create all keys
|
||||
Key keys[20];
|
||||
for(size_t index = 0; index < 20; ++index) {
|
||||
keys[index] = Symbol('X', index);
|
||||
}
|
||||
|
||||
// Create factors that will form a specific tree structure
|
||||
// Loop over the included timestamps
|
||||
for(size_t index = index1; index < index2; ++index) {
|
||||
|
||||
switch(index) {
|
||||
case 0:
|
||||
{
|
||||
graph.add(PriorFactor<Pose3>(keys[0], poses[0], noisePrior));
|
||||
// Add new variables
|
||||
theta.insert(keys[0], poses[0].compose(poseError));
|
||||
timestamps[keys[0]] = double(0);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
// Add odometry factor between 0 and 1
|
||||
Pose3 poseDelta = poses[0].between(poses[1]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[0], keys[1], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[1], poses[1].compose(poseError));
|
||||
timestamps[keys[1]] = double(1);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
// Add odometry factor between 1 and 3
|
||||
Pose3 poseDelta = poses[1].between(poses[3]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[1], keys[3], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 2 and 3
|
||||
poseDelta = poses[2].between(poses[3]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[2], keys[3], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[2], poses[2].compose(poseError));
|
||||
timestamps[keys[2]] = double(2);
|
||||
theta.insert(keys[3], poses[3].compose(poseError));
|
||||
timestamps[keys[3]] = double(3);
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
{
|
||||
// Add odometry factor between 3 and 5
|
||||
Pose3 poseDelta = poses[3].between(poses[5]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[3], keys[5], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[5], poses[5].compose(poseError));
|
||||
timestamps[keys[5]] = double(5);
|
||||
break;
|
||||
}
|
||||
case 6:
|
||||
{
|
||||
// Add odometry factor between 3 and 6
|
||||
Pose3 poseDelta = poses[3].between(poses[6]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[3], keys[6], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 5 and 6
|
||||
poseDelta = poses[5].between(poses[6]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[5], keys[6], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[6], poses[6].compose(poseError));
|
||||
timestamps[keys[6]] = double(6);
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
// Add odometry factor between 4 and 7
|
||||
Pose3 poseDelta = poses[4].between(poses[7]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[4], keys[7], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 6 and 7
|
||||
poseDelta = poses[6].between(poses[7]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[6], keys[7], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[4], poses[4].compose(poseError));
|
||||
timestamps[keys[4]] = double(4);
|
||||
theta.insert(keys[7], poses[7].compose(poseError));
|
||||
timestamps[keys[7]] = double(7);
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
break;
|
||||
|
||||
case 9:
|
||||
{
|
||||
// Add odometry factor between 6 and 9
|
||||
Pose3 poseDelta = poses[6].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[6], keys[9], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 7 and 9
|
||||
poseDelta = poses[7].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[7], keys[9], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 8 and 9
|
||||
poseDelta = poses[8].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[8], keys[9], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[8], poses[8].compose(poseError));
|
||||
timestamps[keys[8]] = double(8);
|
||||
theta.insert(keys[9], poses[9].compose(poseError));
|
||||
timestamps[keys[9]] = double(9);
|
||||
break;
|
||||
}
|
||||
case 10:
|
||||
{
|
||||
// Add odometry factor between 9 and 10
|
||||
Pose3 poseDelta = poses[9].between(poses[10]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[9], keys[10], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[10], poses[10].compose(poseError));
|
||||
timestamps[keys[10]] = double(10);
|
||||
break;
|
||||
}
|
||||
case 11:
|
||||
{
|
||||
// Add odometry factor between 10 and 11
|
||||
Pose3 poseDelta = poses[10].between(poses[11]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[10], keys[11], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[11], poses[11].compose(poseError));
|
||||
timestamps[keys[11]] = double(11);
|
||||
break;
|
||||
}
|
||||
case 12:
|
||||
{
|
||||
// Add odometry factor between 7 and 12
|
||||
Pose3 poseDelta = poses[7].between(poses[12]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[7], keys[12], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 9 and 12
|
||||
poseDelta = poses[9].between(poses[12]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[9], keys[12], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[12], poses[12].compose(poseError));
|
||||
timestamps[keys[12]] = double(12);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
case 13:
|
||||
{
|
||||
// Add odometry factor between 10 and 13
|
||||
Pose3 poseDelta = poses[10].between(poses[13]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[10], keys[13], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 12 and 13
|
||||
poseDelta = poses[12].between(poses[13]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[12], keys[13], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[13], poses[13].compose(poseError));
|
||||
timestamps[keys[13]] = double(13);
|
||||
break;
|
||||
}
|
||||
case 14:
|
||||
{
|
||||
// Add odometry factor between 11 and 14
|
||||
Pose3 poseDelta = poses[11].between(poses[14]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[11], keys[14], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 13 and 14
|
||||
poseDelta = poses[13].between(poses[14]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[13], keys[14], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[14], poses[14].compose(poseError));
|
||||
timestamps[keys[14]] = double(14);
|
||||
break;
|
||||
}
|
||||
case 15:
|
||||
break;
|
||||
|
||||
case 16:
|
||||
{
|
||||
// Add odometry factor between 13 and 16
|
||||
Pose3 poseDelta = poses[13].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[13], keys[16], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 14 and 16
|
||||
poseDelta = poses[14].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[14], keys[16], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 15 and 16
|
||||
poseDelta = poses[15].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[15], keys[16], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[15], poses[15].compose(poseError));
|
||||
timestamps[keys[15]] = double(15);
|
||||
theta.insert(keys[16], poses[16].compose(poseError));
|
||||
timestamps[keys[16]] = double(16);
|
||||
break;
|
||||
}
|
||||
case 17:
|
||||
{
|
||||
// Add odometry factor between 16 and 17
|
||||
Pose3 poseDelta = poses[16].between(poses[17]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[16], keys[17], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[17], poses[17].compose(poseError));
|
||||
timestamps[keys[17]] = double(17);
|
||||
break;
|
||||
}
|
||||
case 18:
|
||||
{
|
||||
// Add odometry factor between 17 and 18
|
||||
Pose3 poseDelta = poses[17].between(poses[18]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[17], keys[18], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[18], poses[18].compose(poseError));
|
||||
timestamps[keys[18]] = double(18);
|
||||
break;
|
||||
}
|
||||
case 19:
|
||||
{
|
||||
// Add odometry factor between 14 and 19
|
||||
Pose3 poseDelta = poses[14].between(poses[19]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[14], keys[19], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 16 and 19
|
||||
poseDelta = poses[16].between(poses[19]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[16], keys[19], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[19], poses[19].compose(poseError));
|
||||
timestamps[keys[19]] = double(19);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) {
|
||||
|
||||
// Create an L-M optimizer
|
||||
LevenbergMarquardtParams parameters;
|
||||
parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR;
|
||||
|
||||
LevenbergMarquardtOptimizer optimizer(graph, theta, parameters);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError;
|
||||
do {
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
if(rootValues.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(rootValues);
|
||||
// Update the error value with the new theta
|
||||
optimizer.state().error = graph.error(optimizer.state().values);
|
||||
}
|
||||
|
||||
// Do next iteration
|
||||
currentError = optimizer.error();
|
||||
optimizer.iterate();
|
||||
|
||||
} while(optimizer.iterations() < parameters.maxIterations &&
|
||||
!checkConvergence(parameters.relativeErrorTol, parameters.absoluteErrorTol,
|
||||
parameters.errorTol, currentError, optimizer.error(), parameters.verbosity));
|
||||
|
||||
// return the final optimized values
|
||||
return optimizer.values();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key != factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key == factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
void 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+" ");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_Batch )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
// This tests adds all of the factors to the filter at once (i.e. batch)
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilter filter(parameters, lag);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap fullTimestamps;
|
||||
|
||||
// Create all factors
|
||||
CreateFactors(fullGraph, fullTheta, fullTimestamps, 0, 20);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(fullGraph, fullTheta, fullTimestamps);
|
||||
Values actual = filter.calculateEstimate();
|
||||
|
||||
// Optimize with L-M
|
||||
Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
|
||||
// Check smoother versus batch
|
||||
CHECK(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
// This tests adds the factors to the filter as they are created (i.e. incrementally)
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilter filter(parameters, lag);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
|
||||
// Add odometry from time 0 to time 10
|
||||
for(size_t i = 0; i < 20; ++i) {
|
||||
// Create containers to keep the new factors
|
||||
Values newTheta;
|
||||
NonlinearFactorGraph newGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
|
||||
|
||||
// Create factors
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, i, i+1);
|
||||
|
||||
// Add these entries to the filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
Values actual = filter.calculateEstimate();
|
||||
|
||||
// Add these entries to the full batch version
|
||||
fullGraph.push_back(newGraph);
|
||||
fullTheta.insert(newTheta);
|
||||
Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
fullTheta = expected;
|
||||
|
||||
// Compare filter solution with full batch
|
||||
CHECK(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, synchronize )
|
||||
{
|
||||
// Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// The filter is operating on a known tree structure, so the factors and summarization can
|
||||
// be predicted for testing purposes
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilterTester filter(parameters, lag);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values newTheta, fullTheta;
|
||||
NonlinearFactorGraph newGraph, fullGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
|
||||
|
||||
// Create all factors
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, 0, 13);
|
||||
fullTheta.insert(newTheta);
|
||||
fullGraph.push_back(newGraph);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
Values updatedTheta = filter.calculateEstimate();
|
||||
|
||||
// Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// Create Ordering
|
||||
std::map<Key, int> constraints;
|
||||
constraints[Symbol('X', 8)] = 1;
|
||||
constraints[Symbol('X', 9)] = 1;
|
||||
constraints[Symbol('X', 10)] = 1;
|
||||
constraints[Symbol('X', 11)] = 1;
|
||||
constraints[Symbol('X', 12)] = 1;
|
||||
Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// Linearize into a Gaussian Factor Graph
|
||||
GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
bayesTree.insert(root);
|
||||
|
||||
// std::cout << "BAYES TREE:" << std::endl;
|
||||
// SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
|
||||
// BAYES TREE:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X3 X5 | X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X4 | X7 )
|
||||
|
||||
// Extract the nonlinear factors that should be sent to the smoother
|
||||
std::vector<Key> smootherKeys;
|
||||
smootherKeys.push_back(Symbol('X', 0));
|
||||
smootherKeys.push_back(Symbol('X', 1));
|
||||
smootherKeys.push_back(Symbol('X', 2));
|
||||
smootherKeys.push_back(Symbol('X', 3));
|
||||
smootherKeys.push_back(Symbol('X', 4));
|
||||
smootherKeys.push_back(Symbol('X', 5));
|
||||
smootherKeys.push_back(Symbol('X', 6));
|
||||
NonlinearFactorGraph expectedSmootherFactors;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
// Extract smoother values
|
||||
Values expectedSmootherValues;
|
||||
expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Extract the filter summarized factors
|
||||
// Cached factors that represent the filter side (i.e. the X8 and X10 clique)
|
||||
NonlinearFactorGraph expectedFilterSumarization;
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta));
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta));
|
||||
// And any factors that involve only the root (X7, X9, X12)
|
||||
std::vector<Key> rootKeys;
|
||||
rootKeys.push_back(Symbol('X', 7));
|
||||
rootKeys.push_back(Symbol('X', 9));
|
||||
rootKeys.push_back(Symbol('X', 12));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
}
|
||||
|
||||
// Extract the new root values
|
||||
Values expectedRootValues;
|
||||
expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
|
||||
|
||||
|
||||
// Start the synchronization process
|
||||
NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization;
|
||||
Values actualSmootherValues, actualRootValues;
|
||||
filter.presync();
|
||||
filter.synchronize(smootherSummarization); // Supplying an empty factor graph here
|
||||
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
filter.postsync();
|
||||
|
||||
|
||||
|
||||
// Compare filter sync variables versus the expected
|
||||
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4));
|
||||
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9));
|
||||
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Now add additional factors to the filter and re-sync
|
||||
newGraph.resize(0); newTheta.clear(); newTimestamps.clear();
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, 13, 20);
|
||||
fullTheta.insert(newTheta);
|
||||
fullGraph.push_back(newGraph);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
updatedTheta = filter.calculateEstimate();
|
||||
|
||||
// Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// Create Ordering
|
||||
constraints.clear();
|
||||
constraints[Symbol('X', 15)] = 1;
|
||||
constraints[Symbol('X', 16)] = 1;
|
||||
constraints[Symbol('X', 17)] = 1;
|
||||
constraints[Symbol('X', 18)] = 1;
|
||||
constraints[Symbol('X', 19)] = 1;
|
||||
ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// Linearize into a Gaussian Factor Graph
|
||||
linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
root = jt.eliminate(EliminateQR);
|
||||
bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
bayesTree.insert(root);
|
||||
|
||||
// std::cout << "BAYES TREE:" << std::endl;
|
||||
// SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
|
||||
// BAYES TREE:
|
||||
// P( X14 X16 X19 )
|
||||
// P( X17 | X16 )
|
||||
// P( X18 | X17 )
|
||||
// P( X15 | X16 )
|
||||
// P( X13 | X14 X16 )
|
||||
// P( X11 | X13 X14 )
|
||||
// P( X10 | X11 X13 )
|
||||
// P( X12 | X10 X13 )
|
||||
// P( X9 | X12 X10 )
|
||||
// P( X7 | X9 X12 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X3 X5 | X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X4 | X7 )
|
||||
// P( X8 | X9 )
|
||||
|
||||
// Extract the cached factors for X4 and X6. These are the new smoother summarized factors
|
||||
// TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points?
|
||||
smootherSummarization.resize(0);
|
||||
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta));
|
||||
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Extract the nonlinear factors that should be sent to the smoother
|
||||
smootherKeys.clear();
|
||||
smootherKeys.push_back(Symbol('X', 8));
|
||||
smootherKeys.push_back(Symbol('X', 10));
|
||||
smootherKeys.push_back(Symbol('X', 11));
|
||||
smootherKeys.push_back(Symbol('X', 13));
|
||||
expectedSmootherFactors.resize(0);
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
// And any factors that involve only the old root (X7, X9, X12)
|
||||
rootKeys.clear();
|
||||
rootKeys.push_back(Symbol('X', 7));
|
||||
rootKeys.push_back(Symbol('X', 9));
|
||||
rootKeys.push_back(Symbol('X', 12));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
|
||||
|
||||
// Extract smoother Values
|
||||
expectedSmootherValues.clear();
|
||||
expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8)));
|
||||
expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10)));
|
||||
expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11)));
|
||||
expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13)));
|
||||
|
||||
// Extract the filter summarized factors
|
||||
// Cached factors that represent the filter side (i.e. the X15 and X17 clique)
|
||||
expectedFilterSumarization.resize(0);
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta));
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta));
|
||||
// And any factors that involve only the root (X14, X16, X17)
|
||||
rootKeys.clear();
|
||||
rootKeys.push_back(Symbol('X', 14));
|
||||
rootKeys.push_back(Symbol('X', 16));
|
||||
rootKeys.push_back(Symbol('X', 19));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
}
|
||||
|
||||
// Extract the new root keys
|
||||
expectedRootValues.clear();
|
||||
expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14)));
|
||||
expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16)));
|
||||
expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19)));
|
||||
|
||||
|
||||
|
||||
// Start the synchronization process
|
||||
actualSmootherFactors.resize(0); actualFilterSumarization.resize(0);
|
||||
actualSmootherValues.clear(); actualRootValues.clear();
|
||||
filter.presync();
|
||||
filter.synchronize(smootherSummarization);
|
||||
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
filter.postsync();
|
||||
|
||||
|
||||
|
||||
// Compare filter sync variables versus the expected
|
||||
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4));
|
||||
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,708 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testConcurrentBatchSmoother.cpp
|
||||
* @brief Unit tests for the Concurrent Batch Smoother
|
||||
* @author Stephen Williams (swilliams8@gatech.edu)
|
||||
* @date Jan 5, 2013
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
|
||||
const Pose3 poseInitial;
|
||||
const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
|
||||
const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
|
||||
|
||||
// Set up noise models for the factors
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5));
|
||||
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0));
|
||||
|
||||
// Create a derived class to allow testing protected member functions
|
||||
class ConcurrentBatchSmootherTester : public ConcurrentBatchSmoother {
|
||||
public:
|
||||
ConcurrentBatchSmootherTester(const LevenbergMarquardtParams& parameters) : ConcurrentBatchSmoother(parameters) { };
|
||||
virtual ~ConcurrentBatchSmootherTester() { };
|
||||
|
||||
// Add accessors to the protected members
|
||||
void presync() {
|
||||
ConcurrentBatchSmoother::presync();
|
||||
};
|
||||
void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) {
|
||||
ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors);
|
||||
};
|
||||
void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) {
|
||||
ConcurrentBatchSmoother::synchronize(smootherFactors, smootherValues, summarizedFactors, rootValues);
|
||||
};
|
||||
void postsync() {
|
||||
ConcurrentBatchSmoother::postsync();
|
||||
};
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGraph& actual, const Values& theta, double tol = 1e-9) {
|
||||
|
||||
FastSet<Key> expectedKeys = expected.keys();
|
||||
FastSet<Key> actualKeys = actual.keys();
|
||||
|
||||
// Verify the set of keys in both graphs are the same
|
||||
if(!std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin()))
|
||||
return false;
|
||||
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
BOOST_FOREACH(Key key, expectedKeys) {
|
||||
ordering.push_back(key);
|
||||
}
|
||||
|
||||
// Linearize each factor graph
|
||||
GaussianFactorGraph expectedGaussian;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) {
|
||||
if(factor)
|
||||
expectedGaussian.push_back( factor->linearize(theta, ordering) );
|
||||
}
|
||||
GaussianFactorGraph actualGaussian;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) {
|
||||
if(factor)
|
||||
actualGaussian.push_back( factor->linearize(theta, ordering) );
|
||||
}
|
||||
|
||||
// Convert linear factor graph into a dense Hessian
|
||||
Matrix expectedHessian = expectedGaussian.augmentedHessian();
|
||||
Matrix actualHessian = actualGaussian.augmentedHessian();
|
||||
|
||||
// Zero out the lower-right entry. This corresponds to a constant in the optimization,
|
||||
// which does not affect the result. Further, in conversions between Jacobians and Hessians,
|
||||
// this term is ignored.
|
||||
expectedHessian(expectedHessian.rows()-1, expectedHessian.cols()-1) = 0.0;
|
||||
actualHessian(actualHessian.rows()-1, actualHessian.cols()-1) = 0.0;
|
||||
|
||||
// Compare Hessians
|
||||
return assert_equal(expectedHessian, actualHessian, tol);
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0, size_t index2 = 1) {
|
||||
|
||||
// Calculate all poses
|
||||
Pose3 poses[20];
|
||||
poses[0] = poseInitial;
|
||||
for(size_t index = 1; index < 20; ++index) {
|
||||
poses[index] = poses[index-1].compose(poseOdometry);
|
||||
}
|
||||
|
||||
// Create all keys
|
||||
Key keys[20];
|
||||
for(size_t index = 0; index < 20; ++index) {
|
||||
keys[index] = Symbol('X', index);
|
||||
}
|
||||
|
||||
// Create factors that will form a specific tree structure
|
||||
// Loop over the included timestamps
|
||||
for(size_t index = index1; index < index2; ++index) {
|
||||
|
||||
switch(index) {
|
||||
case 0:
|
||||
{
|
||||
graph.add(PriorFactor<Pose3>(keys[0], poses[0], noisePrior));
|
||||
// Add new variables
|
||||
theta.insert(keys[0], poses[0].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
// Add odometry factor between 0 and 1
|
||||
Pose3 poseDelta = poses[0].between(poses[1]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[0], keys[1], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[1], poses[1].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
// Add odometry factor between 1 and 3
|
||||
Pose3 poseDelta = poses[1].between(poses[3]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[1], keys[3], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 2 and 3
|
||||
poseDelta = poses[2].between(poses[3]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[2], keys[3], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[2], poses[2].compose(poseError));
|
||||
theta.insert(keys[3], poses[3].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
{
|
||||
// Add odometry factor between 3 and 5
|
||||
Pose3 poseDelta = poses[3].between(poses[5]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[3], keys[5], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[5], poses[5].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 6:
|
||||
{
|
||||
// Add odometry factor between 3 and 6
|
||||
Pose3 poseDelta = poses[3].between(poses[6]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[3], keys[6], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 5 and 6
|
||||
poseDelta = poses[5].between(poses[6]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[5], keys[6], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[6], poses[6].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
// Add odometry factor between 4 and 7
|
||||
Pose3 poseDelta = poses[4].between(poses[7]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[4], keys[7], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 6 and 7
|
||||
poseDelta = poses[6].between(poses[7]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[6], keys[7], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[4], poses[4].compose(poseError));
|
||||
theta.insert(keys[7], poses[7].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
break;
|
||||
|
||||
case 9:
|
||||
{
|
||||
// Add odometry factor between 6 and 9
|
||||
Pose3 poseDelta = poses[6].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[6], keys[9], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 7 and 9
|
||||
poseDelta = poses[7].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[7], keys[9], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 8 and 9
|
||||
poseDelta = poses[8].between(poses[9]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[8], keys[9], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[8], poses[8].compose(poseError));
|
||||
theta.insert(keys[9], poses[9].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 10:
|
||||
{
|
||||
// Add odometry factor between 9 and 10
|
||||
Pose3 poseDelta = poses[9].between(poses[10]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[9], keys[10], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[10], poses[10].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 11:
|
||||
{
|
||||
// Add odometry factor between 10 and 11
|
||||
Pose3 poseDelta = poses[10].between(poses[11]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[10], keys[11], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[11], poses[11].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 12:
|
||||
{
|
||||
// Add odometry factor between 7 and 12
|
||||
Pose3 poseDelta = poses[7].between(poses[12]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[7], keys[12], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 9 and 12
|
||||
poseDelta = poses[9].between(poses[12]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[9], keys[12], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[12], poses[12].compose(poseError));
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
case 13:
|
||||
{
|
||||
// Add odometry factor between 10 and 13
|
||||
Pose3 poseDelta = poses[10].between(poses[13]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[10], keys[13], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 12 and 13
|
||||
poseDelta = poses[12].between(poses[13]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[12], keys[13], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[13], poses[13].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 14:
|
||||
{
|
||||
// Add odometry factor between 11 and 14
|
||||
Pose3 poseDelta = poses[11].between(poses[14]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[11], keys[14], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 13 and 14
|
||||
poseDelta = poses[13].between(poses[14]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[13], keys[14], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[14], poses[14].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 15:
|
||||
break;
|
||||
|
||||
case 16:
|
||||
{
|
||||
// Add odometry factor between 13 and 16
|
||||
Pose3 poseDelta = poses[13].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[13], keys[16], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 14 and 16
|
||||
poseDelta = poses[14].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[14], keys[16], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 15 and 16
|
||||
poseDelta = poses[15].between(poses[16]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[15], keys[16], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[15], poses[15].compose(poseError));
|
||||
theta.insert(keys[16], poses[16].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 17:
|
||||
{
|
||||
// Add odometry factor between 16 and 17
|
||||
Pose3 poseDelta = poses[16].between(poses[17]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[16], keys[17], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[17], poses[17].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 18:
|
||||
{
|
||||
// Add odometry factor between 17 and 18
|
||||
Pose3 poseDelta = poses[17].between(poses[18]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[17], keys[18], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[18], poses[18].compose(poseError));
|
||||
break;
|
||||
}
|
||||
case 19:
|
||||
{
|
||||
// Add odometry factor between 14 and 19
|
||||
Pose3 poseDelta = poses[14].between(poses[19]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[14], keys[19], poseDelta, noiseOdometery));
|
||||
// Add odometry factor between 16 and 19
|
||||
poseDelta = poses[16].between(poses[19]);
|
||||
graph.add(BetweenFactor<Pose3>(keys[16], keys[19], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[19], poses[19].compose(poseError));
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) {
|
||||
|
||||
// Create an L-M optimizer
|
||||
LevenbergMarquardtParams parameters;
|
||||
parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR;
|
||||
|
||||
LevenbergMarquardtOptimizer optimizer(graph, theta, parameters);
|
||||
|
||||
// Use a custom optimization loop so the linearization points can be controlled
|
||||
double currentError;
|
||||
do {
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
if(rootValues.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(rootValues);
|
||||
// Update the error value with the new theta
|
||||
optimizer.state().error = graph.error(optimizer.state().values);
|
||||
}
|
||||
|
||||
// Do next iteration
|
||||
currentError = optimizer.error();
|
||||
optimizer.iterate();
|
||||
|
||||
} while(optimizer.iterations() < parameters.maxIterations &&
|
||||
!checkConvergence(parameters.relativeErrorTol, parameters.absoluteErrorTol,
|
||||
parameters.errorTol, currentError, optimizer.error(), parameters.verbosity));
|
||||
|
||||
// return the final optimized values
|
||||
return optimizer.values();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key != factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key == factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
void 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+" ");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical
|
||||
// This tests adds all of the factors to the smoother at once (i.e. batch)
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
||||
// Create a Concurrent Batch Smoother
|
||||
ConcurrentBatchSmoother smoother(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
|
||||
// Create all factors
|
||||
CreateFactors(fullGraph, fullTheta, 0, 20);
|
||||
|
||||
// Optimize with Concurrent Batch Smoother
|
||||
smoother.update(fullGraph, fullTheta);
|
||||
Values actual = smoother.calculateEstimate();
|
||||
|
||||
// Optimize with L-M
|
||||
Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
|
||||
// Check smoother versus batch
|
||||
CHECK(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_Incremental )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical
|
||||
// This tests adds the factors to the smoother as they are created (i.e. incrementally)
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
||||
// Create a Concurrent Batch Smoother
|
||||
ConcurrentBatchSmoother smoother(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
|
||||
// Add odometry from time 0 to time 10
|
||||
for(size_t i = 0; i < 20; ++i) {
|
||||
// Create containers to keep the new factors
|
||||
Values newTheta;
|
||||
NonlinearFactorGraph newGraph;
|
||||
|
||||
// Create factors
|
||||
CreateFactors(newGraph, newTheta, i, i+1);
|
||||
|
||||
// Add these entries to the filter
|
||||
smoother.update(newGraph, newTheta);
|
||||
Values actual = smoother.calculateEstimate();
|
||||
|
||||
// Add these entries to the full batch version
|
||||
fullGraph.push_back(newGraph);
|
||||
fullTheta.insert(newTheta);
|
||||
Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
fullTheta = expected;
|
||||
|
||||
// Compare filter solution with full batch
|
||||
CHECK(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
|
||||
{
|
||||
// Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// The smoother is operating on a known tree structure, so the factors and summarization can
|
||||
// be predicted for testing purposes
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
||||
// Create a Concurrent Batch Smoother
|
||||
ConcurrentBatchSmootherTester smoother(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
|
||||
// Create factors for times 0 - 12
|
||||
// When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian
|
||||
// ... this Bayes Tree is produced:
|
||||
// Bayes Tree:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X5 X3 | X6 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X2 | X3 )
|
||||
// P( X4 | X7 )
|
||||
// We then produce the inputs necessary for the 'synchronize' function.
|
||||
// The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12)
|
||||
CreateFactors(fullGraph, fullTheta, 0, 13);
|
||||
|
||||
// Optimize the full graph
|
||||
Values optimalTheta = BatchOptimize(fullGraph, fullTheta);
|
||||
|
||||
// Re-eliminate to create the Bayes Tree
|
||||
Ordering ordering;
|
||||
ordering.push_back(Symbol('X', 2));
|
||||
ordering.push_back(Symbol('X', 0));
|
||||
ordering.push_back(Symbol('X', 1));
|
||||
ordering.push_back(Symbol('X', 4));
|
||||
ordering.push_back(Symbol('X', 5));
|
||||
ordering.push_back(Symbol('X', 3));
|
||||
ordering.push_back(Symbol('X', 6));
|
||||
ordering.push_back(Symbol('X', 8));
|
||||
ordering.push_back(Symbol('X', 11));
|
||||
ordering.push_back(Symbol('X', 10));
|
||||
ordering.push_back(Symbol('X', 7));
|
||||
ordering.push_back(Symbol('X', 9));
|
||||
ordering.push_back(Symbol('X', 12));
|
||||
Values linpoint;
|
||||
linpoint.insert(optimalTheta);
|
||||
GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
bayesTree.insert(root);
|
||||
|
||||
// Extract the values for the smoother keys. This consists of the branches: X4 and X6
|
||||
// Extract the non-root values from the initial values to test the smoother optimization
|
||||
Values smootherValues;
|
||||
smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0)));
|
||||
smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1)));
|
||||
smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2)));
|
||||
smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3)));
|
||||
smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4)));
|
||||
smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5)));
|
||||
smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Extract the optimal root values
|
||||
Values rootValues;
|
||||
rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7)));
|
||||
rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9)));
|
||||
rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12)));
|
||||
|
||||
// Extract the nonlinear smoother factors as any factor with a non-root smoother key
|
||||
std::set<Key> smootherKeys;
|
||||
smootherKeys.insert(Symbol('X', 0));
|
||||
smootherKeys.insert(Symbol('X', 1));
|
||||
smootherKeys.insert(Symbol('X', 2));
|
||||
smootherKeys.insert(Symbol('X', 3));
|
||||
smootherKeys.insert(Symbol('X', 4));
|
||||
smootherKeys.insert(Symbol('X', 5));
|
||||
smootherKeys.insert(Symbol('X', 6));
|
||||
NonlinearFactorGraph smootherFactors;
|
||||
FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors);
|
||||
|
||||
// Extract the filter summarized factors. This consists of the linear cached factors from
|
||||
// the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys
|
||||
NonlinearFactorGraph filterSummarization;
|
||||
filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, linpoint));
|
||||
filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, linpoint));
|
||||
std::set<Key> rootKeys;
|
||||
rootKeys.insert(Symbol('X', 7));
|
||||
rootKeys.insert(Symbol('X', 9));
|
||||
rootKeys.insert(Symbol('X', 12));
|
||||
FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization);
|
||||
|
||||
|
||||
|
||||
// Perform the synchronization procedure
|
||||
NonlinearFactorGraph actualSmootherSummarization;
|
||||
smoother.presync();
|
||||
smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
smoother.postsync();
|
||||
|
||||
// Verify the returned smoother values is empty in the first iteration
|
||||
NonlinearFactorGraph expectedSmootherSummarization;
|
||||
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Perform a full update of the smoother. Since the root values/summarized filter factors were
|
||||
// created at the optimal values, the smoother should be identical to the batch optimization
|
||||
smoother.update();
|
||||
Values actualSmootherTheta = smoother.calculateEstimate();
|
||||
|
||||
// Create the expected values as the optimal set
|
||||
Values expectedSmootherTheta;
|
||||
expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Compare filter solution with full batch
|
||||
CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Add a loop closure factor to the smoother and re-check. Since the filter
|
||||
// factors were created at the optimal linpoint, and since the new loop closure
|
||||
// does not involve filter keys, the smoother should still yeild the optimal solution
|
||||
// The new Bayes Tree is:
|
||||
// Bayes Tree:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X4 | X6 X7 )
|
||||
// P( X3 X5 | X4 X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 X4 )
|
||||
// P( X0 | X1 )
|
||||
Pose3 poseDelta = fullTheta.at<Pose3>(Symbol('X', 1)).between(fullTheta.at<Pose3>(Symbol('X', 4)));
|
||||
NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor<Pose3>(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery));
|
||||
fullGraph.push_back(loopClosure);
|
||||
optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues);
|
||||
|
||||
// Recreate the Bayes Tree
|
||||
linpoint.clear();
|
||||
linpoint.insert(optimalTheta);
|
||||
linpoint.update(rootValues);
|
||||
linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
root = jt.eliminate(EliminateQR);
|
||||
bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
bayesTree.insert(root);
|
||||
|
||||
// Add the loop closure to the smoother
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.push_back(loopClosure);
|
||||
smoother.update(newFactors);
|
||||
actualSmootherTheta = smoother.calculateEstimate();
|
||||
|
||||
// Create the expected values as the optimal set
|
||||
expectedSmootherTheta.clear();
|
||||
expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Compare filter solution with full batch
|
||||
// TODO: Check This
|
||||
// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Now perform a second synchronization to test the smoother-calculated summarization
|
||||
actualSmootherSummarization.resize(0);
|
||||
smootherFactors.resize(0);
|
||||
smootherValues.clear();
|
||||
smoother.presync();
|
||||
smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
smoother.postsync();
|
||||
|
||||
// Extract the expected smoother summarization from the Bayes Tree
|
||||
// The smoother branches after the addition of the loop closure is only X6
|
||||
expectedSmootherSummarization.resize(0);
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor());
|
||||
LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint));
|
||||
expectedSmootherSummarization.push_back(ljf);
|
||||
|
||||
// Compare smoother factors with the expected factors by computing the hessian information matrix
|
||||
// TODO: Check This
|
||||
// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4));
|
||||
|
||||
|
||||
|
||||
// TODO: Modify the second synchronization so that the filter sends an additional set of factors.
|
||||
// I'm not sure what additional code this will exercise, but just for good measure.
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,316 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testRFID.cpp
|
||||
* @brief Unit tests for the RFID factor
|
||||
* @author Stephen Williams (swilliams8@gatech.edu)
|
||||
* @date Jan 16, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, equals_jacobian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
|
||||
// Create two identical factors and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
LinearizedJacobianFactor jacobian1(jf, ordering, values);
|
||||
LinearizedJacobianFactor jacobian2(jf, ordering, values);
|
||||
|
||||
CHECK(assert_equal(jacobian1, jacobian2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, clone_jacobian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
// Create one factor that is a clone of the other and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
LinearizedJacobianFactor jacobian1(jf, ordering, values);
|
||||
LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
|
||||
CHECK(assert_equal(jacobian1, *jacobian2));
|
||||
|
||||
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values, ordering));
|
||||
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values, ordering));
|
||||
CHECK(assert_equal(*jf1, *jf2));
|
||||
|
||||
Matrix information1 = jf1->augmentedInformation();
|
||||
Matrix information2 = jf2->augmentedInformation();
|
||||
CHECK(assert_equal(information1, information2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, add_jacobian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
// Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values));
|
||||
NonlinearFactorGraph graph1; graph1.push_back(jacobian);
|
||||
NonlinearFactorGraph graph2; graph2.add(*jacobian);
|
||||
|
||||
// TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version
|
||||
// However, I am currently unable to reproduce the error in this unit test.
|
||||
// I don't know if this affects the Hessian version as well.
|
||||
CHECK(assert_equal(graph1, graph2));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( LinearizedFactor, error_jacobian )
|
||||
//{
|
||||
// // Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
// Key key1(1);
|
||||
// Key key2(2);
|
||||
// Ordering ordering;
|
||||
// ordering.push_back(key1);
|
||||
// ordering.push_back(key2);
|
||||
// Values values;
|
||||
// values.insert(key1, Point3(-22.4, +8.5, +2.4));
|
||||
// values.insert(key2, Point3(-21.0, +5.0, +21.0));
|
||||
//
|
||||
// Point3 measured(1.0, -2.5, 17.8);
|
||||
// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
// BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
|
||||
//
|
||||
//
|
||||
// // Create a linearized jacobian factors
|
||||
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
// LinearizedJacobianFactor jacobian(jf, ordering, values);
|
||||
//
|
||||
//
|
||||
// for(double x1 = -10; x1 < 10; x1 += 2.0) {
|
||||
// for(double y1 = -10; y1 < 10; y1 += 2.0) {
|
||||
// for(double z1 = -10; z1 < 10; z1 += 2.0) {
|
||||
//
|
||||
// for(double x2 = -10; x2 < 10; x2 += 2.0) {
|
||||
// for(double y2 = -10; y2 < 10; y2 += 2.0) {
|
||||
// for(double z2 = -10; z2 < 10; z2 += 2.0) {
|
||||
//
|
||||
// Values linpoint;
|
||||
// linpoint.insert(key1, Point3(x1, y1, z1));
|
||||
// linpoint.insert(key2, Point3(x2, y2, z2));
|
||||
//
|
||||
// // Check that the error of the Linearized Jacobian and the original factor match
|
||||
// // This only works because a BetweenFactor on a Point3 is actually a linear system
|
||||
// double expected_error = betweenFactor.error(linpoint);
|
||||
// double actual_error = jacobian.error(linpoint);
|
||||
// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
|
||||
//
|
||||
// // Check that the linearized factors are identical
|
||||
// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint, ordering);
|
||||
// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint, ordering);
|
||||
// CHECK(assert_equal(*expected_factor, *actual_factor));
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, equals_hessian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
|
||||
// Create two identical factors and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
|
||||
LinearizedHessianFactor hessian1(hf, ordering, values);
|
||||
LinearizedHessianFactor hessian2(hf, ordering, values);
|
||||
|
||||
CHECK(assert_equal(hessian1, hessian2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, clone_hessian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
|
||||
// Create two identical factors and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
|
||||
LinearizedHessianFactor hessian1(hf, ordering, values);
|
||||
LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast<LinearizedHessianFactor>(hessian1.clone());
|
||||
|
||||
CHECK(assert_equal(hessian1, *hessian2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( LinearizedFactor, add_hessian )
|
||||
{
|
||||
// Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
Key x1(1);
|
||||
Key x2(2);
|
||||
Ordering ordering;
|
||||
ordering.push_back(x1);
|
||||
ordering.push_back(x2);
|
||||
Values values;
|
||||
values.insert(x1, Point3(-22.4, +8.5, +2.4));
|
||||
values.insert(x2, Point3(-21.0, +5.0, +21.0));
|
||||
|
||||
Point3 measured(1.0, -2.5, 17.8);
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
|
||||
|
||||
|
||||
// Create two identical factors and make sure they're equal
|
||||
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
|
||||
LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values));
|
||||
NonlinearFactorGraph graph1; graph1.push_back(hessian);
|
||||
NonlinearFactorGraph graph2; graph2.add(*hessian);
|
||||
|
||||
CHECK(assert_equal(graph1, graph2));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( LinearizedFactor, error_hessian )
|
||||
//{
|
||||
// // Create a Between Factor from a Point3. This is actually a linear factor.
|
||||
// Key key1(1);
|
||||
// Key key2(2);
|
||||
// Ordering ordering;
|
||||
// ordering.push_back(key1);
|
||||
// ordering.push_back(key2);
|
||||
// Values values;
|
||||
// values.insert(key1, Point3(-22.4, +8.5, +2.4));
|
||||
// values.insert(key2, Point3(-21.0, +5.0, +21.0));
|
||||
//
|
||||
// Point3 measured(1.0, -2.5, 17.8);
|
||||
// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
// BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
|
||||
//
|
||||
//
|
||||
// // Create a linearized hessian factor
|
||||
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
|
||||
// HessianFactor::shared_ptr hf(new HessianFactor(*jf));
|
||||
// LinearizedHessianFactor hessian(hf, ordering, values);
|
||||
//
|
||||
//
|
||||
// for(double x1 = -10; x1 < 10; x1 += 2.0) {
|
||||
// for(double y1 = -10; y1 < 10; y1 += 2.0) {
|
||||
// for(double z1 = -10; z1 < 10; z1 += 2.0) {
|
||||
//
|
||||
// for(double x2 = -10; x2 < 10; x2 += 2.0) {
|
||||
// for(double y2 = -10; y2 < 10; y2 += 2.0) {
|
||||
// for(double z2 = -10; z2 < 10; z2 += 2.0) {
|
||||
//
|
||||
// Values linpoint;
|
||||
// linpoint.insert(key1, Point3(x1, y1, z1));
|
||||
// linpoint.insert(key2, Point3(x2, y2, z2));
|
||||
//
|
||||
// // Check that the error of the Linearized Hessian and the original factor match
|
||||
// // This only works because a BetweenFactor on a Point3 is actually a linear system
|
||||
// double expected_error = betweenFactor.error(linpoint);
|
||||
// double actual_error = hessian.error(linpoint);
|
||||
// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
|
||||
//
|
||||
// // Check that the linearized factors are identical
|
||||
// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering)));
|
||||
// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering);
|
||||
// CHECK(assert_equal(*expected_factor, *actual_factor));
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main()
|
||||
{
|
||||
TestResult tr; return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue