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
gttic(optimize);
if(factors_.size() > 0) {
result = optimize();
result = optimize(factors_, theta_, ordering_, delta_, separatorValues_, parameters_);
}
gttoc(optimize);
@ -124,8 +124,23 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
values.insert(smootherValues_);
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
// 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
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) {
// Recalculate the variable index
variableIndex_ = VariableIndex(*factors_.symbolic(ordering_));
// Calculate the 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 = keysToMove ? 1 : 0;
int group1 = (keysToMove && (keysToMove->size() > 0) ) ? 1 : 0;
// 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
if(keysToMove && keysToMove->size() > 0) {
@ -296,34 +311,35 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
}
// Generate the permutation
Permutation forwardPermutation = *inference::PermutationCOLAMD_(variableIndex_, cmember);
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() {
ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) {
// Create output result structure
Result result;
result.nonlinearVariables = theta_.size() - separatorValues_.size();
result.linearVariables = separatorValues_.size();
result.nonlinearVariables = theta.size() - linearValues.size();
result.linearVariables = linearValues.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;
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);
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;
@ -335,20 +351,20 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
gttic(optimizer_iteration);
{
// 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
while(true) {
// Add prior factors at the current solution
gttic(damp);
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(size_t j=0; j<delta_.size(); ++j) {
Matrix A = lambda * eye(delta_[j].size());
Vector b = lambda * delta_[j];
SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size());
for(size_t j=0; j<delta.size(); ++j) {
Matrix A = lambda * eye(delta[j].size());
Vector b = lambda * delta[j];
SharedDiagonal model = noiseModel::Unit::Create(delta[j].size());
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
@ -358,14 +374,14 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta, ordering_);
evalpoint = theta.retract(newDelta, ordering);
gttoc(solve);
// Evaluate the new error
// Evaluate the new nonlinear error
gttic(compute_error);
double error = factors_.error(evalpoint);
double error = factors.error(evalpoint);
gttoc(compute_error);
if(error < result.error) {
@ -373,15 +389,15 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize() {
// Update the error value
result.error = error;
// Update the linearization point
theta_ = evalpoint;
theta = evalpoint;
// Reset the deltas to zeros
delta_.setZero();
delta.setZero();
// Put the linearization points and deltas back for specific variables
if(separatorValues_.size() > 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);
if(linearValues.size() > 0) {
theta.update(linearValues);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) {
Index index = ordering.at(key_value.key);
delta.at(index) = newDelta.at(index);
}
}
// 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
// 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')
// 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
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
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
// 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
{
@ -535,7 +553,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph);
VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination
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
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. 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
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
@ -194,7 +193,8 @@ private:
void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
/** 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
* This effectively moves the separator.