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

20
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -454,12 +454,14 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
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(!separatorValues_.exists(key)) {
separatorValues_.insert(key, theta_.at(key));
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(!separatorValues_.exists(key)) {
separatorValues_.insert(key, theta_.at(key));
}
}
}
}
@ -554,8 +556,10 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
}
}
return marginalFactors;

34
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -75,27 +75,19 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
}
gttoc(augment_system);
// Reorder the system to ensure efficient optimization (and marginalization) performance
gttic(reorder);
reorder();
gttoc(reorder);
// Optimize the factors using a modified version of L-M
gttic(optimize);
factors_.print("factors_");
theta_.print("theta_");
if(factors_.size() > 0) {
// Reorder the system to ensure efficient optimization (and marginalization) performance
gttic(reorder);
reorder();
gttoc(reorder);
// Optimize the factors using a modified version of L-M
gttic(optimize);
result = optimize();
gttoc(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,8 +406,10 @@ 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());
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
smootherSummarization_.push_back(marginalFactor);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
smootherSummarization_.push_back(marginalFactor);
}
}
}