From ebc6a66c3a0f99851edc853e39a45d0de84f0fbc Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sat, 10 Aug 2013 17:16:31 +0000 Subject: [PATCH] Updated to the marginal factor calculation in BatchFixedLagSmoother to be more efficient --- .../nonlinear/BatchFixedLagSmoother.cpp | 281 ++++++++++++++---- .../nonlinear/BatchFixedLagSmoother.h | 4 + .../tests/testBatchFixedLagSmoother.cpp | 6 +- 3 files changed, 235 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 0a84d307a..167215db2 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -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 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& keys) { /* ************************************************************************* */ void BatchFixedLagSmoother::reorder(const std::set& 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& 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& 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 removedFactorSlots; + VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); BOOST_FOREACH(Key key, marginalizeKeys) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(slots.begin(), slots.end()); - } - - // Construct an elimination tree to perform sparse elimination - std::vector 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 indicesToEliminate; - BOOST_FOREACH(Key key, marginalizeKeys) { - indicesToEliminate.insert(ordering_.at(key)); - } - BOOST_FOREACH(Key key, marginalizeKeys) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); - } - - // 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)); - } - } + BOOST_FOREACH(size_t slot, slots) { + removedFactorSlots.insert(slot); } } - insertFactors(marginalFactors); - // Remove the marginalized variables and factors from the filter + if(debug) { + std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; + BOOST_FOREACH(size_t slot, removedFactorSlots) { + std::cout << slot << " "; + } + std::cout << std::endl; + } + + // 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)); + } + } + + 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& keys, const std::st std::cout << std::endl; } +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& 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& 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 allKeys(graph.keys()); + if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + + // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys + FastSet 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 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 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 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 diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index dd22939bf..b8597c79c 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -206,9 +206,13 @@ protected: static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); }; + static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); + static void PrintKeySet(const gtsam::FastSet& 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); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 09fe3c381..3c7405da3 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -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));