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

Updated BatchFixedLagSmoother to use the latest version of optimization and marginalization code

release/4.3a0
Stephen Williams 2013-04-25 18:10:21 +00:00
parent fe07dee964
commit 1e1dfdd808
3 changed files with 419 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

530
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -18,7 +18,8 @@
*/ */
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/LinearizedFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
@ -50,11 +51,27 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
} }
// Add the new factors // Add the new factors
updateFactors(newFactors); insertFactors(newFactors);
// Add the new variables // Add the new variables
theta_.insert(newTheta); theta_.insert(newTheta);
// Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
ordering_.push_back(key_value.key);
}
// Augment Delta
std::vector<size_t> dims;
dims.reserve(newTheta.size());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
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 // Update the Timestamps associated with the factor keys
updateKeyTimestampMap(timestamps); updateKeyTimestampMap(timestamps);
@ -72,48 +89,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
std::cout << std::endl; std::cout << std::endl;
} }
// Marginalize out these variables. // Reorder
// This removes any factors that touch marginalized variables and adds new linear(ized) factors to the graph reorder(marginalizableKeys);
marginalizeKeys(marginalizableKeys);
// Create the optimizer // Optimize
Values linpoint;
linpoint.insert(theta_);
if(enforceConsistency_ && linearizedKeys_.size() > 0) {
linpoint.update(linearizedKeys_);
}
LevenbergMarquardtOptimizer optimizer(factors_, linpoint, parameters_);
// Use a custom optimization loop so the linearization points can be controlled
double currentError;
do {
// Do next iteration
currentError = optimizer.error();
optimizer.iterate();
// Force variables associated with linearized factors to keep the same linearization point
if(enforceConsistency_ && linearizedKeys_.size() > 0) {
// Put the old values of the linearized keys back into the optimizer state
optimizer.state().values.update(linearizedKeys_);
optimizer.state().error = factors_.error(optimizer.state().values);
}
// Maybe show output
if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues");
if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl;
} while(optimizer.iterations() < parameters_.maxIterations &&
!checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol,
parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity));
// Update the Values from the optimizer
theta_ = optimizer.values();
// Create result structure
Result result; Result result;
result.iterations = optimizer.state().iterations; if(theta_.size() > 0) {
result.linearVariables = linearizedKeys_.size(); result = optimize();
result.nonlinearVariables = theta_.size() - linearizedKeys_.size(); }
result.error = optimizer.state().error;
// Marginalize out old variables.
if(marginalizableKeys.size() > 0) {
marginalize(marginalizableKeys);
}
if(debug) { if(debug) {
std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; 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) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
Index index; Index index;
// Insert the factor into an existing hole in the factor graph, if possible // Insert the factor into an existing hole in the factor graph, if possible
@ -172,191 +160,233 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
factorIndex_.erase(key); factorIndex_.erase(key);
// Erase the key from the set of linearized keys // Erase the key from the set of linearized keys
if(linearizedKeys_.exists(key)) { if(linearKeys_.exists(key)) {
linearizedKeys_.erase(key); linearKeys_.erase(key);
} }
} }
eraseKeyTimestampMap(keys); 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<Index> 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 { void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
std::set<Index> factors;
std::set<Key> keys;
FactorTree(const std::set<Index>& factors, const NonlinearFactorGraph& allFactors) : factors(factors) { // Calculate a variable index
BOOST_FOREACH(const Index& factor, factors) { VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size());
BOOST_FOREACH(Key key, *(allFactors.at(factor))) {
keys.insert(key); // 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<int> 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) { // Generate the permutation
const NonlinearFactor::shared_ptr& f = allFactors.at(factor); Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex, cmember);
std::set<Key>::const_iterator iter = std::find_first_of(keys.begin(), keys.end(), f->begin(), f->end());
return iter != keys.end();
}
template <class ForwardIterator> // Permute the ordering, variable index, and deltas
bool hasCommonKeys(ForwardIterator first, ForwardIterator last, const NonlinearFactorGraph& allFactors) { ordering_.permuteInPlace(forwardPermutation);
for(ForwardIterator factor = first; factor != last; ++factor) { delta_.permuteInPlace(forwardPermutation);
if(hasCommonKeys(*factor, allFactors)) }
return true;
}
return false;
}
};
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::marginalizeKeys(const std::set<Key>& 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"); // Set optimization parameters
if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() START" << std::endl; 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<delta_.size(); ++j) {
size_t dim = delta_[j].size();
Matrix A = eye(dim);
Vector b = delta_[j];
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
}
gttoc(damp);
result.intermediateSteps++;
std::cout << "Trying Lambda = " << lambda << std::endl;
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta, ordering_);
gttoc(solve);
std::cout << " Max Delta = " << newDelta.asVector().maxCoeff() << std::endl;
// Evaluate the new error
gttic(compute_error);
double error = factors_.error(evalpoint);
gttoc(compute_error);
std::cout << " New Error = " << error << std::endl;
std::cout << " Change = " << result.error - error << std::endl;
if(error < result.error) {
std::cout << " Keeping Change" << std::endl;
// Keep this change
// Update the error value
result.error = error;
// Update the linearization point
theta_ = evalpoint;
// Reset the deltas to zeros
delta_.setZero();
// Put the linearization points and deltas back for specific variables
if(enforceConsistency_ && (linearKeys_.size() > 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<Key>& marginalizeKeys) {
// In order to marginalize out the selected variables, the factors involved in those variables // 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 // 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(ized) factors from // remaining variables needs to be accounted for. This will be done with linear container factors
// a partial clique marginalization (or from the iSAM2 cached factor if the entire clique is removed). // from the result of a partial elimination. This function removes the marginalized factors and
// This function finds the set of factors to be removed and generates the linearized factors that // adds the linearized factors back in.
// must be added.
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. // Use the variable Index to mark the factors that will be marginalized
typedef std::list<FactorTree> FactorForest; std::set<size_t> removedFactorSlots;
FactorForest factorForest; BOOST_FOREACH(Key key, marginalizeKeys) {
BOOST_FOREACH(Key key, marginalizableKeys) { const FastList<size_t>& slots = variableIndex[ordering_.at(key)];
removedFactorSlots.insert(slots.begin(), slots.end());
if(debug) std::cout << "Looking for factors involving key " << DefaultKeyFormatter(key) << std::endl;
// Get the factors associated with this variable
const std::set<size_t>& factors = factorIndex_.at(key);
if(debug) { std::cout << "Found the following factors:" << std::endl; BOOST_FOREACH(size_t i, factors) { std::cout << " "; PrintSymbolicFactor(factors_.at(i)); } }
// Loop over existing factor trees, looking for common keys
std::vector<FactorForest::iterator> commonTrees;
for(FactorForest::iterator tree = factorForest.begin(); tree != factorForest.end(); ++tree) {
if(tree->hasCommonKeys(factors.begin(), factors.end(), factors_)) {
commonTrees.push_back(tree);
}
}
if(debug) std::cout << "Found " << commonTrees.size() << " common trees." << std::endl;
if(commonTrees.size() == 0) {
// No common trees were found. Create a new one.
factorForest.push_back(FactorTree(factors, factors_));
if(debug) std::cout << "Created a new tree." << std::endl;
} else {
// Extract the last common tree
FactorForest::iterator commonTree = commonTrees.back();
commonTrees.pop_back();
// Merge the current factors into this tree
commonTree->push_back(FactorTree(factors, factors_));
// Merge all other common trees into this one, deleting the other trees from the forest.
BOOST_FOREACH(FactorForest::iterator& tree, commonTrees) {
commonTree->push_back(*tree);
factorForest.erase(tree);
}
}
}
if(debug) std::cout << "Found " << factorForest.size() << " factor trees in the set of removed factors." << std::endl;
// For each tree in the forest:
// (0) construct an ordering for the tree
// (1) construct a linear factor graph
// (2) solve for the marginal factors
// (3) convert the marginal factors into Linearized Factors
// (4) remove the marginalized factors from the graph
// (5) add the factors in this tree to the graph
BOOST_FOREACH(const FactorTree& factorTree, factorForest) {
// (0) construct an ordering for this tree
// The ordering should place the marginalizable keys first, then the remaining keys
Ordering ordering;
std::set<Key> marginalizableTreeKeys;
std::set_intersection(factorTree.keys.begin(), factorTree.keys.end(),
marginalizableKeys.begin(), marginalizableKeys.end(),
std::inserter(marginalizableTreeKeys, marginalizableTreeKeys.end()));
std::set<Key> remainingTreeKeys;
std::set_difference(factorTree.keys.begin(), factorTree.keys.end(),
marginalizableTreeKeys.begin(), marginalizableTreeKeys.end(),
std::inserter(remainingTreeKeys, remainingTreeKeys.end()));
// TODO: It may be worthwhile to use CCOLAMD here. (but maybe not???)
BOOST_FOREACH(Key key, marginalizableTreeKeys) {
ordering.push_back(key);
}
BOOST_FOREACH(Key key, remainingTreeKeys) {
ordering.push_back(key);
}
// (1) construct a linear factor graph
GaussianFactorGraph graph;
BOOST_FOREACH(size_t factor, factorTree.factors) {
graph.push_back( factors_.at(factor)->linearize(theta_, ordering) );
}
if(debug) PrintSymbolicGraph(graph, ordering, "Factor Tree:");
// (2) solve for the marginal factors
// Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables)
// and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2
std::vector<Index> variables;
BOOST_FOREACH(Key key, marginalizableTreeKeys) {
variables.push_back(ordering.at(key));
}
std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph> result = graph.eliminate(variables);
graph = result.second;
if(debug) PrintSymbolicGraph(graph, ordering, "Factors on Remaining Variables:");
// (3) convert the marginal factors into Linearized Factors
NonlinearFactorGraph newFactors;
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, graph) {
// These factors are all generated from BayesNet conditionals. They should all be Jacobians.
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(gaussianFactor);
assert(jacobianFactor);
LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, 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<Key> 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);
} }
if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl; // Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
// This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically
// Find the subset of nodes/keys that must be eliminated
std::set<Index> indicesToEliminate;
BOOST_FOREACH(Key key, marginalizeKeys) {
indicesToEliminate.insert(ordering_.at(key));
}
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<Index> 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<Index>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<Index> parents(n, none);
std::vector<Index> 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::shared_ptr> BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
// Number of variables
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
std::vector<shared_ptr> 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<Index>& 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 } /// namespace gtsam

98
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -55,7 +55,7 @@ public:
* a single variable is needed, it is faster to call calculateEstimate(const KEY&). * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/ */
Values calculateEstimate() const { Values calculateEstimate() const {
return theta_; return theta_.retract(delta_, ordering_);
} }
/** Compute an estimate for a single variable using its incomplete linear delta computed /** Compute an estimate for a single variable using its incomplete linear delta computed
@ -66,7 +66,9 @@ public:
*/ */
template<class VALUE> template<class VALUE>
VALUE calculateEstimate(Key key) const { VALUE calculateEstimate(Key key) const {
return theta_.at<VALUE>(key); const Index index = ordering_.at(key);
const Vector delta = delta_.at(index);
return theta_.at<VALUE>(key).retract(delta);
} }
/** read the current set of optimizer parameters */ /** read the current set of optimizer parameters */
@ -79,6 +81,26 @@ public:
return parameters_; 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: protected:
/** A typedef defining an Key-Factor mapping **/ /** A typedef defining an Key-Factor mapping **/
@ -98,8 +120,14 @@ protected:
/** The current linearization point **/ /** The current linearization point **/
Values theta_; Values theta_;
/** The set of keys involved in current linearized factors. These keys should not be relinearized. **/ /** The set of keys involved in current linear factors. These keys should not be relinearized. **/
Values linearizedKeys_; 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. **/ /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
std::queue<size_t> availableSlots_; std::queue<size_t> availableSlots_;
@ -110,7 +138,7 @@ protected:
/** Augment the list of factors with a set of new factors */ /** 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 */ /** Remove factors from the list of factors by slot index */
void removeFactors(const std::set<size_t>& deleteFactors); void removeFactors(const std::set<size_t>& deleteFactors);
@ -118,9 +146,65 @@ protected:
/** Erase any keys associated with timestamps before the provided time */ /** Erase any keys associated with timestamps before the provided time */
void eraseKeys(const std::set<Key>& keys); void eraseKeys(const std::set<Key>& keys);
/** Marginalize out selected variables */ /** Use colamd to update into an efficient ordering */
void marginalizeKeys(const std::set<Key>& marginalizableKeys); void reorder(const std::set<Key>& marginalizeKeys = std::set<Key>());
/** Optimize the current graph using a modified version of L-M */
Result optimize();
/** Marginalize out selected variables */
void marginalize(const std::set<Key>& marginalizableKeys);
// A custom elimination tree that supports forests and partial elimination
class EliminationForest {
public:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditional::shared_ptr> 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<Index> 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<shared_ptr> 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<Index>& indices, const EliminationForest::shared_ptr& tree);
};
private: private:
/** Private methods for printing debug information */ /** Private methods for printing debug information */

4
gtsam_unstable/nonlinear/FixedLagSmoother.h
View File

@ -45,13 +45,15 @@ public:
// TODO: Think of some more things to put here // TODO: Think of some more things to put here
struct Result { struct Result {
size_t iterations; ///< The number of optimizer iterations performed 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 nonlinearVariables; ///< The number of variables that can be relinearized
size_t linearVariables; ///< The number of variables that must keep a constant linearization point size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error 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 /// Getter methods
size_t getIterations() const { return iterations; } size_t getIterations() const { return iterations; }
size_t getIntermediateSteps() const { return intermediateSteps; }
size_t getNonlinearVariables() const { return nonlinearVariables; } size_t getNonlinearVariables() const { return nonlinearVariables; }
size_t getLinearVariables() const { return linearVariables; } size_t getLinearVariables() const { return linearVariables; }
double getError() const { return error; } double getError() const { return error; }