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

Updated to the marginal factor calculation in BatchFixedLagSmoother to 

be more efficient
release/4.3a0
Stephen Williams 2013-08-10 17:16:31 +00:00
parent 12d003229a
commit ebc6a66c3a
3 changed files with 235 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

271
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -50,27 +50,25 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
std::cout << "BatchFixedLagSmoother::update() START" << std::endl;
}
// Add the new factors
insertFactors(newFactors);
// Add the new variables
// Update all of the internal variables with the new information
gttic(augment_system);
// Add the new variables to theta
theta_.insert(newTheta);
// Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
ordering_.push_back(key_value.key);
}
// Augment Delta
std::vector<size_t> dims;
dims.reserve(newTheta.size());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
ordering_.push_back(key_value.key);
dims.push_back(key_value.value.dim());
}
// Augment Delta
delta_.append(dims);
for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) {
delta_[i].setZero();
}
// Add the new factors to the graph, updating the variable index
insertFactors(newFactors);
gttoc(augment_system);
// Update the Timestamps associated with the factor keys
updateKeyTimestampMap(timestamps);
@ -90,18 +88,24 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
}
// Reorder
gttic(reorder);
reorder(marginalizableKeys);
gttoc(reorder);
// Optimize
gttic(optimize);
Result result;
if(theta_.size() > 0) {
if(factors_.size() > 0) {
result = optimize();
}
gttoc(optimize);
// Marginalize out old variables.
gttic(marginalize);
if(marginalizableKeys.size() > 0) {
marginalize(marginalizableKeys);
}
gttoc(marginalize);
if(debug) {
std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl;
@ -188,6 +192,20 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
/* ************************************************************************* */
void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
const bool debug = ISDEBUG("BatchFixedLagSmoother reorder");
if(debug) {
std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl;
}
if(debug) {
std::cout << "Marginalizable Keys: ";
BOOST_FOREACH(Key key, marginalizeKeys) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << std::endl;
}
// Calculate a variable index
VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size());
@ -211,10 +229,25 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
// Permute the ordering, variable index, and deltas
ordering_.permuteInPlace(forwardPermutation);
delta_.permuteInPlace(forwardPermutation);
if(debug) {
ordering_.print("New Ordering: ");
}
if(debug) {
std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl;
}
}
/* ************************************************************************* */
FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
const bool debug = ISDEBUG("BatchFixedLagSmoother optimize");
if(debug) {
std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl;
}
// Create output result structure
Result result;
result.nonlinearVariables = theta_.size() - linearKeys_.size();
@ -224,7 +257,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
double lambda = parameters_.lambdaInitial;
double lambdaFactor = parameters_.lambdaFactor;
double lambdaUpperBound = parameters_.lambdaUpperBound;
double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound;
double lambdaLowerBound = 1.0e-10;
size_t maxIterations = parameters_.maxIterations;
double relativeErrorTol = parameters_.relativeErrorTol;
double absoluteErrorTol = parameters_.absoluteErrorTol;
@ -234,6 +267,17 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
Values evalpoint = theta_.retract(delta_, ordering_);
result.error = factors_.error(evalpoint);
// check if we're already close enough
if(result.error <= errorTol) {
if(debug) { std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " << errorTol << std::endl; }
return result;
}
if(debug) {
std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() << std::endl;
std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl;
}
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
VectorValues newDelta;
@ -248,6 +292,9 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
// Keep increasing lambda until we make make progress
while(true) {
if(debug) { std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; }
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
@ -279,6 +326,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
double error = factors_.error(evalpoint);
gttoc(compute_error);
if(debug) {
std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() << std::endl;
std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl;
}
if(error < result.error) {
// Keep this change
// Update the error value
@ -304,22 +356,31 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
break;
} else {
// Reject this change
// Increase lambda and continue searching
lambda *= lambdaFactor;
if(lambda > lambdaUpperBound) {
if(lambda >= lambdaUpperBound) {
// The maximum lambda has been used. Print a warning and end the search.
std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
break;
} else {
// Increase lambda and continue searching
lambda *= lambdaFactor;
}
}
} // end while
}
gttoc(optimizer_iteration);
if(debug) { std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; }
result.iterations++;
} while(result.iterations < maxIterations &&
!checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT));
if(debug) { std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; }
if(debug) {
std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl;
}
return result;
}
@ -331,59 +392,63 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
// from the result of a partial elimination. This function removes the marginalized factors and
// adds the linearized factors back in.
// Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys')
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize");
// Create a variable index
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
if(debug) std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl;
// Use the variable Index to mark the factors that will be marginalized
if(debug) {
std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: ";
BOOST_FOREACH(Key key, marginalizeKeys) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << std::endl;
}
// Identify all of the factors involving any marginalized variable. These must be removed.
std::set<size_t> removedFactorSlots;
VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size());
BOOST_FOREACH(Key key, marginalizeKeys) {
const FastList<size_t>& slots = variableIndex[ordering_.at(key)];
removedFactorSlots.insert(slots.begin(), slots.end());
BOOST_FOREACH(size_t slot, slots) {
removedFactorSlots.insert(slot);
}
}
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically
// Find the subset of nodes/keys that must be eliminated
std::set<Index> indicesToEliminate;
BOOST_FOREACH(Key key, marginalizeKeys) {
indicesToEliminate.insert(ordering_.at(key));
if(debug) {
std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: ";
BOOST_FOREACH(size_t slot, removedFactorSlots) {
std::cout << slot << " ";
}
BOOST_FOREACH(Key key, marginalizeKeys) {
EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key)));
std::cout << std::endl;
}
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors
// Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
marginalFactors.push_back(marginalFactor);
// Add the keys associated with the marginal factor to the separator values
BOOST_FOREACH(Key key, *marginalFactor) {
if(!linearKeys_.exists(key)) {
linearKeys_.insert(key, theta_.at(key));
// Add the removed factors to a factor graph
NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) {
if(factors_.at(slot)) {
removedFactors.push_back(factors_.at(slot));
}
}
}
}
insertFactors(marginalFactors);
// Remove the marginalized variables and factors from the filter
if(debug) {
PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: ");
}
// Calculate marginal factors on the remaining keys
NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction());
if(debug) {
PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: ");
}
// Remove marginalized factors from the factor graph
removeFactors(removedFactorSlots);
// Remove marginalized keys from the system
eraseKeys(marginalizeKeys);
// Insert the new marginal factors
insertFactors(marginalFactors);
}
/* ************************************************************************* */
@ -395,6 +460,15 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::st
std::cout << std::endl;
}
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<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 NonlinearFactor::shared_ptr& factor) {
std::cout << "f(";
@ -523,5 +597,102 @@ void BatchFixedLagSmoother::EliminationForest::removeChildrenIndices(std::set<In
}
}
/* ************************************************************************* */
NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors");
if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl;
if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: ");
// Get the set of all keys involved in the factor graph
FastSet<Key> allKeys(graph.keys());
if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: ");
// Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
FastSet<Key> remainingKeys;
std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: ");
if(marginalizeKeys.size() == 0) {
// There are no keys to marginalize. Simply return the input factors
if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl;
return graph;
} else {
// Create a subset of theta that only contains the required keys
Values values;
BOOST_FOREACH(Key key, allKeys) {
values.insert(key, theta.at(key));
}
// Calculate the ordering: [Others Root]
std::map<Key, int> constraints;
BOOST_FOREACH(Key key, marginalizeKeys) {
constraints[key] = 0;
}
BOOST_FOREACH(Key key, remainingKeys) {
constraints[key] = 1;
}
Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints);
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( BatchFixedLagSmoother::EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a forest. Only the top-most node/index of each tree needs to be eliminated; all of the children will be eliminated automatically
// Find the subset of nodes/keys that must be eliminated
std::set<Index> indicesToEliminate;
BOOST_FOREACH(Key key, marginalizeKeys) {
indicesToEliminate.insert(ordering.at(key));
}
BOOST_FOREACH(Key key, marginalizeKeys) {
EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering.at(key)));
}
if(debug) PrintKeySet(indicesToEliminate, "BatchFixedLagSmoother::calculateMarginalFactors Indices To Eliminate: ");
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(eliminateFunction);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
if(debug) {
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: ";
PrintSymbolicFactor(marginalFactor);
}
}
}
// Also add any remaining factors that were unaffected by marginalizing out the selected variables.
// These are part of the marginal on the remaining variables as well.
BOOST_FOREACH(Key key, remainingKeys) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, forest.at(ordering.at(key))->factors()) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
if(debug) {
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Remaining Factor: ";
PrintSymbolicFactor(marginalFactor);
}
}
}
if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: ");
if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl;
return marginalFactors;
}
}
/* ************************************************************************* */
} /// namespace gtsam

4
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -206,9 +206,13 @@ protected:
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);
};
static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);

6
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -54,7 +54,11 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example )
// the BatchFixedLagSmoother should be identical (even with the linearized approximations at
// the end of the smoothing lag)
SETDEBUG("BatchFixedLagSmoother update", false);
// SETDEBUG("BatchFixedLagSmoother update", true);
// SETDEBUG("BatchFixedLagSmoother reorder", true);
// SETDEBUG("BatchFixedLagSmoother optimize", true);
// SETDEBUG("BatchFixedLagSmoother marginalize", true);
// SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true);
// Set up parameters
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));