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

Removed VariableIndex from the class members as it was often left in an inconsistent state

release/4.3a0
Stephen Williams 2013-04-25 18:12:05 +00:00
parent 1e1dfdd808
commit 009012005e
2 changed files with 96 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -81,7 +81,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
// Optimize the factors using a modified version of L-M // Optimize the factors using a modified version of L-M
gttic(optimize); gttic(optimize);
if(factors_.size() > 0) { if(factors_.size() > 0) {
result = optimize(); result = optimize(factors_, theta_, ordering_, delta_, separatorValues_, parameters_);
} }
gttoc(optimize); gttoc(optimize);
@ -124,8 +124,23 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
values.insert(smootherValues_); values.insert(smootherValues_);
values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother
// Perform an optional optimization on the to-be-sent-to-the-smoother factors
if(true) {
// Create ordering and delta
Ordering ordering = *graph.orderingCOLAMD(values);
VectorValues delta = values.zeroVectors(ordering);
// Optimize this graph using a modified version of L-M // Optimize this graph using a modified version of L-M
// TODO: optimize(graph, values, ordering, delta, separatorValues, parameters_);
// Update filter theta and delta
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) {
theta_.update(key_value.key, values.at(key_value.key));
delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key));
}
// Update the fixed linearization points (since they just changed)
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorValues_.update(key_value.key, values.at(key_value.key));
}
}
// Create separate ordering constraints that force either the filter keys or the smoother keys to the front // Create separate ordering constraints that force either the filter keys or the smoother keys to the front
typedef std::map<Key, int> OrderingConstraints; typedef std::map<Key, int> OrderingConstraints;
@ -278,15 +293,15 @@ void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
/* ************************************************************************* */ /* ************************************************************************* */
void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysToMove) { void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysToMove) {
// Recalculate the variable index // Calculate the variable index
variableIndex_ = VariableIndex(*factors_.symbolic(ordering_)); 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 // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
int group0 = 0; int group0 = 0;
int group1 = keysToMove ? 1 : 0; int group1 = (keysToMove && (keysToMove->size() > 0) ) ? 1 : 0;
// Initialize all variables to group1 // Initialize all variables to group1
std::vector<int> cmember(variableIndex_.size(), group1); std::vector<int> cmember(variableIndex.size(), group1);
// Set all of the keysToMove to Group0 // Set all of the keysToMove to Group0
if(keysToMove && keysToMove->size() > 0) { if(keysToMove && keysToMove->size() > 0) {
@ -296,34 +311,35 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
} }
// Generate the permutation // Generate the permutation
Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember); Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex, cmember);
// Permute the ordering, variable index, and deltas // Permute the ordering, variable index, and deltas
ordering_.permuteInPlace(forwardPermutation); ordering_.permuteInPlace(forwardPermutation);
variableIndex_.permuteInPlace(forwardPermutation);
delta_.permuteInPlace(forwardPermutation); delta_.permuteInPlace(forwardPermutation);
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() { ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) {
// Create output result structure // Create output result structure
Result result; Result result;
result.nonlinearVariables = theta_.size() - separatorValues_.size(); result.nonlinearVariables = theta.size() - linearValues.size();
result.linearVariables = separatorValues_.size(); result.linearVariables = linearValues.size();
// Set optimization parameters // Set optimization parameters
double lambda = parameters_.lambdaInitial; double lambda = parameters.lambdaInitial;
double lambdaFactor = parameters_.lambdaFactor; double lambdaFactor = parameters.lambdaFactor;
double lambdaUpperBound = parameters_.lambdaUpperBound; double lambdaUpperBound = parameters.lambdaUpperBound;
double lambdaLowerBound = 0.5 / parameters_.lambdaUpperBound; double lambdaLowerBound = 0.5 / parameters.lambdaUpperBound;
size_t maxIterations = parameters_.maxIterations; size_t maxIterations = parameters.maxIterations;
double relativeErrorTol = parameters_.relativeErrorTol; double relativeErrorTol = parameters.relativeErrorTol;
double absoluteErrorTol = parameters_.absoluteErrorTol; double absoluteErrorTol = parameters.absoluteErrorTol;
double errorTol = parameters_.errorTol; double errorTol = parameters.errorTol;
// Create a Values that holds the current evaluation point // Create a Values that holds the current evaluation point
Values evalpoint = theta_.retract(delta_, ordering_); Values evalpoint = theta.retract(delta, ordering);
result.error = factors_.error(evalpoint); result.error = factors.error(evalpoint);
// Use a custom optimization loop so the linearization points can be controlled // Use a custom optimization loop so the linearization points can be controlled
double previousError; double previousError;
@ -335,20 +351,20 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
gttic(optimizer_iteration); gttic(optimizer_iteration);
{ {
// Linearize graph around the linearization point // Linearize graph around the linearization point
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering);
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while(true) { while(true) {
// Add prior factors at the current solution // Add prior factors at the current solution
gttic(damp); gttic(damp);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph); GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); dampedFactorGraph.reserve(linearFactorGraph.size() + delta.size());
{ {
// for each of the variables, add a prior at the current solution // for each of the variables, add a prior at the current solution
for(size_t j=0; j<delta_.size(); ++j) { for(size_t j=0; j<delta.size(); ++j) {
Matrix A = lambda * eye(delta_[j].size()); Matrix A = lambda * eye(delta[j].size());
Vector b = lambda * delta_[j]; Vector b = lambda * delta[j];
SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size()); SharedDiagonal model = noiseModel::Unit::Create(delta[j].size());
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model)); GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior); dampedFactorGraph.push_back(prior);
} }
@ -358,14 +374,14 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
gttic(solve); gttic(solve);
// Solve Damped Gaussian Factor Graph // Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction());
// update the evalpoint with the new delta // update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta, ordering_); evalpoint = theta.retract(newDelta, ordering);
gttoc(solve); gttoc(solve);
// Evaluate the new error // Evaluate the new nonlinear error
gttic(compute_error); gttic(compute_error);
double error = factors_.error(evalpoint); double error = factors.error(evalpoint);
gttoc(compute_error); gttoc(compute_error);
if(error < result.error) { if(error < result.error) {
@ -373,15 +389,15 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
// Update the error value // Update the error value
result.error = error; result.error = error;
// Update the linearization point // Update the linearization point
theta_ = evalpoint; theta = evalpoint;
// Reset the deltas to zeros // Reset the deltas to zeros
delta_.setZero(); delta.setZero();
// Put the linearization points and deltas back for specific variables // Put the linearization points and deltas back for specific variables
if(separatorValues_.size() > 0) { if(linearValues.size() > 0) {
theta_.update(separatorValues_); theta.update(linearValues);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) {
Index index = ordering_.at(key_value.key); Index index = ordering.at(key_value.key);
delta_.at(index) = newDelta.at(index); delta.at(index) = newDelta.at(index);
} }
} }
// Decrease lambda for next time // Decrease lambda for next time
@ -420,23 +436,25 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// from the result of a partial elimination. This function removes the marginalized factors and // from the result of a partial elimination. This function removes the marginalized factors and
// adds the linearized factors back in. // adds the linearized factors back in.
std::set<size_t> removedFactorSlots;
std::vector<size_t> marginalSlots;
// Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove')
// Note: It is assumed the ordering already has these keys first // Note: It is assumed the ordering already has these keys first
{
// Use the variable Index to mark the factors that will be marginalized
BOOST_FOREACH(Key key, keysToMove) {
const FastList<size_t>& slots = variableIndex_[ordering_.at(key)];
removedFactorSlots.insert(slots.begin(), slots.end());
}
// Create the linear factor graph // Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Calculate the variable index
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
// Use the variable Index to mark the factors that will be marginalized
std::set<size_t> removedFactorSlots;
BOOST_FOREACH(Key key, keysToMove) {
const FastList<size_t>& slots = variableIndex[ordering_.at(key)];
removedFactorSlots.insert(slots.begin(), slots.end());
}
// Construct an elimination tree to perform sparse elimination // Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex_) ); 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 // 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 // Find the subset of nodes/keys that must be eliminated
@ -465,8 +483,8 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
} }
} }
} }
marginalSlots = insertFactors(marginalFactors); std::vector<size_t> marginalSlots = insertFactors(marginalFactors);
}
// Cache marginalized variables and factors for later transmission to the smoother // Cache marginalized variables and factors for later transmission to the smoother
{ {
@ -535,7 +553,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering); GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index // Construct a variable index
VariableIndex variableIndex(linearFactorGraph); VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination // Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );

4
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -126,7 +126,6 @@ protected:
Values theta_; ///< Current linearization point of all variables 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 Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< The current set of linear deltas from the linearization point 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. Note: after marginalization, this is left in an inconsistent state
std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
std::vector<size_t> smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors std::vector<size_t> smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors
@ -194,7 +193,8 @@ private:
void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none); void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
/** Use a modified version of L-M to update the linearization point and delta */ /** Use a modified version of L-M to update the linearization point and delta */
Result optimize(); static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters);
/** Marginalize out the set of requested variables from the filter, caching them for the smoother /** Marginalize out the set of requested variables from the filter, caching them for the smoother
* This effectively moves the separator. * This effectively moves the separator.