diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index cbf884ff6..7e8c4cddf 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -18,7 +18,8 @@ */ #include -#include +#include +#include #include #include #include @@ -50,11 +51,27 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } // Add the new factors - updateFactors(newFactors); + insertFactors(newFactors); // Add the new variables theta_.insert(newTheta); + // Add new variables to the end of the ordering + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + ordering_.push_back(key_value.key); + } + + // Augment Delta + std::vector dims; + dims.reserve(newTheta.size()); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + dims.push_back(key_value.value.dim()); + } + delta_.append(dims); + for(size_t i = delta_.size() - dims.size(); i < delta_.size(); ++i) { + delta_[i].setZero(); + } + // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -72,48 +89,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap std::cout << std::endl; } - // Marginalize out these variables. - // This removes any factors that touch marginalized variables and adds new linear(ized) factors to the graph - marginalizeKeys(marginalizableKeys); + // Reorder + reorder(marginalizableKeys); - // Create the optimizer - Values linpoint; - linpoint.insert(theta_); - if(enforceConsistency_ && linearizedKeys_.size() > 0) { - linpoint.update(linearizedKeys_); - } - LevenbergMarquardtOptimizer optimizer(factors_, linpoint, parameters_); - - // Use a custom optimization loop so the linearization points can be controlled - double currentError; - do { - // Do next iteration - currentError = optimizer.error(); - optimizer.iterate(); - - // Force variables associated with linearized factors to keep the same linearization point - if(enforceConsistency_ && linearizedKeys_.size() > 0) { - // Put the old values of the linearized keys back into the optimizer state - optimizer.state().values.update(linearizedKeys_); - optimizer.state().error = factors_.error(optimizer.state().values); - } - - // Maybe show output - if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues"); - if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl; - } while(optimizer.iterations() < parameters_.maxIterations && - !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, - parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity)); - - // Update the Values from the optimizer - theta_ = optimizer.values(); - - // Create result structure + // Optimize Result result; - result.iterations = optimizer.state().iterations; - result.linearVariables = linearizedKeys_.size(); - result.nonlinearVariables = theta_.size() - linearizedKeys_.size(); - result.error = optimizer.state().error; + if(theta_.size() > 0) { + result = optimize(); + } + + // Marginalize out old variables. + if(marginalizableKeys.size() > 0) { + marginalize(marginalizableKeys); + } if(debug) { std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; @@ -123,7 +111,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } /* ************************************************************************* */ -void BatchFixedLagSmoother::updateFactors(const NonlinearFactorGraph& newFactors) { +void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { Index index; // Insert the factor into an existing hole in the factor graph, if possible @@ -172,191 +160,233 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if(linearizedKeys_.exists(key)) { - linearizedKeys_.erase(key); + if(linearKeys_.exists(key)) { + linearKeys_.erase(key); } } eraseKeyTimestampMap(keys); + + // Permute the ordering such that the removed keys are at the end. + // This is a prerequisite for removing them from several structures + std::vector toBack; + BOOST_FOREACH(Key key, keys) { + toBack.push_back(ordering_.at(key)); + } + Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); + ordering_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); + + // Remove marginalized keys from the ordering and delta + for(size_t i = 0; i < keys.size(); ++i) { + ordering_.pop_back(); + delta_.pop_back(); + } + } /* ************************************************************************* */ -struct FactorTree { - std::set factors; - std::set keys; +void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { - FactorTree(const std::set& factors, const NonlinearFactorGraph& allFactors) : factors(factors) { - BOOST_FOREACH(const Index& factor, factors) { - BOOST_FOREACH(Key key, *(allFactors.at(factor))) { - keys.insert(key); - } + // Calculate a variable index + VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size()); + + // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 + int group0 = 0; + int group1 = marginalizeKeys.size() > 0 ? 1 : 0; + + // Initialize all variables to group1 + std::vector cmember(variableIndex.size(), group1); + + // Set all of the marginalizeKeys to Group0 + if(marginalizeKeys.size() > 0) { + BOOST_FOREACH(Key key, marginalizeKeys) { + cmember[ordering_.at(key)] = group0; } - }; - - void push_back(const FactorTree& factorTree) { - factors.insert(factorTree.factors.begin(), factorTree.factors.end()); - keys.insert(factorTree.keys.begin(), factorTree.keys.end()); } - bool hasCommonKeys(Index factor, const NonlinearFactorGraph& allFactors) { - const NonlinearFactor::shared_ptr& f = allFactors.at(factor); - std::set::const_iterator iter = std::find_first_of(keys.begin(), keys.end(), f->begin(), f->end()); - return iter != keys.end(); - } + // Generate the permutation + Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex, cmember); - template - bool hasCommonKeys(ForwardIterator first, ForwardIterator last, const NonlinearFactorGraph& allFactors) { - for(ForwardIterator factor = first; factor != last; ++factor) { - if(hasCommonKeys(*factor, allFactors)) - return true; - } - return false; - } -}; + // Permute the ordering, variable index, and deltas + ordering_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); +} /* ************************************************************************* */ -void BatchFixedLagSmoother::marginalizeKeys(const std::set& marginalizableKeys) { +FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { + // Create output result structure + Result result; + result.nonlinearVariables = theta_.size() - linearKeys_.size(); + result.linearVariables = linearKeys_.size(); - const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() START" << std::endl; + // 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); +std::cout << "Initial Error = " << result.error << std::endl; + // 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 + double sigma = 1.0 / std::sqrt(lambda); + for(size_t j=0; j 0)) { + theta_.update(linearKeys_); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { + 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 { +std::cout << " Rejecting Change" << std::endl; + // 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)); +std::cout << "Final Error = " << result.error << std::endl; + return result; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // In order to marginalize out the selected variables, the factors involved in those variables - // must be identified and removed from iSAM2. Also, the effect of those removed factors on the - // remaining variables needs to be accounted for. This will be done with linear(ized) factors from - // a partial clique marginalization (or from the iSAM2 cached factor if the entire clique is removed). - // This function finds the set of factors to be removed and generates the linearized factors that - // must be added. + // must be identified and removed. Also, the effect of those removed factors on the + // remaining variables needs to be accounted for. This will be done with linear container factors + // from the result of a partial elimination. This function removes the marginalized factors and + // adds the linearized factors back in. - if(marginalizableKeys.size() > 0) { + // Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys') + // Note: It is assumed the ordering already has these keys first + // Create the linear factor graph + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); - if(debug) PrintKeySet(marginalizableKeys, "Marginalizable Keys:"); + // Create a variable index + VariableIndex variableIndex(linearFactorGraph, ordering_.size()); - // Find all of the factors associated with marginalizable variables. This set of factors may form a forest. - typedef std::list FactorForest; - FactorForest factorForest; - BOOST_FOREACH(Key key, marginalizableKeys) { - - if(debug) std::cout << "Looking for factors involving key " << DefaultKeyFormatter(key) << std::endl; - - // Get the factors associated with this variable - const std::set& factors = factorIndex_.at(key); - - if(debug) { std::cout << "Found the following factors:" << std::endl; BOOST_FOREACH(size_t i, factors) { std::cout << " "; PrintSymbolicFactor(factors_.at(i)); } } - - // Loop over existing factor trees, looking for common keys - std::vector commonTrees; - for(FactorForest::iterator tree = factorForest.begin(); tree != factorForest.end(); ++tree) { - if(tree->hasCommonKeys(factors.begin(), factors.end(), factors_)) { - commonTrees.push_back(tree); - } - } - - if(debug) std::cout << "Found " << commonTrees.size() << " common trees." << std::endl; - - if(commonTrees.size() == 0) { - // No common trees were found. Create a new one. - factorForest.push_back(FactorTree(factors, factors_)); - - if(debug) std::cout << "Created a new tree." << std::endl; - - } else { - // Extract the last common tree - FactorForest::iterator commonTree = commonTrees.back(); - commonTrees.pop_back(); - // Merge the current factors into this tree - commonTree->push_back(FactorTree(factors, factors_)); - // Merge all other common trees into this one, deleting the other trees from the forest. - BOOST_FOREACH(FactorForest::iterator& tree, commonTrees) { - commonTree->push_back(*tree); - factorForest.erase(tree); - } - } - } - - if(debug) std::cout << "Found " << factorForest.size() << " factor trees in the set of removed factors." << std::endl; - - // For each tree in the forest: - // (0) construct an ordering for the tree - // (1) construct a linear factor graph - // (2) solve for the marginal factors - // (3) convert the marginal factors into Linearized Factors - // (4) remove the marginalized factors from the graph - // (5) add the factors in this tree to the graph - BOOST_FOREACH(const FactorTree& factorTree, factorForest) { - // (0) construct an ordering for this tree - // The ordering should place the marginalizable keys first, then the remaining keys - Ordering ordering; - - std::set marginalizableTreeKeys; - std::set_intersection(factorTree.keys.begin(), factorTree.keys.end(), - marginalizableKeys.begin(), marginalizableKeys.end(), - std::inserter(marginalizableTreeKeys, marginalizableTreeKeys.end())); - - std::set remainingTreeKeys; - std::set_difference(factorTree.keys.begin(), factorTree.keys.end(), - marginalizableTreeKeys.begin(), marginalizableTreeKeys.end(), - std::inserter(remainingTreeKeys, remainingTreeKeys.end())); - - // TODO: It may be worthwhile to use CCOLAMD here. (but maybe not???) - BOOST_FOREACH(Key key, marginalizableTreeKeys) { - ordering.push_back(key); - } - BOOST_FOREACH(Key key, remainingTreeKeys) { - ordering.push_back(key); - } - - // (1) construct a linear factor graph - GaussianFactorGraph graph; - BOOST_FOREACH(size_t factor, factorTree.factors) { - graph.push_back( factors_.at(factor)->linearize(theta_, ordering) ); - } - - if(debug) PrintSymbolicGraph(graph, ordering, "Factor Tree:"); - - // (2) solve for the marginal factors - // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables) - // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2 - std::vector variables; - BOOST_FOREACH(Key key, marginalizableTreeKeys) { - variables.push_back(ordering.at(key)); - } - std::pair result = graph.eliminate(variables); - graph = result.second; - - if(debug) PrintSymbolicGraph(graph, ordering, "Factors on Remaining Variables:"); - - // (3) convert the marginal factors into Linearized Factors - NonlinearFactorGraph newFactors; - BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, graph) { - // These factors are all generated from BayesNet conditionals. They should all be Jacobians. - JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(gaussianFactor); - assert(jacobianFactor); - LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, theta_)); - // add it to the new factor set - newFactors.push_back(factor); - } - - // (4) remove the marginalized factors from the graph - removeFactors(factorTree.factors); - - // (5) add the factors in this tree to the main set of factors - updateFactors(newFactors); - - // (6) add the keys involved in the linear(ized) factors to the linearizedKey list - FastSet linearizedKeys = newFactors.keys(); - BOOST_FOREACH(Key key, linearizedKeys) { - if(!linearizedKeys_.exists(key)) { - linearizedKeys_.insert(key, theta_.at(key)); - } - } - } - - // Remove the marginalized keys from the smoother data structures - eraseKeys(marginalizableKeys); + // Use the variable Index to mark the factors that will be marginalized + std::set removedFactorSlots; + BOOST_FOREACH(Key key, marginalizeKeys) { + const FastList& slots = variableIndex[ordering_.at(key)]; + removedFactorSlots.insert(slots.begin(), slots.end()); } - if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl; + // Construct an elimination tree to perform sparse elimination + std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); + + // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically + // Find the subset of nodes/keys that must be eliminated + std::set indicesToEliminate; + BOOST_FOREACH(Key key, marginalizeKeys) { + indicesToEliminate.insert(ordering_.at(key)); + } + BOOST_FOREACH(Key key, marginalizeKeys) { + EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); + } + + // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables + // Convert the marginal factors into Linear Container Factors + // Add the marginal factor variables to the separator + NonlinearFactorGraph marginalFactors; + BOOST_FOREACH(Index index, indicesToEliminate) { + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + if(gaussianFactor->size() > 0) { + LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); + marginalFactors.push_back(marginalFactor); + // Add the keys associated with the marginal factor to the separator values + BOOST_FOREACH(Key key, *marginalFactor) { + if(!linearKeys_.exists(key)) { + linearKeys_.insert(key, theta_.at(key)); + } + } + } + } + insertFactors(marginalFactors); + + // Remove the marginalized variables and factors from the filter + // Remove marginalized factors from the factor graph + removeFactors(removedFactorSlots); + + // Remove marginalized keys from the system + eraseKeys(marginalizeKeys); } /* ************************************************************************* */ @@ -406,5 +436,95 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, } } +/* ************************************************************************* */ +std::vector BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) { + // Number of factors and variables + const size_t m = structure.nFactors(); + const size_t n = structure.size(); + + static const Index none = std::numeric_limits::max(); + + // Allocate result parent vector and vector of last factor columns + std::vector parents(n, none); + std::vector prevCol(m, none); + + // for column j \in 1 to n do + for (Index j = 0; j < n; j++) { + // for row i \in Struct[A*j] do + BOOST_FOREACH(const size_t i, structure[j]) { + if (prevCol[i] != none) { + Index k = prevCol[i]; + // find root r of the current tree that contains k + Index r = k; + while (parents[r] != none) + r = parents[r]; + if (r != j) parents[r] = j; + } + prevCol[i] = j; + } + } + + return parents; +} + +/* ************************************************************************* */ +std::vector BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { + // Compute the tree structure + std::vector parents(ComputeParents(structure)); + + // Number of variables + const size_t n = structure.size(); + + static const Index none = std::numeric_limits::max(); + + // Create tree structure + std::vector trees(n); + for (Index k = 1; k <= n; k++) { + Index j = n - k; // Start at the last variable and loop down to 0 + trees[j].reset(new EliminationForest(j)); // Create a new node on this variable + if (parents[j] != none) // If this node has a parent, add it to the parent's children + trees[parents[j]]->add(trees[j]); + } + + // Hang factors in right places + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { + if(factor && factor->size() > 0) { + Index j = *std::min_element(factor->begin(), factor->end()); + if(j < structure.size()) + trees[j]->add(factor); + } + } + + return trees; +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { + + // Create the list of factors to be eliminated, initially empty, and reserve space + GaussianFactorGraph factors; + factors.reserve(this->factors_.size() + this->subTrees_.size()); + + // Add all factors associated with the current node + factors.push_back(this->factors_.begin(), this->factors_.end()); + + // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] + BOOST_FOREACH(const shared_ptr& child, subTrees_) + factors.push_back(child->eliminateRecursive(function)); + + // Combine all factors (from this node and from subtrees) into a joint factor + GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); + + return eliminated.second; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::EliminationForest::removeChildrenIndices(std::set& indices, const BatchFixedLagSmoother::EliminationForest::shared_ptr& tree) { + BOOST_FOREACH(const EliminationForest::shared_ptr& child, tree->children()) { + indices.erase(child->key()); + removeChildrenIndices(indices, child); + } +} + /* ************************************************************************* */ } /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 7f3e89803..dd22939bf 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -55,7 +55,7 @@ public: * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_; + return theta_.retract(delta_, ordering_); } /** Compute an estimate for a single variable using its incomplete linear delta computed @@ -66,7 +66,9 @@ public: */ template VALUE calculateEstimate(Key key) const { - return theta_.at(key); + const Index index = ordering_.at(key); + const Vector delta = delta_.at(index); + return theta_.at(key).retract(delta); } /** read the current set of optimizer parameters */ @@ -79,6 +81,26 @@ public: return parameters_; } + /** 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_; + } + protected: /** A typedef defining an Key-Factor mapping **/ @@ -98,8 +120,14 @@ protected: /** The current linearization point **/ Values theta_; - /** The set of keys involved in current linearized factors. These keys should not be relinearized. **/ - Values linearizedKeys_; + /** The set of keys involved in current linear factors. These keys should not be relinearized. **/ + Values linearKeys_; + + /** The current ordering */ + Ordering ordering_; + + /** The current set of linear deltas */ + VectorValues delta_; /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/ std::queue availableSlots_; @@ -110,7 +138,7 @@ protected: /** Augment the list of factors with a set of new factors */ - void updateFactors(const NonlinearFactorGraph& newFactors); + void insertFactors(const NonlinearFactorGraph& newFactors); /** Remove factors from the list of factors by slot index */ void removeFactors(const std::set& deleteFactors); @@ -118,9 +146,65 @@ protected: /** Erase any keys associated with timestamps before the provided time */ void eraseKeys(const std::set& keys); - /** Marginalize out selected variables */ - void marginalizeKeys(const std::set& marginalizableKeys); + /** Use colamd to update into an efficient ordering */ + void reorder(const std::set& marginalizeKeys = std::set()); + /** Optimize the current graph using a modified version of L-M */ + Result optimize(); + + /** Marginalize out selected variables */ + void marginalize(const std::set& marginalizableKeys); + + + // A custom elimination tree that supports forests and partial elimination + class EliminationForest { + public: + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + private: + typedef FastList Factors; + typedef FastList SubTrees; + typedef std::vector Conditionals; + + Index key_; ///< index associated with root + Factors factors_; ///< factors associated with root + SubTrees subTrees_; ///< sub-trees + + /** default constructor, private, as you should use Create below */ + EliminationForest(Index key = 0) : key_(key) {} + + /** + * Static internal function to build a vector of parent pointers using the + * algorithm of Gilbert et al., 2001, BIT. + */ + static std::vector ComputeParents(const VariableIndex& structure); + + /** add a factor, for Create use only */ + void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } + + /** add a subtree, for Create use only */ + void add(const shared_ptr& child) { subTrees_.push_back(child); } + + public: + + /** return the key associated with this tree node */ + Index key() const { return key_; } + + /** return the const reference of children */ + const SubTrees& children() const { return subTrees_; } + + /** return the const reference to the factors */ + const Factors& factors() const { return factors_; } + + /** Create an elimination tree from a factor graph */ + static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); + + /** Recursive routine that eliminates the factors arranged in an elimination tree */ + GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); + + /** Recursive function that helps find the top of each tree */ + static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); + }; private: /** Private methods for printing debug information */ diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 4af10b667..89dba73ca 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -45,13 +45,15 @@ public: // TODO: Think of some more things to put here struct Result { size_t iterations; ///< The number of optimizer iterations performed + size_t intermediateSteps; ///< The number of intermediate steps performed within the optimization. For L-M, this is the number of lambdas tried. 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), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}; /// Getter methods size_t getIterations() const { return iterations; } + size_t getIntermediateSteps() const { return intermediateSteps; } size_t getNonlinearVariables() const { return nonlinearVariables; } size_t getLinearVariables() const { return linearVariables; } double getError() const { return error; }