From 0b5c07e543c3840ddbb23fbfd932f58d691bd691 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 9 Apr 2013 21:24:05 +0000 Subject: [PATCH] Starting to clean up and refactor the Concurrent Filtering and Smoothing classes. Currently the synchronization is disables, as is the marginalization in the filter. --- .../nonlinear/ConcurrentBatchFilter.cpp | 1803 +++++++++-------- .../nonlinear/ConcurrentBatchFilter.h | 148 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 801 ++++---- .../nonlinear/ConcurrentBatchSmoother.h | 119 +- .../ConcurrentFilteringAndSmoothing.h | 19 +- 5 files changed, 1606 insertions(+), 1284 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 768f552bd..7026a625c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -17,114 +17,93 @@ */ #include -#include -#include +#include +#include #include -#include - -#include +#include 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"); + std::cout << " Factors:" << std::endl; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + PrintNonlinearFactor(factor, " ", keyFormatter); + } theta_.print("Values:\n"); } /* ************************************************************************* */ -ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +bool ConcurrentBatchFilter::equals(const ConcurrentFilter& rhs, double tol) const { + const ConcurrentBatchFilter* filter = dynamic_cast(&rhs); + return filter + && factors_.equals(filter->factors_) + && theta_.equals(filter->theta_) + && ordering_.equals(filter->ordering_) + && delta_.equals(filter->delta_) + && variableIndex_.equals(filter->variableIndex_) + && separatorValues_.equals(filter->separatorValues_); +} + +/* ************************************************************************* */ +ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const boost::optional >& keysToMove) { gttic(update); // Create the return result meta-data Result result; + // Update all of the internal variables with the new information gttic(augment_system); - // Add the new factors to the graph - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { - insertFactor(factor); - } // Add the new variables to theta theta_.insert(newTheta); - // Update the Timestamps associated with the factor keys - updateKeyTimestampMap(timestamps); + // Add new variables to the end of the ordering + 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); - // Optimize the graph, updating theta + // Reorder the system to ensure efficient optimization (and marginalization) performance + gttic(reorder); + reorder(keysToMove); + gttoc(reorder); + + // Optimize the factors using a modified version of L-M 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; + if(factors_.size() > 0) { + result = optimize(); } gttoc(optimize); + gttic(marginalize); + if(keysToMove && keysToMove->size() > 0){ +// // Generate a permutation that will put the factor graph into the proper order +// std::set activeKeys; +// std::set marginalizableKeys; +// calculateKeySets(activeKeys, marginalizableKeys, registeredSensors_, ordering_, keyTimestampMap_, currentTimestamp, parameters_.smootherLag); +// permuteOrdering(ordering_, factors_, marginalizableKeys, activeKeys); +// +// // Marginalize out inactive states (and remove from ordering/values) +// marginalize(factors_, theta_, ordering_, keyTimestampMap_, linearValues_, marginalizableKeys); + } + gttoc(marginalize); + gttoc(update); return result; } - - - /* ************************************************************************* */ void ConcurrentBatchFilter::presync() { @@ -135,380 +114,653 @@ void ConcurrentBatchFilter::presync() { /* ************************************************************************* */ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { + gttic(synchronize); -std::cout << "ConcurrentBatchFilter::synchronize(*) Begin" << std::endl; - -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; -} - -// 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(); - - - - - - - // 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; - - // Calculate an ordering that puts variables newer than LAG at the top - gttic(compute_ordering); - gtsam::Ordering ordering = computeOrdering(graph_); - gttoc(compute_ordering); - -ordering.print("Hmf5Batch::synchronize(*) Ordering:\n"); - - // Create a Bayes Tree using iSAM2 cliques - gttic(create_bayes_tree); - JunctionTree jt(*graph_.linearize(linpoint, ordering)); - ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); - BayesTree bayesTree; - bayesTree.insert(root); - gttoc(create_bayes_tree); - -std::cout << "ConcurrentBatchFilter::synchronize(*) Create Bayes Tree:" << std::endl; -SymbolicPrintTree(root, ordering, " "); - - - - - - // Remove smoother summarization from the graph. It will be added back at the end. - BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) { - removeFactor(slot); - } - smootherSummarizationSlots_.clear(); - - - - - - - // Find the Separator Cliques - // The root is defined as any clique with a separator key (+ the path to the root) - gttic(identify_separator_cliques); - std::set separatorKeys = findSeparatorKeys(graph_); - std::set separatorCliques; - Values oldSeparatorValues = separatorValues_; - separatorValues_.clear(); - BOOST_FOREACH(gtsam::Key key, separatorKeys) { - Index index = ordering.at(key); - const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index); - if(clique) { - separatorCliques.insert(clique); - } - } - // + the path to the root - // TODO: Given the ordering, is this actually necessary? - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - ISAM2Clique::shared_ptr separatorClique = clique; - while(!separatorClique->isRoot()) { - separatorClique = separatorClique->parent(); - separatorCliques.insert(separatorClique); - } - } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Index index, clique->conditional()->frontals()) { - separatorKeys.insert(ordering.key(index)); - } - } - BOOST_FOREACH(Key key, separatorKeys) { - separatorValues_.insert(key, linpoint.at(key)); - } - gttoc(identify_separator_cliques); - -std::cout << "ConcurrentBatchFilter::synchronize(*) New Separator Cliques:" << std::endl; -BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& clique, separatorCliques) { - std::cout << " P( "; - BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; - } - if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } - BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; - } - std::cout << ")" << std::endl; -} - -std::cout << "ConcurrentBatchFilter::synchronize(*) New Separator Keys: "; -BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, separatorValues_) { -std::cout << gtsam::DefaultKeyFormatter(key_value.key) << " "; -} -std::cout << std::endl; - - // Identify the filter and smoother branches - // First find all of the children of the separator cliques (that are not separator cliques themselves), - // Then check if the child clique contains a filter key. If so, it is a filter branch, if not, it is a smoother branch - gttic(identify_smoother_and_filter_branches); - std::set branches; - // Add all of the children to the children set - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - branches.insert(clique->children().begin(), clique->children().end()); - } - // Remove any child that is also a separator - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - branches.erase(clique); - } - // Find the set of keys inside the lag - std::set insideKeys = findKeysBefore(getCurrentTimestamp() - lag_); - // Classify each child branch as a filter or a smoother branch - std::set filterBranches; - std::set smootherBranches; - BOOST_FOREACH(const ISAM2Clique::shared_ptr& branch, branches) { - std::set frontalKeys; - BOOST_FOREACH(gtsam::Index index, branch->conditional()->frontals()) { - frontalKeys.insert(ordering.key(index)); - } - if(std::find_first_of(frontalKeys.begin(), frontalKeys.end(), insideKeys.begin(), insideKeys.end()) == frontalKeys.end()) { - // No InsideKey exists in this branch. It must be a smoother branch. - smootherBranches.insert(branch); - } else { - // An InsideKey was found. This is a filter branch. - filterBranches.insert(branch); - } - } - gttoc(identify_smoother_and_filter_branches); - -std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Branches:" << std::endl; -BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& clique, smootherBranches) { -std::cout << " P( "; -BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; -} -if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } -BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; -} -std::cout << ")" << std::endl; -} - -std::cout << "ConcurrentBatchFilter::synchronize(*) Filter Branches:" << std::endl; -BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& clique, filterBranches) { -std::cout << " P( "; -BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; -} -if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } -BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { - std::cout << gtsam::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(clique->cachedFactor())) - factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); - else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(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 separatorFactorSlots = findFactorsWithOnly(separatorKeys); - BOOST_FOREACH(size_t slot, separatorFactorSlots) { - 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(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 frontal keys in the smoother branches and grab their current linearization point - gttic(find_smoother_values); - std::set smootherKeys; - std::queue 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_values); - -std::cout << "Hmf5Batch::synchronize(*) Smoother Keys: "; -BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { -std::cout << DefaultKeyFormatter(key_value.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 - // TODO: Are the summarized factors part of the graph currently? - gttic(find_smoother_factors); - std::set 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 << "Hmf5Batch::synchronize(*) Smoother Factors:" << std::endl; -BOOST_FOREACH(const gtsam::NonlinearFactor::shared_ptr& factor, smootherFactors_) { -std::cout << " f( "; -BOOST_FOREACH(gtsam::Key key, factor->keys()) { - std::cout << gtsam::DefaultKeyFormatter(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); - NonlinearFactorGraph smootherSummarization; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { - if(smootherBranches.size() == 0) { - factor->print("Summarized Factor Before Marginalization:\n"); - NonlinearFactor::shared_ptr marginalFactor = marginalizeKeysFromFactor(factor, separatorKeys); - 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; - } - } - } - // 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(clique->cachedFactor())) - factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); - else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(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(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); - - std::cout << "ConcurrentBatchFilter::synchronize(*) End" << std::endl; gttoc(synchronize); } +/* ************************************************************************* */ +void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) { + + gttic(get_summarized_factors); + + // Copy the previous calculated smoother summarization factors into the output + summarizedFactors.push_back(filterSummarization_); + separatorValues.insert(separatorValues_); + + gttoc(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_); + + gttoc(get_smoother_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::postsync() { + + gttic(postsync); + + gttoc(postsync); +} + +/* ************************************************************************* */ +std::vector ConcurrentBatchFilter::insertFactors(const NonlinearFactorGraph& factors) { + + gttic(insert_factors); + + // create the output vector + std::vector slots; + slots.reserve(factors.size()); + + // Insert the factor into an existing hole in the factor graph, if possible + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { + size_t slot; + if(availableSlots_.size() > 0) { + slot = availableSlots_.front(); + availableSlots_.pop(); + factors_.replace(slot, factor); + } else { + slot = factors_.size(); + factors_.push_back(factor); + } + slots.push_back(slot); + } + + // Augment the Variable Index + variableIndex_.augment(*factors.symbolic(ordering_)); + + gttoc(insert_factors); + + return slots; +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { + + gttic(remove_factors); + + // For each factor slot to delete... + SymbolicFactorGraph factors; + BOOST_FOREACH(size_t slot, slots) { + // Create a symbolic version for the variable index + factors.push_back(factors_.at(slot)->symbolic(ordering_)); + + // Remove the factor from the graph + factors_.remove(slot); + + // Mark the factor slot as available + availableSlots_.push(slot); + } + + // Remove references to this factor from the VariableIndex + variableIndex_.remove(slots, factors); + + gttoc(remove_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::reorder(const boost::optional >& keysToMove) { + + // COLAMD groups will be used to place marginalize keys in Group 0, active keys in Group 1, and separator keys in Group 2 + int group0 = 0; + int group1 = group0 + (keysToMove ? 1 : 0); + int group2 = group1 + 1; + + // Initialize all variables to group1 + std::vector cmember(variableIndex_.size(), group1); + + // Set all of the keysToMove to Group0 + if(keysToMove && keysToMove->size() > 0) { + BOOST_FOREACH(Key key, *keysToMove) { + cmember[ordering_.at(key)] = group0; + } + } + + // Set all of the separator keys to Group2 + if(separatorValues_.size() > 0) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + cmember[ordering_.at(key_value.key)] = group2; + } + } + + // Generate the permutation + Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember); + + // Permute the ordering, variable index, and deltas + ordering_.permuteInPlace(forwardPermutation); + variableIndex_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); +} + +/* ************************************************************************* */ +ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() { + // Create output result structure + Result result; + result.nonlinearVariables = theta_.size() - separatorValues_.size(); + result.linearVariables = separatorValues_.size(); + + // Set optimization parameters + double lambda = parameters_.lambdaInitial; + double lambdaFactor = parameters_.lambdaFactor; + double lambdaUpperBound = parameters_.lambdaUpperBound; + double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound; + size_t maxIterations = parameters_.maxIterations; + double relativeErrorTol = parameters_.relativeErrorTol; + double absoluteErrorTol = parameters_.absoluteErrorTol; + double errorTol = parameters_.errorTol; + + // Create a Values that holds the current evaluation point + Values evalpoint = theta_.retract(delta_, ordering_); + result.error = factors_.error(evalpoint); + + // Use a custom optimization loop so the linearization points can be controlled + double previousError; + VectorValues newDelta; + do { + previousError = result.error; + + // Do next iteration + gttic(optimizer_iteration); + { + // Linearize graph around the linearization point + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + + // Keep increasing lambda until we make make progress + while(true) { + // Add prior factors at the current solution + gttic(damp); + GaussianFactorGraph dampedFactorGraph(linearFactorGraph); + dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); + { + // for each of the variables, add a prior at the current solution + for(size_t j=0; j 0) { + theta_.update(separatorValues_); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + Index index = ordering_.at(key_value.key); + delta_.at(index) = newDelta.at(index); + } + } + // Decrease lambda for next time + lambda /= lambdaFactor; + if(lambda < lambdaLowerBound) { + lambda = lambdaLowerBound; + } + // End this lambda search iteration + break; + } else { + // Reject this change + // Increase lambda and continue searching + lambda *= lambdaFactor; + if(lambda > lambdaUpperBound) { + // The maximum lambda has been used. Print a warning and end the search. + std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; + break; + } + } + } // end while + } + gttoc(optimizer_iteration); + + result.iterations++; + } while(result.iterations < maxIterations && + !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); + + return result; +} + + +/* ************************************************************************* */ +void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, + const std::string& indent, const KeyFormatter& keyFormatter) { + std::cout << indent; + if(factor) { + if(boost::dynamic_pointer_cast(factor)) { + std::cout << "l( "; + } else { + std::cout << "f( "; + } + BOOST_FOREACH(Key key, *factor) { + std::cout << keyFormatter(key) << " "; + } + std::cout << ")" << std::endl; + } else { + std::cout << "{ NULL }" << std::endl; + } +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + const std::string& indent, const KeyFormatter& keyFormatter) { + std::cout << indent; + if(factor) { + std::cout << "g( "; + BOOST_FOREACH(Index index, *factor) { + std::cout << keyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + } else { + std::cout << "{ NULL }" << std::endl; + } +} + +///* ************************************************************************* */ +//void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { +// +// gttic(synchronize); +// +// std::cout << "ConcurrentBatchFilter::synchronize(*) Begin" << std::endl; +// +//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; +//} +// +//// 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(); +// +// +// +// +// +// +// // 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; +// +// // Calculate an ordering that puts variables newer than LAG at the top +// gttic(compute_ordering); +// Ordering ordering = computeOrdering(graph_); +// gttoc(compute_ordering); +// +//ordering.print("Hmf5Batch::synchronize(*) Ordering:\n"); +// +// // Create a Bayes Tree using iSAM2 cliques +// gttic(create_bayes_tree); +// JunctionTree jt(*graph_.linearize(linpoint, ordering)); +// ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); +// BayesTree bayesTree; +// bayesTree.insert(root); +// gttoc(create_bayes_tree); +// +//std::cout << "ConcurrentBatchFilter::synchronize(*) Create Bayes Tree:" << std::endl; +//SymbolicPrintTree(root, ordering, " "); +// +// +// +// +// +// // Remove smoother summarization from the graph. It will be added back at the end. +// BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) { +// removeFactor(slot); +// } +// smootherSummarizationSlots_.clear(); +// +// +// +// +// +// +// // Find the Separator Cliques +// // The root is defined as any clique with a separator key (+ the path to the root) +// gttic(identify_separator_cliques); +// std::set separatorKeys = findSeparatorKeys(graph_); +// std::set separatorCliques; +// Values oldSeparatorValues = separatorValues_; +// separatorValues_.clear(); +// BOOST_FOREACH(Key key, separatorKeys) { +// Index index = ordering.at(key); +// const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index); +// if(clique) { +// separatorCliques.insert(clique); +// } +// } +// // + the path to the root +// // TODO: Given the ordering, is this actually necessary? +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { +// ISAM2Clique::shared_ptr separatorClique = clique; +// while(!separatorClique->isRoot()) { +// separatorClique = separatorClique->parent(); +// separatorCliques.insert(separatorClique); +// } +// } +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { +// BOOST_FOREACH(Index index, clique->conditional()->frontals()) { +// separatorKeys.insert(ordering.key(index)); +// } +// } +// BOOST_FOREACH(Key key, separatorKeys) { +// separatorValues_.insert(key, linpoint.at(key)); +// } +// gttoc(identify_separator_cliques); +// +//std::cout << "ConcurrentBatchFilter::synchronize(*) New Separator Cliques:" << std::endl; +//BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { +// 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; +//} +// +//std::cout << "ConcurrentBatchFilter::synchronize(*) New Separator Keys: "; +//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { +//std::cout << DefaultKeyFormatter(key_value.key) << " "; +//} +//std::cout << std::endl; +// +// // Identify the filter and smoother branches +// // First find all of the children of the separator cliques (that are not separator cliques themselves), +// // Then check if the child clique contains a filter key. If so, it is a filter branch, if not, it is a smoother branch +// gttic(identify_smoother_and_filter_branches); +// std::set branches; +// // Add all of the children to the children set +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { +// branches.insert(clique->children().begin(), clique->children().end()); +// } +// // Remove any child that is also a separator +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { +// branches.erase(clique); +// } +// // Find the set of keys inside the lag +// std::set insideKeys = findKeysBefore(getCurrentTimestamp() - lag_); +// // Classify each child branch as a filter or a smoother branch +// std::set filterBranches; +// std::set smootherBranches; +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& branch, branches) { +// std::set frontalKeys; +// BOOST_FOREACH(Index index, branch->conditional()->frontals()) { +// frontalKeys.insert(ordering.key(index)); +// } +// if(std::find_first_of(frontalKeys.begin(), frontalKeys.end(), insideKeys.begin(), insideKeys.end()) == frontalKeys.end()) { +// // No InsideKey exists in this branch. It must be a smoother branch. +// smootherBranches.insert(branch); +// } else { +// // An InsideKey was found. This is a filter branch. +// filterBranches.insert(branch); +// } +// } +// gttoc(identify_smoother_and_filter_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; +//} +// +//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(clique->cachedFactor())) +// factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); +// else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(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 separatorFactorSlots = findFactorsWithOnly(separatorKeys); +// BOOST_FOREACH(size_t slot, separatorFactorSlots) { +// 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(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 frontal keys in the smoother branches and grab their current linearization point +// gttic(find_smoother_values); +// std::set smootherKeys; +// std::queue 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_values); +// +//std::cout << "Hmf5Batch::synchronize(*) Smoother Keys: "; +//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { +//std::cout << DefaultKeyFormatter(key_value.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 +// // TODO: Are the summarized factors part of the graph currently? +// gttic(find_smoother_factors); +// std::set 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 << "Hmf5Batch::synchronize(*) Smoother Factors:" << std::endl; +//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors_) { +//std::cout << " f( "; +//BOOST_FOREACH(Key key, factor->keys()) { +// std::cout << DefaultKeyFormatter(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); +// NonlinearFactorGraph smootherSummarization; +// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { +// if(smootherBranches.size() == 0) { +// factor->print("Summarized Factor Before Marginalization:\n"); +// NonlinearFactor::shared_ptr marginalFactor = marginalizeKeysFromFactor(factor, separatorKeys); +// 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; +// } +// } +// } +// // 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(clique->cachedFactor())) +// factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); +// else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(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(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); +// +// std::cout << "ConcurrentBatchFilter::synchronize(*) End" << std::endl; +// gttoc(synchronize); +//} +// ///* ************************************************************************* */ //void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { // gttic(synchronize); @@ -1000,373 +1252,298 @@ std::cout << ")" << 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_); - - gttoc(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_); - - gttoc(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 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::max(); - } - - gttoc(get_current_timestamp); - - return timestamp; -} - -/* ************************************************************************* */ -std::set ConcurrentBatchFilter::findKeysBefore(double timestamp) const { - - gttic(find_keys_before); - - std::set 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 ConcurrentBatchFilter::findKeysAfter(double timestamp) const { - - gttic(find_keys_after); - - std::set 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 ConcurrentBatchFilter::findFactorsWithAny(const std::set& keys) const { - // Find the set of factor slots for each specified key - std::set 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 ConcurrentBatchFilter::findFactorsWithOnly(const std::set& keys) const { - // Find the set of factor slots with any of the provided keys - std::set factorSlots = findFactorsWithAny(keys); - // Test each factor for non-specified keys - std::set::iterator slot = factorSlots.begin(); - while(slot != factorSlots.end()) { - const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); - std::set 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; -} - -/* ************************************************************************* */ -std::set ConcurrentBatchFilter::findSeparatorKeys(const NonlinearFactorGraph& graph) const { - - // Create a random ordering - gtsam::Ordering ordering = *theta_.orderingArbitrary(); - - // Calculate a Variable Index using this ordering - gtsam::VariableIndex variableIndex(graph, theta_.size()); - - // Find the set of keys outside the smoothing lag - std::set outsideKeys = findKeysBefore(getCurrentTimestamp() - lag_); - - // Find the set of factors that involve OutsideKeys - std::set outsideFactorSlots; - BOOST_FOREACH(gtsam::Key key, outsideKeys) { - gtsam::Index index = ordering.at(key); - BOOST_FOREACH(size_t factorSlot, variableIndex[index]) { - outsideFactorSlots.insert(factorSlot); - } - } - - // Find the set of all keys involved in OutsideFactors - std::set involvedKeys; - BOOST_FOREACH(size_t factorSlot, outsideFactorSlots) { - BOOST_FOREACH(gtsam::Key key, graph.at(factorSlot)->keys()) { - involvedKeys.insert(key); - } - } - - // The difference between the OutsideKeys and the InvolvedKeys is the set of keys - // inside the lag, but attached to keys outside the lag... i.e. the separator - std::set separatorKeys; - std::set_difference(involvedKeys.begin(), involvedKeys.end(), outsideKeys.begin(), outsideKeys.end(), std::inserter(separatorKeys, separatorKeys.end())); - - return separatorKeys; -} - -/* ************************************************************************* */ -Ordering ConcurrentBatchFilter::computeOrdering(const NonlinearFactorGraph& graph) const { - - // Find the set of keys outside the smoothing lag - std::set outsideKeys = findKeysBefore(getCurrentTimestamp() - lag_); - - // Find the set of keys inseide the smoother lag - std::set insideKeys = findKeysAfter(getCurrentTimestamp() - lag_); - - // Find the separator keys - std::set separatorKeys = findSeparatorKeys(graph); - - // Remove separator keys from inside keys - BOOST_FOREACH(gtsam::Key key, separatorKeys) { - insideKeys.erase(key); - } - - // Create a set of COLAMD constraints to create an ordering {OutsideKeys InsideKeys SeparatorKeys} - typedef std::map Constraints; - Constraints orderingConstraints; - int group = 0; - if(outsideKeys.size() > 0) { - BOOST_FOREACH(gtsam::Key key, outsideKeys) { - orderingConstraints[key] = group; - } - ++group; - } - if(insideKeys.size() > 0) { - BOOST_FOREACH(gtsam::Key key, insideKeys) { - orderingConstraints[key] = group; - } - ++group; - } - if(separatorKeys.size() > 0) { - BOOST_FOREACH(gtsam::Key key, separatorKeys) { - orderingConstraints[key] = group; - } - ++group; - } - - // Create the constrained ordering - return *graph.orderingCOLAMDConstrained(theta_, orderingConstraints); -} - -/* ************************************************************************* */ -NonlinearFactor::shared_ptr ConcurrentBatchFilter::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& remainingKeys) const { - - const bool debug = false; - if(debug) std::cout << "ConcurrentBatchFilter::marginalizeKeys() START" << std::endl; - - LinearizedGaussianFactor::shared_ptr linearizedFactor = boost::dynamic_pointer_cast(factor); - assert(linearizedFactor); - - // Sort the keys for this factor - std::set factorKeys; - BOOST_FOREACH(Key key, *linearizedFactor) { - factorKeys.insert(key); - } - - // Calculate the set of keys to marginalize - std::set 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 variables; - BOOST_FOREACH(Key key, marginalizeKeys) { - variables.push_back(ordering.at(key)); - } - std::pair 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(graph.at(0)); - assert(jacobianFactor); - marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, linearizedFactor->linearizationPoint())); - } - return marginalFactor; - } -} +// +///* ************************************************************************* */ +//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 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::max(); +// } +// +// gttoc(get_current_timestamp); +// +// return timestamp; +//} +// +///* ************************************************************************* */ +//std::set ConcurrentBatchFilter::findKeysBefore(double timestamp) const { +// +// gttic(find_keys_before); +// +// std::set 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 ConcurrentBatchFilter::findKeysAfter(double timestamp) const { +// +// gttic(find_keys_after); +// +// std::set 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 ConcurrentBatchFilter::findFactorsWithAny(const std::set& keys) const { +// // Find the set of factor slots for each specified key +// std::set 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 ConcurrentBatchFilter::findFactorsWithOnly(const std::set& keys) const { +// // Find the set of factor slots with any of the provided keys +// std::set factorSlots = findFactorsWithAny(keys); +// // Test each factor for non-specified keys +// std::set::iterator slot = factorSlots.begin(); +// while(slot != factorSlots.end()) { +// const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); +// std::set 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; +//} +// +///* ************************************************************************* */ +//std::set ConcurrentBatchFilter::findSeparatorKeys(const NonlinearFactorGraph& graph) const { +// +// // Create a random ordering +// Ordering ordering = *theta_.orderingArbitrary(); +// +// // Calculate a Variable Index using this ordering +// VariableIndex variableIndex(graph, theta_.size()); +// +// // Find the set of keys outside the smoothing lag +// std::set outsideKeys = findKeysBefore(getCurrentTimestamp() - lag_); +// +// // Find the set of factors that involve OutsideKeys +// std::set outsideFactorSlots; +// BOOST_FOREACH(Key key, outsideKeys) { +// Index index = ordering.at(key); +// BOOST_FOREACH(size_t factorSlot, variableIndex[index]) { +// outsideFactorSlots.insert(factorSlot); +// } +// } +// +// // Find the set of all keys involved in OutsideFactors +// std::set involvedKeys; +// BOOST_FOREACH(size_t factorSlot, outsideFactorSlots) { +// BOOST_FOREACH(Key key, graph.at(factorSlot)->keys()) { +// involvedKeys.insert(key); +// } +// } +// +// // The difference between the OutsideKeys and the InvolvedKeys is the set of keys +// // inside the lag, but attached to keys outside the lag... i.e. the separator +// std::set separatorKeys; +// std::set_difference(involvedKeys.begin(), involvedKeys.end(), outsideKeys.begin(), outsideKeys.end(), std::inserter(separatorKeys, separatorKeys.end())); +// +// return separatorKeys; +//} +// +///* ************************************************************************* */ +//Ordering ConcurrentBatchFilter::computeOrdering(const NonlinearFactorGraph& graph) const { +// +// // Find the set of keys outside the smoothing lag +// std::set outsideKeys = findKeysBefore(getCurrentTimestamp() - lag_); +// +// // Find the set of keys inseide the smoother lag +// std::set insideKeys = findKeysAfter(getCurrentTimestamp() - lag_); +// +// // Find the separator keys +// std::set separatorKeys = findSeparatorKeys(graph); +// +// // Remove separator keys from inside keys +// BOOST_FOREACH(Key key, separatorKeys) { +// insideKeys.erase(key); +// } +// +// // Create a set of COLAMD constraints to create an ordering {OutsideKeys InsideKeys SeparatorKeys} +// typedef std::map Constraints; +// Constraints orderingConstraints; +// int group = 0; +// if(outsideKeys.size() > 0) { +// BOOST_FOREACH(Key key, outsideKeys) { +// orderingConstraints[key] = group; +// } +// ++group; +// } +// if(insideKeys.size() > 0) { +// BOOST_FOREACH(Key key, insideKeys) { +// orderingConstraints[key] = group; +// } +// ++group; +// } +// if(separatorKeys.size() > 0) { +// BOOST_FOREACH(Key key, separatorKeys) { +// orderingConstraints[key] = group; +// } +// ++group; +// } +// +// // Create the constrained ordering +// return *graph.orderingCOLAMDConstrained(theta_, orderingConstraints); +//} +// +///* ************************************************************************* */ +//NonlinearFactor::shared_ptr ConcurrentBatchFilter::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& remainingKeys) const { +// +// const bool debug = false; +// if(debug) std::cout << "ConcurrentBatchFilter::marginalizeKeys() START" << std::endl; +// +// LinearizedGaussianFactor::shared_ptr linearizedFactor = boost::dynamic_pointer_cast(factor); +// assert(linearizedFactor); +// +// // Sort the keys for this factor +// std::set factorKeys; +// BOOST_FOREACH(Key key, *linearizedFactor) { +// factorKeys.insert(key); +// } +// +// // Calculate the set of keys to marginalize +// std::set 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 variables; +// BOOST_FOREACH(Key key, marginalizeKeys) { +// variables.push_back(ordering.at(key)); +// } +// std::pair 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(graph.at(0)); +// assert(jacobianFactor); +// marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, linearizedFactor->linearizationPoint())); +// } +// return marginalFactor; +// } +//} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 2ea9eadbd..481563e17 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -19,10 +19,8 @@ // \callgraph #pragma once -#include "ConcurrentFilteringAndSmoothing.h" +#include #include -#include -#include #include namespace gtsam { @@ -33,37 +31,61 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter { public: - + typedef boost::shared_ptr shared_ptr; typedef ConcurrentFilter Base; ///< typedef for base class - typedef std::map KeyTimestampMap; ///< Typedef for a Key-Timestamp map/database - typedef std::multimap TimestampKeyMap;///< Typedef for a Timestamp-Key map/database - /** - * Meta information returned about the update - */ - // TODO: Think of some more things to put here + /** Meta information returned about the update */ struct Result { size_t iterations; ///< The number of optimizer iterations performed + size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point double error; ///< The final factor graph error - Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}; }; /** Default constructor */ - ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters, double lag) : parameters_(parameters), lag_(lag) {}; + ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters) : parameters_(parameters) {}; /** Default destructor */ virtual ~ConcurrentBatchFilter() {}; - // Implement a GTSAM standard 'print' function + /** Implement a GTSAM standard 'print' function */ void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** Check if two Concurrent Filters are equal */ + bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const; + + /** Access the current set of factors */ + const NonlinearFactorGraph& getFactors() const { + return factors_; + } + + /** Access the current linearization point */ + const Values& getLinearizationPoint() const { + return theta_; + } + + /** Access the current ordering */ + const Ordering& getOrdering() const { + return ordering_; + } + + /** Access the current set of deltas to the linearization point */ + const VectorValues& getDelta() const { + return delta_; + } + + /** Access the nonlinear variable index */ + const VariableIndex& getVariableIndex() const { + return variableIndex_; + } + /** Compute the current best estimate of all variables and return a full Values structure. * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_; + return getLinearizationPoint().retract(getDelta(), getOrdering()); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -72,8 +94,10 @@ public: * @return */ template - VALUE calculateEstimate(const Key key) const { - return theta_.at(key); + VALUE calculateEstimate(Key key) const { + const Index index = getOrdering().at(key); + const Vector delta = getDelta().at(index); + return getLinearizationPoint().at(key).retract(delta); } /** @@ -86,30 +110,25 @@ public: * @param newTheta Initialization points for new variables to be added to the filter * You must include here all new variables occurring in newFactors that were not already * in the filter. + * @param keysToMove An optional set of keys to remove from the filter and */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap()); + const boost::optional >& keysToMove = boost::none); protected: - /** A typedef defining an Key-Factor mapping **/ - typedef std::map > FactorIndex; - LevenbergMarquardtParams parameters_; ///< LM parameters - double lag_; ///< Time before keys are transitioned from the filter to the smoother - NonlinearFactorGraph graph_; ///< The graph of all the smoother factors - Values theta_; ///< Current solution - TimestampKeyMap timestampKeyMap_; ///< The set of keys associated with each timestamp - KeyTimestampMap keyTimestampMap_; ///< The timestamp associated with each key + NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter + Values theta_; ///< Current linearization point of all variables in the filter + Ordering ordering_; ///< The current ordering used to calculate the linear deltas + VectorValues delta_; ///< The current set of linear deltas from the linearization point + VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable std::queue 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 smootherSummarizationSlots_; ///< The slots in graph for the last set of smoother summarized factors - Values separatorValues_; + Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. + std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors // Storage for information to be sent to the smoother NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother - Values rootValues_; ///< The set of keys to be kept in the root and their linearization points NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother @@ -126,7 +145,7 @@ protected: * @param summarizedFactors The summarized factors for the filter branch * @param rootValues The linearization points of the root clique variables */ - virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues); + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues); /** * Populate the provided containers with factors being sent to the smoother from the filter. These @@ -152,47 +171,34 @@ protected: virtual void postsync(); - /** Augment the graph with a new factor - * - * @param factor The new factor to add to the graph - * @return The slot in the graph where the factor was inserted - */ - size_t insertFactor(const NonlinearFactor::shared_ptr& factor); - - /** Remove a factor from the graph by slot index */ - void removeFactor(size_t slot); - - /** Remove the specified key from all data structures */ - void removeKey(Key key); - - /** Update the Timestamps associated with the keys */ - void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); - - /** Find the most recent timestamp of the system */ - double getCurrentTimestamp() const; - - /** Find all of the keys associated with timestamps before the provided time */ - std::set findKeysBefore(double timestamp) const; - - /** Find all of the keys associated with timestamps after the provided time */ - std::set findKeysAfter(double timestamp) const; - - /** Find all of the nonlinear factors that contain any of the provided keys */ - std::set findFactorsWithAny(const std::set& keys) const; - - /** Find all of the nonlinear factors that contain only the provided keys */ - std::set findFactorsWithOnly(const std::set& 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& remainingKeys) const; - - std::set findSeparatorKeys(const gtsam::NonlinearFactorGraph& graph) const; - - gtsam::Ordering computeOrdering(const gtsam::NonlinearFactorGraph& graph) const; - private: - typedef BayesTree::sharedClique Clique; - static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = ""); + + /** Augment the graph with new factors + * + * @param factors The factors to add to the graph + * @return The slots in the graph where they were inserted + */ + std::vector insertFactors(const NonlinearFactorGraph& factors); + + /** Remove factors from the graph by slot index + * + * @param slots The slots in the factor graph that should be deleted + * */ + void removeFactors(const std::vector& slots); + + /** Use colamd to update into an efficient ordering */ + void reorder(const boost::optional >& keysToMove = boost::none); + + /** Use a modified version of L-M to update the linearization point and delta */ + Result optimize(); + + /** Print just the nonlinear keys in a nonlinear factor */ + static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, + const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** Print just the nonlinear keys in a linear factor */ + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchFilter diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 2507fc8a2..a973dd42a 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -18,38 +18,32 @@ #include #include -#include +#include #include #include -#include namespace gtsam { -///* ************************************************************************* */ -//void ConcurrentBatchSmoother::SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent) { -// std::cout << indent << "P( "; -// BOOST_FOREACH(Index index, clique->conditional()->frontals()){ -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// if(clique->conditional()->nrParents() > 0) { -// std::cout << "| "; -// } -// BOOST_FOREACH(Index index, clique->conditional()->parents()){ -// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; -// } -// std::cout << ")" << std::endl; -// -// BOOST_FOREACH(const Clique& child, clique->children()) { -// SymbolicPrintTree(child, ordering, indent+" "); -// } -//} +/* ************************************************************************* */ +void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s; + std::cout << " Factors:" << std::endl; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + PrintNonlinearFactor(factor, " ", keyFormatter); + } + theta_.print("Values:\n"); +} /* ************************************************************************* */ -void ConcurrentBatchSmoother::print(const std::string& s, - const KeyFormatter& keyFormatter) const { - std::cout << s; - graph_.print("Factors:\n"); - theta_.print("Values:\n"); +bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) const { + const ConcurrentBatchSmoother* smoother = dynamic_cast(&rhs); + return smoother + && factors_.equals(smoother->factors_) + && theta_.equals(smoother->theta_) + && ordering_.equals(smoother->ordering_) + && delta_.equals(smoother->delta_) + && variableIndex_.equals(smoother->variableIndex_) + && separatorValues_.equals(smoother->separatorValues_); } /* ************************************************************************* */ @@ -57,164 +51,217 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF gttic(update); - // Create result structure + // Create the return result meta-data Result result; + // Update all of the internal variables with the new information gttic(augment_system); - // Add the new factors to the graph - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { - insertFactor(factor); - } // Add the new variables to theta theta_.insert(newTheta); + // Add new variables to the end of the ordering + std::vector dims; + dims.reserve(newTheta.size()); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + ordering_.push_back(key_value.key); + dims.push_back(key_value.value.dim()); + } + // Augment Delta + delta_.append(dims); + for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { + delta_[i].setZero(); + } + // Add the new factors to the graph, updating the variable index + insertFactors(newFactors); gttoc(augment_system); - // Optimize the graph, updating theta - gttic(optimize); - if(graph_.size() > 0){ - optimize(); + // Reorder the system to ensure efficient optimization (and marginalization) performance + gttic(reorder); + reorder(); + gttoc(reorder); - // TODO: fill in the results structure properly - result.iterations = 0; - result.nonlinearVariables = theta_.size() - separatorValues_.size(); - result.linearVariables = separatorValues_.size(); - result.error = 0; + // Optimize the factors using a modified version of L-M + gttic(optimize); + if(factors_.size() > 0) { + result = optimize(); } gttoc(optimize); - // Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these - // calculations while the filter is still running + gttic(presync); - // Calculate and store the information passed up to the separator. This requires: - // 1) Calculate an ordering that forces the separator variables to be in the root - // 2) Eliminate all of the variables except the root. This produces one or more - // linear, marginal factors on the separator variables. - // 3) Convert the marginal factors into nonlinear ones using the 'LinearContainerFactor' - - if(separatorValues_.size() > 0) { - // Force variables associated with root keys to keep the same linearization point - gttic(enforce_consistency); - Values linpoint; - linpoint.insert(theta_); - linpoint.insert(separatorValues_); - -//linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n"); - - gttoc(enforce_consistency); - - // Calculate a root-constrained ordering - gttic(compute_ordering); - std::map constraints; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - constraints[key_value.key] = 1; - } - Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints); - gttoc(compute_ordering); - - // Create a Bayes Tree using iSAM2 cliques - gttic(create_bayes_tree); - JunctionTree jt(*graph_.linearize(linpoint, ordering)); - ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); - BayesTree bayesTree; - bayesTree.insert(root); - gttoc(create_bayes_tree); - -//ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n"); -std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; -std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl; -//SymbolicPrintTree(root, ordering, " "); - - // Extract the marginal factors from the smoother - // For any non-filter factor that involves a root variable, - // calculate its marginal on the root variables using the - // current linearization point. - - // Find all of the smoother branches as the children of root cliques that are not also root cliques - gttic(find_smoother_branches); - std::set rootCliques; - std::set smootherBranches; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key)); - if(clique) { - rootCliques.insert(clique); - smootherBranches.insert(clique->children().begin(), clique->children().end()); - } - } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) { - smootherBranches.erase(rootClique); - } - gttoc(find_smoother_branches); - - // Extract the cached factors on the root cliques from the smoother branches - gttic(extract_cached_factors); - GaussianFactorGraph cachedFactors; - BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { - cachedFactors.push_back(clique->cachedFactor()); - } - gttoc(extract_cached_factors); - -std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl; -BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { - std::cout << " g( "; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; - } - std::cout << ")" << std::endl; -} - - // Marginalize out any additional (non-root) variables - gttic(marginalize_extra_variables); - // The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 } - Index minRootIndex = linpoint.size() - separatorValues_.size(); - // Calculate the set of keys to be marginalized - FastSet cachedIndices = cachedFactors.keys(); - std::vector marginalizeIndices; - std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex); - -std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: "; -BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; } -std::cout << std::endl; - - // If non-root-keys are present, marginalize them out - if(marginalizeIndices.size() > 0) { - // Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph - GaussianConditional::shared_ptr conditional; - boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction()); - } - gttoc(marginalize_extra_variables); - -std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl; -BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { - std::cout << " g( "; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; - } - std::cout << ")" << std::endl; -} - - // Convert factors into 'Linearized' nonlinear factors - gttic(store_cached_factors); - smootherSummarization_.resize(0); - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) { - LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(gaussianFactor, ordering, linpoint)); - smootherSummarization_.push_back(factor); - } - gttoc(store_cached_factors); - -std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl; -BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) { - std::cout << " f( "; - BOOST_FOREACH(Key key, factor->keys()) { - std::cout << DefaultKeyFormatter(key) << " "; - } - std::cout << ")" << std::endl; -} - - } gttoc(presync); gttoc(update); + + + + + + + + + + +// gttic(update); +// +// // Create result structure +// Result result; +// +// gttic(augment_system); +// // Add the new factors to the graph +// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { +// insertFactor(factor); +// } +// // Add the new variables to theta +// theta_.insert(newTheta); +// gttoc(augment_system); +// +// // Optimize the graph, updating theta +// gttic(optimize); +// if(graph_.size() > 0){ +// optimize(); +// +// // TODO: fill in the results structure properly +// result.iterations = 0; +// result.nonlinearVariables = theta_.size() - separatorValues_.size(); +// result.linearVariables = separatorValues_.size(); +// result.error = 0; +// } +// gttoc(optimize); +// +// // Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these +// // calculations while the filter is still running +// gttic(presync); +// // Calculate and store the information passed up to the separator. This requires: +// // 1) Calculate an ordering that forces the separator variables to be in the root +// // 2) Eliminate all of the variables except the root. This produces one or more +// // linear, marginal factors on the separator variables. +// // 3) Convert the marginal factors into nonlinear ones using the 'LinearContainerFactor' +// +// if(separatorValues_.size() > 0) { +// // Force variables associated with root keys to keep the same linearization point +// gttic(enforce_consistency); +// Values linpoint; +// linpoint.insert(theta_); +// linpoint.insert(separatorValues_); +// +////linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n"); +// +// gttoc(enforce_consistency); +// +// // Calculate a root-constrained ordering +// gttic(compute_ordering); +// std::map constraints; +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { +// constraints[key_value.key] = 1; +// } +// Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints); +// gttoc(compute_ordering); +// +// // Create a Bayes Tree using iSAM2 cliques +// gttic(create_bayes_tree); +// JunctionTree jt(*graph_.linearize(linpoint, ordering)); +// ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); +// BayesTree bayesTree; +// bayesTree.insert(root); +// gttoc(create_bayes_tree); +// +////ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n"); +//std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; +//std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl; +////SymbolicPrintTree(root, ordering, " "); +// +// // Extract the marginal factors from the smoother +// // For any non-filter factor that involves a root variable, +// // calculate its marginal on the root variables using the +// // current linearization point. +// +// // Find all of the smoother branches as the children of root cliques that are not also root cliques +// gttic(find_smoother_branches); +// std::set rootCliques; +// std::set smootherBranches; +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { +// const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key)); +// if(clique) { +// rootCliques.insert(clique); +// smootherBranches.insert(clique->children().begin(), clique->children().end()); +// } +// } +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) { +// smootherBranches.erase(rootClique); +// } +// gttoc(find_smoother_branches); +// +// // Extract the cached factors on the root cliques from the smoother branches +// gttic(extract_cached_factors); +// GaussianFactorGraph cachedFactors; +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { +// cachedFactors.push_back(clique->cachedFactor()); +// } +// gttoc(extract_cached_factors); +// +//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl; +//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { +// std::cout << " g( "; +// BOOST_FOREACH(Index index, factor->keys()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +// } +// std::cout << ")" << std::endl; +//} +// +// // Marginalize out any additional (non-root) variables +// gttic(marginalize_extra_variables); +// // The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 } +// Index minRootIndex = linpoint.size() - separatorValues_.size(); +// // Calculate the set of keys to be marginalized +// FastSet cachedIndices = cachedFactors.keys(); +// std::vector marginalizeIndices; +// std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex); +// +//std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: "; +//BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; } +//std::cout << std::endl; +// +// // If non-root-keys are present, marginalize them out +// if(marginalizeIndices.size() > 0) { +// // Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph +// GaussianConditional::shared_ptr conditional; +// boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction()); +// } +// gttoc(marginalize_extra_variables); +// +//std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl; +//BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { +// std::cout << " g( "; +// BOOST_FOREACH(Index index, factor->keys()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +// } +// std::cout << ")" << std::endl; +//} +// +// // Convert factors into 'Linearized' nonlinear factors +// gttic(store_cached_factors); +// smootherSummarization_.resize(0); +// BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) { +// LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(gaussianFactor, ordering, linpoint)); +// smootherSummarization_.push_back(factor); +// } +// gttoc(store_cached_factors); +// +//std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl; +//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) { +// std::cout << " f( "; +// BOOST_FOREACH(Key key, factor->keys()) { +// std::cout << DefaultKeyFormatter(key) << " "; +// } +// std::cout << ")" << std::endl; +//} +// +// } +// gttoc(presync); +// +// gttoc(update); + return result; } @@ -223,7 +270,6 @@ void ConcurrentBatchSmoother::presync() { gttic(presync); - gttoc(presync); } @@ -245,23 +291,43 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa gttic(synchronize); // Remove the previous filter summarization from the graph - BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { - removeFactor(slot); - } - filterSummarizationSlots_.clear(); + removeFactors(filterSummarizationSlots_); - // Insert the new filter summarized factors - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { - filterSummarizationSlots_.push_back(insertFactor(factor)); + // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta + std::vector dims; + dims.reserve(smootherValues.size() + separatorValues.size()); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) { + Values::iterator iter = theta_.find(key_value.key); + if(iter == theta_.end()) { + theta_.insert(key_value.key, key_value.value); + ordering_.push_back(key_value.key); + dims.push_back(key_value.value.dim()); + } else { + iter->value = key_value.value; + } + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) { + Values::iterator iter = theta_.find(key_value.key); + if(iter == theta_.end()) { + theta_.insert(key_value.key, key_value.value); + ordering_.push_back(key_value.key); + dims.push_back(key_value.value.dim()); + } else { + iter->value = key_value.value; + } + } + + // Augment Delta + delta_.append(dims); + for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { + delta_[i].setZero(); } // Insert the new smoother factors - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors) { - insertFactor(factor); - } + insertFactors(smootherFactors); - // Insert new linpoints into the values - theta_.insert(smootherValues); + // Insert the new filter summarized factors + filterSummarizationSlots_ = insertFactors(summarizedFactors); // Update the list of root keys separatorValues_ = separatorValues; @@ -278,167 +344,175 @@ void ConcurrentBatchSmoother::postsync() { } /* ************************************************************************* */ -size_t ConcurrentBatchSmoother::insertFactor(const NonlinearFactor::shared_ptr& factor) { +std::vector ConcurrentBatchSmoother::insertFactors(const NonlinearFactorGraph& factors) { gttic(insert_factors); + // create the output vector + std::vector slots; + slots.reserve(factors.size()); + // 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); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { + size_t slot; + if(availableSlots_.size() > 0) { + slot = availableSlots_.front(); + availableSlots_.pop(); + factors_.replace(slot, factor); + } else { + slot = factors_.size(); + factors_.push_back(factor); + } + slots.push_back(slot); } + // Augment the Variable Index + variableIndex_.augment(*factors.symbolic(ordering_)); + gttoc(insert_factors); - return slot; + return slots; } /* ************************************************************************* */ -void ConcurrentBatchSmoother::removeFactor(size_t slot) { +void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { gttic(remove_factors); - // Remove references to this factor from the FactorIndex - BOOST_FOREACH(Key key, *(graph_.at(slot))) { - factorIndex_[key].erase(slot); + // For each factor slot to delete... + SymbolicFactorGraph factors; + BOOST_FOREACH(size_t slot, slots) { + // Create a symbolic version for the variable index + factors.push_back(factors_.at(slot)->symbolic(ordering_)); + + // Remove the factor from the graph + factors_.remove(slot); + + // Mark the factor slot as available + availableSlots_.push(slot); } - // Remove this factor from the graph - graph_.remove(slot); - // Mark the factor slot as avaiable - availableSlots_.push(slot); + + // Remove references to this factor from the VariableIndex + variableIndex_.remove(slots, factors); gttoc(remove_factors); } /* ************************************************************************* */ -std::set ConcurrentBatchSmoother::findFactorsWithAny(const std::set& keys) const { - // Find the set of factor slots for each specified key - std::set factorSlots; - BOOST_FOREACH(Key key, keys) { - FactorIndex::const_iterator iter = factorIndex_.find(key); - if(iter != factorIndex_.end()) { - factorSlots.insert(iter->second.begin(), iter->second.end()); +void ConcurrentBatchSmoother::reorder() { + + // Initialize all variables to group0 + std::vector cmember(variableIndex_.size(), 0); + + // Set all of the separator keys to Group1 + if(separatorValues_.size() > 0) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + cmember[ordering_.at(key_value.key)] = 1; } } - return factorSlots; + // Generate the permutation + Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember); + + // Permute the ordering, variable index, and deltas + ordering_.permuteInPlace(forwardPermutation); + variableIndex_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); } /* ************************************************************************* */ -std::set ConcurrentBatchSmoother::findFactorsWithOnly(const std::set& keys) const { - // Find the set of factor slots with any of the provided keys - std::set factorSlots = findFactorsWithAny(keys); - // Test each factor for non-specified keys - std::set::iterator slot = factorSlots.begin(); - while(slot != factorSlots.end()) { - const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); - std::set factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted - if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) { - factorSlots.erase(slot++); - } else { - ++slot; - } - } +ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { - return factorSlots; -} + // Create output result structure + Result result; + result.nonlinearVariables = theta_.size() - separatorValues_.size(); + result.linearVariables = separatorValues_.size(); -/* ************************************************************************* */ -void ConcurrentBatchSmoother::optimize() { + // Set optimization parameters + double lambda = parameters_.lambdaInitial; + double lambdaFactor = parameters_.lambdaFactor; + double lambdaUpperBound = parameters_.lambdaUpperBound; + double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound; + size_t maxIterations = parameters_.maxIterations; + double relativeErrorTol = parameters_.relativeErrorTol; + double absoluteErrorTol = parameters_.absoluteErrorTol; + double errorTol = parameters_.errorTol; - bool debug = ISDEBUG("ConcurrentBatchSmoother::optimize"); - - if(debug) std::cout << "ConcurrentBatchSmoother::optimize() Start" << std::endl; - - // Calculate a good ordering - // TODO: - - // Create a Values that holds the current evaluation point (simply use the linpoint initially, as a full delta is not available) - gtsam::Values evalpoint = theta_.retract(delta_, ordering_); + // Create a Values that holds the current evaluation point + Values evalpoint = theta_.retract(delta_, ordering_); + result.error = factors_.error(evalpoint); // Use a custom optimization loop so the linearization points can be controlled - double currentError = graph_.error(evalpoint); double previousError; - gtsam::VectorValues newDelta; - size_t iterations = 0; - double lambda = parameters_.lambdaInitial; + VectorValues newDelta; do { - previousError = currentError; + previousError = result.error; // Do next iteration gttic(optimizer_iteration); { // Linearize graph around the linearization point - gtsam::GaussianFactorGraph linearFactorGraph = *graph_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Keep increasing lambda until we make make progress while(true) { // Add prior factors at the current solution gttic(damp); - gtsam::GaussianFactorGraph dampedFactorGraph(linearFactorGraph); + GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution for(size_t j=0; j 0) { theta_.update(separatorValues_); - BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, separatorValues_) { - gtsam::Index index = ordering_.at(key_value.key); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + Index index = ordering_.at(key_value.key); delta_.at(index) = newDelta.at(index); } } // Decrease lambda for next time - lambda /= parameters_.lambdaFactor; - if(lambda < (0.5 / parameters_.lambdaUpperBound)) { - lambda = (0.5 / parameters_.lambdaUpperBound); + lambda /= lambdaFactor; + if(lambda < lambdaLowerBound) { + lambda = lambdaLowerBound; } // End this lambda search iteration break; } else { // Reject this change // Increase lambda and continue searching - lambda *= parameters_.lambdaFactor; - if(lambda > parameters_.lambdaUpperBound) { + lambda *= lambdaFactor; + if(lambda > lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; break; @@ -448,90 +522,23 @@ void ConcurrentBatchSmoother::optimize() { } gttoc(optimizer_iteration); - ++iterations; - } while(iterations < parameters_.maxIterations && - !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, - previousError, currentError, gtsam::NonlinearOptimizerParams::SILENT)); + result.iterations++; + } while(result.iterations < maxIterations && + !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); - if(debug) std::cout << "ConcurrentBatchSmoother::optimize() Finish" << std::endl; + return result; } /* ************************************************************************* */ -NonlinearFactor::shared_ptr ConcurrentBatchSmoother::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& keysToKeep, const Values& theta) const { - -factor->print("Factor Before:\n"); - - // Sort the keys for this factor - std::set factorKeys; - BOOST_FOREACH(Key key, *factor) { - factorKeys.insert(key); - } - - // Calculate the set of keys to marginalize - std::set marginalizeKeys; - std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); - std::set 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 variables; - BOOST_FOREACH(Key key, marginalizeKeys) { - variables.push_back(ordering.at(key)); - } -// std::pair result = graph.eliminate(variables); - GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size()); -result.first->print("Resulting Conditional:\n"); -result.second->print("Resulting Linear Factor:\n"); -// graph = result.second; - graph.replace(0, result.second); - - // (3) convert the marginal factors into Linearized Factors - NonlinearFactor::shared_ptr marginalFactor; - assert(graph.size() <= 1); - if(graph.size() > 0) { -graph.at(0)->print("Linear Factor After:\n"); - marginalFactor.reset(new LinearContainerFactor(graph.at(0), ordering, theta)); - } -marginalFactor->print("Factor After:\n"); - return marginalFactor; - } -} - -/* ************************************************************************* */ -void ConcurrentBatchSmoother::PrintNonlinearFactor(const gtsam::NonlinearFactor::shared_ptr& factor, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { +void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { - if(boost::dynamic_pointer_cast(factor)) { + if(boost::dynamic_pointer_cast(factor)) { std::cout << "l( "; } else { std::cout << "f( "; } - BOOST_FOREACH(gtsam::Key key, *factor) { + BOOST_FOREACH(Key key, *factor) { std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; @@ -541,11 +548,11 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const gtsam::NonlinearFactor: } /* ************************************************************************* */ -void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { +void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { std::cout << "g( "; - BOOST_FOREACH(gtsam::Index index, *factor) { + BOOST_FOREACH(Index index, *factor) { std::cout << keyFormatter(ordering.key(index)) << " "; } std::cout << ")" << std::endl; @@ -554,15 +561,119 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::sha } } + + + ///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintSingleClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { +//std::set ConcurrentBatchSmoother::findFactorsWithAny(const std::set& keys) const { +// // Find the set of factor slots for each specified key +// std::set 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 ConcurrentBatchSmoother::findFactorsWithOnly(const std::set& keys) const { +// // Find the set of factor slots with any of the provided keys +// std::set factorSlots = findFactorsWithAny(keys); +// // Test each factor for non-specified keys +// std::set::iterator slot = factorSlots.begin(); +// while(slot != factorSlots.end()) { +// const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); +// std::set 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& keysToKeep, const Values& theta) const { +// +//factor->print("Factor Before:\n"); +// +// // Sort the keys for this factor +// std::set factorKeys; +// BOOST_FOREACH(Key key, *factor) { +// factorKeys.insert(key); +// } +// +// // Calculate the set of keys to marginalize +// std::set marginalizeKeys; +// std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); +// std::set 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 variables; +// BOOST_FOREACH(Key key, marginalizeKeys) { +// variables.push_back(ordering.at(key)); +// } +//// std::pair result = graph.eliminate(variables); +// GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size()); +//result.first->print("Resulting Conditional:\n"); +//result.second->print("Resulting Linear Factor:\n"); +//// graph = result.second; +// graph.replace(0, result.second); +// +// // (3) convert the marginal factors into Linearized Factors +// NonlinearFactor::shared_ptr marginalFactor; +// assert(graph.size() <= 1); +// if(graph.size() > 0) { +//graph.at(0)->print("Linear Factor After:\n"); +// marginalFactor.reset(new LinearContainerFactor(graph.at(0), ordering, theta)); +// } +//marginalFactor->print("Factor After:\n"); +// return marginalFactor; +// } +//} + + + +///* ************************************************************************* */ +//void ConcurrentBatchSmoother::PrintSingleClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { // std::cout << indent << "P( "; -// BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()){ +// BOOST_FOREACH(Index index, clique->conditional()->frontals()){ // std::cout << keyFormatter(ordering.key(index)) << " "; // } // if(clique->conditional()->nrParents() > 0){ // std::cout << "| "; -// BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()){ +// BOOST_FOREACH(Index index, clique->conditional()->parents()){ // std::cout << keyFormatter(ordering.key(index)) << " "; // } // } @@ -570,19 +681,19 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const gtsam::GaussianFactor::sha //} // ///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintRecursiveClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { +//void ConcurrentBatchSmoother::PrintRecursiveClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { // // // Print this node // PrintSingleClique(clique, ordering, indent, keyFormatter); // // // Print Children -// BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { +// BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { // PrintRecursiveClique(child, ordering, indent+" ", keyFormatter); // } //} // ///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintBayesTree(const gtsam::ISAM2& bayesTree, const gtsam::Ordering& ordering, const std::string& indent, const gtsam::KeyFormatter& keyFormatter) { +//void ConcurrentBatchSmoother::PrintBayesTree(const ISAM2& bayesTree, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { // // std::cout << indent << "Bayes Tree:" << std::endl; // if (bayesTree.root().use_count() == 0) { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 93fd89d47..98674465c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -21,7 +21,6 @@ #include #include -#include #include namespace gtsam { @@ -32,19 +31,17 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother { public: - + typedef boost::shared_ptr shared_ptr; typedef ConcurrentSmoother Base; ///< typedef for base class - /** - * Meta information returned about the update - */ - // TODO: Think of some more things to put here + /** Meta information returned about the update */ struct Result { size_t iterations; ///< The number of optimizer iterations performed + size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point double error; ///< The final factor graph error - Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}; }; /** Default constructor */ @@ -53,14 +50,42 @@ public: /** Default destructor */ virtual ~ConcurrentBatchSmoother() {}; - // Implement a GTSAM standard 'print' function + /** Implement a GTSAM standard 'print' function */ void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** Check if two Concurrent Smoothers are equal */ + bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const; + + /** Access the current set of factors */ + const NonlinearFactorGraph& getFactors() const { + return factors_; + } + + /** Access the current linearization point */ + const Values& getLinearizationPoint() const { + return theta_; + } + + /** Access the current ordering */ + const Ordering& getOrdering() const { + return ordering_; + } + + /** Access the current set of deltas to the linearization point */ + const VectorValues& getDelta() const { + return delta_; + } + + /** Access the nonlinear variable index */ + const VariableIndex& getVariableIndex() const { + return variableIndex_; + } + /** Compute the current best estimate of all variables and return a full Values structure. * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_; + return getLinearizationPoint().retract(getDelta(), getOrdering()); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -69,8 +94,10 @@ public: * @return */ template - VALUE calculateEstimate(const Key key) const { - return theta_.at(key); + VALUE calculateEstimate(Key key) const { + const Index index = getOrdering().at(key); + const Vector delta = getDelta().at(index); + return getLinearizationPoint().at(key).retract(delta); } /** @@ -92,18 +119,17 @@ public: protected: - /** A typedef defining an Key-Factor mapping **/ - typedef std::map > FactorIndex; - LevenbergMarquardtParams parameters_; ///< LM parameters - NonlinearFactorGraph graph_; ///< The graph of all the smoother factors - Values theta_; ///< Current linearization point - Ordering ordering_; ///< The current ordering used to generate the deltas - VectorValues delta_; ///< Current set of offsets from the linearization point - Values separatorValues_; ///< The set of keys to be kept in the root and their linearization points + NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother + Values theta_; ///< Current linearization point of all variables in the smoother + Ordering ordering_; ///< The current ordering used to calculate the linear deltas + VectorValues delta_; ///< The current set of linear deltas from the linearization point + VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable std::queue 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 filterSummarizationSlots_; ///< The slots in graph for the last set of filter summarized factors + Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. + std::vector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors + + // Storage for information to be sent to the filter NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization /** @@ -130,7 +156,7 @@ protected: * @param rootValues The linearization point of the root variables */ virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, - const NonlinearFactorGraph& summarizedFactors, const Values& rootValues); + const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues); /** * Perform any required operations after the synchronization process finishes. @@ -138,48 +164,33 @@ protected: */ virtual void postsync(); - /** Augment the graph with a new factor + /** Augment the graph with new factors * - * @param factors The factor to add to the graph - * @return The slot in the graph it was inserted into + * @param factors The factors to add to the graph + * @return The slots in the graph where they were inserted */ - size_t insertFactor(const NonlinearFactor::shared_ptr& factor); + std::vector insertFactors(const NonlinearFactorGraph& factors); - /** Remove a factor from the graph by slot index */ - void removeFactor(size_t slot); + /** Remove factors from the graph by slot index + * + * @param slots The slots in the factor graph that should be deleted + * */ + void removeFactors(const std::vector& slots); - /** Optimize the graph using a modified version of L-M */ - void optimize(); + /** Use colamd to update into an efficient ordering */ + void reorder(); - /** Find all of the nonlinear factors that contain any of the provided keys */ - std::set findFactorsWithAny(const std::set& keys) const; - - /** Find all of the nonlinear factors that contain only the provided keys */ - std::set findFactorsWithOnly(const std::set& 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& keysToKeep, const Values& theta) const; + /** Use a modified version of L-M to update the linearization point and delta */ + Result optimize(); private: /** Some printing functions for debugging */ - static void PrintNonlinearFactor(const gtsam::NonlinearFactor::shared_ptr& factor, - const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); + static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, + const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); - static void PrintLinearFactor(const gtsam::GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering, - const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); - -// static void PrintSingleClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, -// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); -// -// static void PrintRecursiveClique(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, -// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); -// -// static void PrintBayesTree(const gtsam::ISAM2& bayesTree, const gtsam::Ordering& ordering, -// const std::string& indent = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); - -// typedef BayesTree::sharedClique Clique; -// static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = ""); + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchSmoother diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 0d23d8fc9..93f8b06be 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { @@ -35,6 +36,14 @@ void GTSAM_UNSTABLE_EXPORT synchronize(ConcurrentFilter& filter, ConcurrentSmoot * The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture. */ class GTSAM_UNSTABLE_EXPORT ConcurrentFilter { +public: + typedef boost::shared_ptr shared_ptr; + + /** Implement a standard 'print' function */ + virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; + + /** Check if two Concurrent Smoothers are equal */ + virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0; protected: @@ -89,7 +98,15 @@ protected: /** * The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture. */ -class ConcurrentSmoother { +class GTSAM_UNSTABLE_EXPORT ConcurrentSmoother { +public: + typedef boost::shared_ptr shared_ptr; + + /** Implement a standard 'print' function */ + virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; + + /** Check if two Concurrent Smoothers are equal */ + virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0; protected: