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

Fixed bugs in marginal generation. It is possible that the marginalization will be "perfect", resulting in no marginal factor. However, a factor was being created anyway.

release/4.3a0
Stephen Williams 2013-04-19 12:13:43 +00:00
parent 584f5c6c8c
commit 3eade960b8
2 changed files with 26 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -454,6 +454,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
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
@ -463,6 +464,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
}
}
}
}
marginalSlots = insertFactors(marginalFactors);
}
@ -554,9 +556,11 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
}
return marginalFactors;
}

16
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -75,6 +75,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
}
gttoc(augment_system);
factors_.print("factors_");
theta_.print("theta_");
if(factors_.size() > 0) {
// Reorder the system to ensure efficient optimization (and marginalization) performance
gttic(reorder);
reorder();
@ -82,20 +85,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
// Optimize the factors using a modified version of L-M
gttic(optimize);
if(factors_.size() > 0) {
result = optimize();
}
gttoc(optimize);
// Moved presync code into the update function. Generally, only one call to smoother.update(*) is performed
// between synchronizations, so no extra work is being done. This also allows the presync code to be performed
// while the filter is still running (instead of during the synchronization when the filter is paused)
gttic(presync);
if(separatorValues_.size() > 0) {
updateSmootherSummarization();
}
gttoc(presync);
gttoc(update);
@ -414,9 +406,11 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// Convert the marginal factors into Linear Container Factors and store
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_));
smootherSummarization_.push_back(marginalFactor);
}
}
}