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

Replaced BOOSE_FOREACH with for in gtsam_unstable folder.

release/4.3a0
yao 2016-05-21 11:52:14 -04:00
parent 3b7c57aedf
commit f7ec58cde0
22 changed files with 194 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/base/BTree.h
View File

@ -395,7 +395,7 @@ namespace gtsam {
}; // const_iterator }; // const_iterator
// to make BTree work with BOOST_FOREACH // to make BTree work with range-based for
// We do *not* want a non-const iterator // We do *not* want a non-const iterator
typedef const_iterator iterator; typedef const_iterator iterator;

2
gtsam_unstable/base/tests/testBTree.cpp
View File

@ -145,7 +145,7 @@ TEST( BTree, iterating )
CHECK(*(++it) == p5) CHECK(*(++it) == p5)
CHECK((++it)==tree.end()) CHECK((++it)==tree.end())
// acid iterator test: BOOST_FOREACH // acid iterator test
int sum = 0; int sum = 0;
for(const KeyInt& p: tree) for(const KeyInt& p: tree)
sum += p.second; sum += p.second;

2
gtsam_unstable/base/tests/testDSF.cpp
View File

@ -295,7 +295,7 @@ TEST(DSF, mergePairwiseMatches) {
// Merge matches // Merge matches
DSF<Measurement> dsf(measurements); DSF<Measurement> dsf(measurements);
for(const Match& m, matches) for(const Match& m: matches)
dsf.makeUnionInPlace(m.first,m.second); dsf.makeUnionInPlace(m.first,m.second);
// Check that sets are merged correctly // Check that sets are merged correctly

44
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -64,7 +64,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
// Add the new variables to theta // Add the new variables to theta
theta_.insert(newTheta); theta_.insert(newTheta);
// Add new variables to the end of the ordering // Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { for(const Values::ConstKeyValuePair& key_value: newTheta) {
ordering_.push_back(key_value.key); ordering_.push_back(key_value.key);
} }
// Augment Delta // Augment Delta
@ -87,7 +87,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
current_timestamp - smootherLag_); current_timestamp - smootherLag_);
if (debug) { if (debug) {
std::cout << "Marginalizable Keys: "; std::cout << "Marginalizable Keys: ";
BOOST_FOREACH(Key key, marginalizableKeys) { for(Key key: marginalizableKeys) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -123,7 +123,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::insertFactors( void BatchFixedLagSmoother::insertFactors(
const NonlinearFactorGraph& newFactors) { const NonlinearFactorGraph& newFactors) {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { for(const NonlinearFactor::shared_ptr& factor: newFactors) {
Key index; Key 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
if (availableSlots_.size() > 0) { if (availableSlots_.size() > 0) {
@ -135,7 +135,7 @@ void BatchFixedLagSmoother::insertFactors(
factors_.push_back(factor); factors_.push_back(factor);
} }
// Update the FactorIndex // Update the FactorIndex
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
factorIndex_[key].insert(index); factorIndex_[key].insert(index);
} }
} }
@ -144,10 +144,10 @@ void BatchFixedLagSmoother::insertFactors(
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::removeFactors( void BatchFixedLagSmoother::removeFactors(
const std::set<size_t>& deleteFactors) { const std::set<size_t>& deleteFactors) {
BOOST_FOREACH(size_t slot, deleteFactors) { for(size_t slot: deleteFactors) {
if (factors_.at(slot)) { if (factors_.at(slot)) {
// Remove references to this factor from the FactorIndex // Remove references to this factor from the FactorIndex
BOOST_FOREACH(Key key, *(factors_.at(slot))) { for(Key key: *(factors_.at(slot))) {
factorIndex_[key].erase(slot); factorIndex_[key].erase(slot);
} }
// Remove the factor from the factor graph // Remove the factor from the factor graph
@ -165,7 +165,7 @@ void BatchFixedLagSmoother::removeFactors(
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) { void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
// Erase the key from the values // Erase the key from the values
theta_.erase(key); theta_.erase(key);
@ -181,7 +181,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
eraseKeyTimestampMap(keys); eraseKeyTimestampMap(keys);
// Remove marginalized keys from the ordering and delta // Remove marginalized keys from the ordering and delta
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key));
delta_.erase(key); delta_.erase(key);
} }
@ -198,7 +198,7 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
if (debug) { if (debug) {
std::cout << "Marginalizable Keys: "; std::cout << "Marginalizable Keys: ";
BOOST_FOREACH(Key key, marginalizeKeys) { for(Key key: marginalizeKeys) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -288,7 +288,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
{ {
// for each of the variables, add a prior at the current solution // for each of the variables, add a prior at the current solution
double sigma = 1.0 / std::sqrt(lambda); double sigma = 1.0 / std::sqrt(lambda);
BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { for(const VectorValues::KeyValuePair& key_value: delta_) {
size_t dim = key_value.second.size(); size_t dim = key_value.second.size();
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
Vector b = key_value.second; Vector b = key_value.second;
@ -332,7 +332,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
// Put the linearization points and deltas back for specific variables // Put the linearization points and deltas back for specific variables
if (enforceConsistency_ && (linearKeys_.size() > 0)) { if (enforceConsistency_ && (linearKeys_.size() > 0)) {
theta_.update(linearKeys_); theta_.update(linearKeys_);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { for(const Values::ConstKeyValuePair& key_value: linearKeys_) {
delta_.at(key_value.key) = newDelta.at(key_value.key); delta_.at(key_value.key) = newDelta.at(key_value.key);
} }
} }
@ -397,7 +397,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: ";
BOOST_FOREACH(Key key, marginalizeKeys) { for(Key key: marginalizeKeys) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -406,14 +406,14 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
// Identify all of the factors involving any marginalized variable. These must be removed. // Identify all of the factors involving any marginalized variable. These must be removed.
std::set<size_t> removedFactorSlots; std::set<size_t> removedFactorSlots;
VariableIndex variableIndex(factors_); VariableIndex variableIndex(factors_);
BOOST_FOREACH(Key key, marginalizeKeys) { for(Key key: marginalizeKeys) {
const FastVector<size_t>& slots = variableIndex[key]; const FastVector<size_t>& slots = variableIndex[key];
removedFactorSlots.insert(slots.begin(), slots.end()); removedFactorSlots.insert(slots.begin(), slots.end());
} }
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: ";
BOOST_FOREACH(size_t slot, removedFactorSlots) { for(size_t slot: removedFactorSlots) {
std::cout << slot << " "; std::cout << slot << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -421,7 +421,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
// Add the removed factors to a factor graph // Add the removed factors to a factor graph
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) { for(size_t slot: removedFactorSlots) {
if (factors_.at(slot)) { if (factors_.at(slot)) {
removedFactors.push_back(factors_.at(slot)); removedFactors.push_back(factors_.at(slot));
} }
@ -456,7 +456,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) { const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { for(gtsam::Key key: keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
} }
std::cout << std::endl; std::cout << std::endl;
@ -466,7 +466,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys, void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys,
const std::string& label) { const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { for(gtsam::Key key: keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
} }
std::cout << std::endl; std::cout << std::endl;
@ -477,7 +477,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
const NonlinearFactor::shared_ptr& factor) { const NonlinearFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
if (factor) { if (factor) {
BOOST_FOREACH(Key key, factor->keys()) { for(Key key: factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
} }
} else { } else {
@ -490,7 +490,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
void BatchFixedLagSmoother::PrintSymbolicFactor( void BatchFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) { const GaussianFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
BOOST_FOREACH(Key key, factor->keys()) { for(Key key: factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
} }
std::cout << " )" << std::endl; std::cout << " )" << std::endl;
@ -500,7 +500,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
void BatchFixedLagSmoother::PrintSymbolicGraph( void BatchFixedLagSmoother::PrintSymbolicGraph(
const NonlinearFactorGraph& graph, const std::string& label) { const NonlinearFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { for(const NonlinearFactor::shared_ptr& factor: graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
} }
} }
@ -509,7 +509,7 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(
void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label) { const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { for(const GaussianFactor::shared_ptr& factor: graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
} }
} }
@ -568,7 +568,7 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
// Wrap in nonlinear container factors // Wrap in nonlinear container factors
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;
marginalFactors.reserve(marginalLinearFactors.size()); marginalFactors.reserve(marginalLinearFactors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
marginalFactors += boost::make_shared<LinearContainerFactor>( marginalFactors += boost::make_shared<LinearContainerFactor>(
gaussianFactor, theta); gaussianFactor, theta);
if (debug) { if (debug) {

44
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -33,7 +33,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
} else { } else {
std::cout << "f( "; std::cout << "f( ";
} }
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -46,7 +46,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl; std::cout << indent << title << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { for(const NonlinearFactor::shared_ptr& factor: factors) {
PrintNonlinearFactor(factor, indent + " ", keyFormatter); PrintNonlinearFactor(factor, indent + " ", keyFormatter);
} }
} }
@ -55,7 +55,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph
void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots, void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl; std::cout << indent << title << std::endl;
BOOST_FOREACH(size_t slot, slots) { for(size_t slot: slots) {
PrintNonlinearFactor(factors.at(slot), indent + " ", keyFormatter); PrintNonlinearFactor(factors.at(slot), indent + " ", keyFormatter);
} }
} }
@ -74,7 +74,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
} else { } else {
std::cout << "g( "; std::cout << "g( ";
} }
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -87,7 +87,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors, void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors,
const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title << std::endl; std::cout << indent << title << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { for(const GaussianFactor::shared_ptr& factor: factors) {
PrintLinearFactor(factor, indent + " ", keyFormatter); PrintLinearFactor(factor, indent + " ", keyFormatter);
} }
} }
@ -139,7 +139,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
// Add the new variables to theta // Add the new variables to theta
theta_.insert(newTheta); theta_.insert(newTheta);
// Add new variables to the end of the ordering // Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { for(const Values::ConstKeyValuePair& key_value: newTheta) {
ordering_.push_back(key_value.key); ordering_.push_back(key_value.key);
} }
// Augment Delta // Augment Delta
@ -222,7 +222,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
// Find the set of new separator keys // Find the set of new separator keys
KeySet newSeparatorKeys; KeySet newSeparatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
newSeparatorKeys.insert(key_value.key); newSeparatorKeys.insert(key_value.key);
} }
@ -236,7 +236,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
graph.push_back(smootherShortcut_); graph.push_back(smootherShortcut_);
Values values; Values values;
values.insert(smootherSummarizationValues); values.insert(smootherSummarizationValues);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
if(!values.exists(key_value.key)) { if(!values.exists(key_value.key)) {
values.insert(key_value.key, key_value.value); values.insert(key_value.key, key_value.value);
} }
@ -321,7 +321,7 @@ std::vector<size_t> ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr
slots.reserve(factors.size()); slots.reserve(factors.size());
// 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
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { for(const NonlinearFactor::shared_ptr& factor: factors) {
size_t slot; size_t slot;
if(availableSlots_.size() > 0) { if(availableSlots_.size() > 0) {
slot = availableSlots_.front(); slot = availableSlots_.front();
@ -345,7 +345,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors); gttic(remove_factors);
// For each factor slot to delete... // For each factor slot to delete...
BOOST_FOREACH(size_t slot, slots) { for(size_t slot: slots) {
// Remove the factor from the graph // Remove the factor from the graph
factors_.remove(slot); factors_.remove(slot);
@ -431,7 +431,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
double sigma = 1.0 / std::sqrt(lambda); double sigma = 1.0 / std::sqrt(lambda);
// for each of the variables, add a prior at the current solution // for each of the variables, add a prior at the current solution
BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta) { for(const VectorValues::KeyValuePair& key_value: delta) {
size_t dim = key_value.second.size(); size_t dim = key_value.second.size();
Matrix A = Matrix::Identity(dim,dim); Matrix A = Matrix::Identity(dim,dim);
Vector b = key_value.second; Vector b = key_value.second;
@ -471,7 +471,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
// Put the linearization points and deltas back for specific variables // Put the linearization points and deltas back for specific variables
if(linearValues.size() > 0) { if(linearValues.size() > 0) {
theta.update(linearValues); theta.update(linearValues);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) { for(const Values::ConstKeyValuePair& key_value: linearValues) {
delta.at(key_value.key) = newDelta.at(key_value.key); delta.at(key_value.key) = newDelta.at(key_value.key);
} }
} }
@ -535,7 +535,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove)
std::vector<size_t> removedFactorSlots; std::vector<size_t> removedFactorSlots;
VariableIndex variableIndex(factors_); VariableIndex variableIndex(factors_);
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
const FastVector<size_t>& slots = variableIndex[key]; const FastVector<size_t>& slots = variableIndex[key];
removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
} }
@ -544,14 +544,14 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
// Remove any linear/marginal factor that made it into the set // Remove any linear/marginal factor that made it into the set
BOOST_FOREACH(size_t index, separatorSummarizationSlots_) { for(size_t index: separatorSummarizationSlots_) {
removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
} }
// TODO: Make this compact // TODO: Make this compact
if(debug) { if(debug) {
std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: "; std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: ";
BOOST_FOREACH(size_t slot, removedFactorSlots) { for(size_t slot: removedFactorSlots) {
std::cout << slot << " "; std::cout << slot << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -559,7 +559,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Add these factors to a factor graph // Add these factors to a factor graph
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) { for(size_t slot: removedFactorSlots) {
if(factors_.at(slot)) { if(factors_.at(slot)) {
removedFactors.push_back(factors_.at(slot)); removedFactors.push_back(factors_.at(slot));
} }
@ -574,10 +574,10 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
KeySet newSeparatorKeys = removedFactors.keys(); KeySet newSeparatorKeys = removedFactors.keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
newSeparatorKeys.insert(key_value.key); newSeparatorKeys.insert(key_value.key);
} }
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
newSeparatorKeys.erase(key); newSeparatorKeys.erase(key);
} }
@ -585,7 +585,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
KeySet shortcutKeys = newSeparatorKeys; KeySet shortcutKeys = newSeparatorKeys;
BOOST_FOREACH(Key key, smootherSummarization_.keys()) { for(Key key: smootherSummarization_.keys()) {
shortcutKeys.insert(key); shortcutKeys.insert(key);
} }
@ -632,7 +632,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
// Update the separatorValues object (should only contain the new separator keys) // Update the separatorValues object (should only contain the new separator keys)
separatorValues_.clear(); separatorValues_.clear();
BOOST_FOREACH(Key key, separatorSummarization.keys()) { for(Key key: separatorSummarization.keys()) {
separatorValues_.insert(key, theta_.at(key)); separatorValues_.insert(key, theta_.at(key));
} }
@ -641,12 +641,12 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
removeFactors(removedFactorSlots); removeFactors(removedFactorSlots);
// Add the linearization point of the moved variables to the smoother cache // Add the linearization point of the moved variables to the smoother cache
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
smootherValues_.insert(key, theta_.at(key)); smootherValues_.insert(key, theta_.at(key));
} }
// Remove marginalized keys from values (and separator) // Remove marginalized keys from values (and separator)
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
theta_.erase(key); theta_.erase(key);
ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key)); ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key));
delta_.erase(key); delta_.erase(key);

2
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -244,7 +244,7 @@ private:
template<class Container> template<class Container>
void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
std::cout << indent << title; std::cout << indent << title;
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
std::cout << " " << keyFormatter(key); std::cout << " " << keyFormatter(key);
} }
std::cout << std::endl; std::cout << std::endl;

24
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -28,7 +28,7 @@ namespace gtsam {
void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s; std::cout << s;
std::cout << " Factors:" << std::endl; std::cout << " Factors:" << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { for(const NonlinearFactor::shared_ptr& factor: factors_) {
PrintNonlinearFactor(factor, " ", keyFormatter); PrintNonlinearFactor(factor, " ", keyFormatter);
} }
theta_.print("Values:\n"); theta_.print("Values:\n");
@ -61,7 +61,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
theta_.insert(newTheta); theta_.insert(newTheta);
// Add new variables to the end of the ordering // Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { for(const Values::ConstKeyValuePair& key_value: newTheta) {
ordering_.push_back(key_value.key); ordering_.push_back(key_value.key);
} }
@ -135,7 +135,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
removeFactors(filterSummarizationSlots_); removeFactors(filterSummarizationSlots_);
// Insert new linpoints into the values, augment the ordering, and store new dims to augment delta // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) { for(const Values::ConstKeyValuePair& key_value: smootherValues) {
std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value); std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
if(iter_inserted.second) { if(iter_inserted.second) {
// If the insert succeeded // If the insert succeeded
@ -146,7 +146,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
iter_inserted.first->value = key_value.value; iter_inserted.first->value = key_value.value;
} }
} }
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) { for(const Values::ConstKeyValuePair& key_value: separatorValues) {
std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value); std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
if(iter_inserted.second) { if(iter_inserted.second) {
// If the insert succeeded // If the insert succeeded
@ -188,7 +188,7 @@ std::vector<size_t> ConcurrentBatchSmoother::insertFactors(const NonlinearFactor
slots.reserve(factors.size()); slots.reserve(factors.size());
// 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
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { for(const NonlinearFactor::shared_ptr& factor: factors) {
size_t slot; size_t slot;
if(availableSlots_.size() > 0) { if(availableSlots_.size() > 0) {
slot = availableSlots_.front(); slot = availableSlots_.front();
@ -212,7 +212,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors); gttic(remove_factors);
// For each factor slot to delete... // For each factor slot to delete...
BOOST_FOREACH(size_t slot, slots) { for(size_t slot: slots) {
// Remove the factor from the graph // Remove the factor from the graph
factors_.remove(slot); factors_.remove(slot);
@ -277,7 +277,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
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
BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { for(const VectorValues::KeyValuePair& key_value: delta_) {
size_t dim = key_value.second.size(); size_t dim = key_value.second.size();
Matrix A = Matrix::Identity(dim,dim); Matrix A = Matrix::Identity(dim,dim);
Vector b = key_value.second; Vector b = key_value.second;
@ -322,7 +322,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
// 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(separatorValues_.size() > 0) {
theta_.update(separatorValues_); theta_.update(separatorValues_);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
delta_.at(key_value.key) = newDelta.at(key_value.key); delta_.at(key_value.key) = newDelta.at(key_value.key);
} }
} }
@ -366,13 +366,13 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// Create a nonlinear factor graph without the filter summarization factors // Create a nonlinear factor graph without the filter summarization factors
NonlinearFactorGraph graph(factors_); NonlinearFactorGraph graph(factors_);
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { for(size_t slot: filterSummarizationSlots_) {
graph.remove(slot); graph.remove(slot);
} }
// Get the set of separator keys // Get the set of separator keys
gtsam::KeySet separatorKeys; gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
separatorKeys.insert(key_value.key); separatorKeys.insert(key_value.key);
} }
@ -389,7 +389,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
} else { } else {
std::cout << "f( "; std::cout << "f( ";
} }
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -403,7 +403,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
std::cout << "g( "; std::cout << "g( ";
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -77,7 +77,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
// Wrap in nonlinear container factors // Wrap in nonlinear container factors
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;
marginalFactors.reserve(marginalLinearFactors.size()); marginalFactors.reserve(marginalLinearFactors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
marginalFactors += boost::make_shared<LinearContainerFactor>(gaussianFactor, theta); marginalFactors += boost::make_shared<LinearContainerFactor>(gaussianFactor, theta);
} }

54
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -69,17 +69,17 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
int group = 1; int group = 1;
// Set all existing variables to Group1 // Set all existing variables to Group1
if(isam2_.getLinearizationPoint().size() > 0) { if(isam2_.getLinearizationPoint().size() > 0) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
orderingConstraints->operator[](key_value.key) = group; orderingConstraints->operator[](key_value.key) = group;
} }
++group; ++group;
} }
// Assign new variables to the root // Assign new variables to the root
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { for(const Values::ConstKeyValuePair& key_value: newTheta) {
orderingConstraints->operator[](key_value.key) = group; orderingConstraints->operator[](key_value.key) = group;
} }
// Set marginalizable variables to Group0 // Set marginalizable variables to Group0
BOOST_FOREACH(Key key, *keysToMove){ for(Key key: *keysToMove){
orderingConstraints->operator[](key) = 0; orderingConstraints->operator[](key) = 0;
} }
} }
@ -92,7 +92,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
boost::optional<FastList<Key> > additionalKeys = boost::none; boost::optional<FastList<Key> > additionalKeys = boost::none;
if(keysToMove && keysToMove->size() > 0) { if(keysToMove && keysToMove->size() > 0) {
std::set<Key> markedKeys; std::set<Key> markedKeys;
BOOST_FOREACH(Key key, *keysToMove) { for(Key key: *keysToMove) {
if(isam2_.getLinearizationPoint().exists(key)) { if(isam2_.getLinearizationPoint().exists(key)) {
ISAM2Clique::shared_ptr clique = isam2_[key]; ISAM2Clique::shared_ptr clique = isam2_[key];
GaussianConditional::const_iterator key_iter = clique->conditional()->begin(); GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
markedKeys.insert(*key_iter); markedKeys.insert(*key_iter);
++key_iter; ++key_iter;
} }
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
RecursiveMarkAffectedKeys(key, child, markedKeys); RecursiveMarkAffectedKeys(key, child, markedKeys);
} }
} }
@ -120,7 +120,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
// Cache these factors for later transmission to the smoother // Cache these factors for later transmission to the smoother
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) { for(size_t slot: removedFactorSlots) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
smootherFactors_.push_back(factor); smootherFactors_.push_back(factor);
@ -128,7 +128,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
} }
} }
// Cache removed values for later transmission to the smoother // Cache removed values for later transmission to the smoother
BOOST_FOREACH(Key key, *keysToMove) { for(Key key: *keysToMove) {
smootherValues_.insert(key, isam2_.getLinearizationPoint().at(key)); smootherValues_.insert(key, isam2_.getLinearizationPoint().at(key));
} }
gttoc(cache_smoother_factors); gttoc(cache_smoother_factors);
@ -138,7 +138,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
FactorIndices deletedFactorsIndices; FactorIndices deletedFactorsIndices;
isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices); isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end()); currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
BOOST_FOREACH(size_t index, deletedFactorsIndices) { for(size_t index: deletedFactorsIndices) {
currentSmootherSummarizationSlots_.erase(std::remove(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), index), currentSmootherSummarizationSlots_.end()); currentSmootherSummarizationSlots_.erase(std::remove(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), index), currentSmootherSummarizationSlots_.end());
} }
gttoc(marginalize); gttoc(marginalize);
@ -193,7 +193,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
graph.push_back(smootherShortcut_); graph.push_back(smootherShortcut_);
Values values; Values values;
values.insert(smootherSummarizationValues); values.insert(smootherSummarizationValues);
BOOST_FOREACH(Key key, newSeparatorKeys) { for(Key key: newSeparatorKeys) {
if(!values.exists(key)) { if(!values.exists(key)) {
values.insert(key, isam2_.getLinearizationPoint().at(key)); values.insert(key, isam2_.getLinearizationPoint().at(key));
} }
@ -201,7 +201,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
// Force iSAM2 not to relinearize anything during this iteration // Force iSAM2 not to relinearize anything during this iteration
FastList<Key> noRelinKeys; FastList<Key> noRelinKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
noRelinKeys.push_back(key_value.key); noRelinKeys.push_back(key_value.key);
} }
@ -232,7 +232,7 @@ void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& fil
filterSummarization = calculateFilterSummarization(); filterSummarization = calculateFilterSummarization();
// Copy the current separator values into the output // Copy the current separator values into the output
BOOST_FOREACH(Key key, isam2_.getFixedVariables()) { for(Key key: isam2_.getFixedVariables()) {
filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key)); filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key));
} }
@ -271,12 +271,12 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons
if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique // Mark the frontal keys of the current clique
BOOST_FOREACH(Key idx, clique->conditional()->frontals()) { for(Key idx: clique->conditional()->frontals()) {
additionalKeys.insert(idx); additionalKeys.insert(idx);
} }
// Recursively mark all of the children // Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
RecursiveMarkAffectedKeys(key, child, additionalKeys); RecursiveMarkAffectedKeys(key, child, additionalKeys);
} }
} }
@ -290,7 +290,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam
// Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
FactorIndices removedFactorSlots; FactorIndices removedFactorSlots;
const VariableIndex& variableIndex = isam2.getVariableIndex(); const VariableIndex& variableIndex = isam2.getVariableIndex();
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
const FastVector<size_t>& slots = variableIndex[key]; const FastVector<size_t>& slots = variableIndex[key];
removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
} }
@ -300,7 +300,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam
removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
// Remove any linear/marginal factor that made it into the set // Remove any linear/marginal factor that made it into the set
BOOST_FOREACH(size_t index, factorsToIgnore) { for(size_t index: factorsToIgnore) {
removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
} }
@ -313,13 +313,13 @@ void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& rem
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
KeySet shortcutKeys; KeySet shortcutKeys;
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { for(size_t slot: currentSmootherSummarizationSlots_) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
shortcutKeys.insert(factor->begin(), factor->end()); shortcutKeys.insert(factor->begin(), factor->end());
} }
} }
BOOST_FOREACH(Key key, previousSmootherSummarization_.keys()) { for(Key key: previousSmootherSummarization_.keys()) {
shortcutKeys.insert(key); shortcutKeys.insert(key);
} }
@ -347,15 +347,15 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// Find all cliques that contain any separator variables // Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques; std::set<ISAM2Clique::shared_ptr> separatorCliques;
BOOST_FOREACH(Key key, separatorKeys) { for(Key key: separatorKeys) {
ISAM2Clique::shared_ptr clique = isam2_[key]; ISAM2Clique::shared_ptr clique = isam2_[key];
separatorCliques.insert( clique ); separatorCliques.insert( clique );
} }
// Create the set of clique keys LC: // Create the set of clique keys LC:
std::vector<Key> cliqueKeys; std::vector<Key> cliqueKeys;
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
BOOST_FOREACH(Key key, clique->conditional()->frontals()) { for(Key key: clique->conditional()->frontals()) {
cliqueKeys.push_back(key); cliqueKeys.push_back(key);
} }
} }
@ -363,8 +363,8 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// Gather all factors that involve only clique keys // Gather all factors that involve only clique keys
std::set<size_t> cliqueFactorSlots; std::set<size_t> cliqueFactorSlots;
BOOST_FOREACH(Key key, cliqueKeys) { for(Key key: cliqueKeys) {
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { for(size_t slot: isam2_.getVariableIndex()[key]) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
std::set<Key> factorKeys(factor->begin(), factor->end()); std::set<Key> factorKeys(factor->begin(), factor->end());
@ -376,29 +376,29 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
} }
// Remove any factor included in the current smoother summarization // Remove any factor included in the current smoother summarization
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { for(size_t slot: currentSmootherSummarizationSlots_) {
cliqueFactorSlots.erase(slot); cliqueFactorSlots.erase(slot);
} }
// Create a factor graph from the identified factors // Create a factor graph from the identified factors
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
BOOST_FOREACH(size_t slot, cliqueFactorSlots) { for(size_t slot: cliqueFactorSlots) {
graph.push_back(isam2_.getFactorsUnsafe().at(slot)); graph.push_back(isam2_.getFactorsUnsafe().at(slot));
} }
// Find the set of children of the separator cliques // Find the set of children of the separator cliques
std::set<ISAM2Clique::shared_ptr> childCliques; std::set<ISAM2Clique::shared_ptr> childCliques;
// Add all of the children // Add all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
childCliques.insert(clique->children.begin(), clique->children.end()); childCliques.insert(clique->children.begin(), clique->children.end());
} }
// Remove any separator cliques that were added because they were children of other separator cliques // Remove any separator cliques that were added because they were children of other separator cliques
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
childCliques.erase(clique); childCliques.erase(clique);
} }
// Augment the factor graph with cached factors from the children // Augment the factor graph with cached factors from the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { for(const ISAM2Clique::shared_ptr& clique: childCliques) {
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
graph.push_back( factor ); graph.push_back( factor );
} }

28
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -66,7 +66,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
// Also, mark the separator keys as fixed linearization points // Also, mark the separator keys as fixed linearization points
FastMap<Key,int> constrainedKeys; FastMap<Key,int> constrainedKeys;
FastList<Key> noRelinKeys; FastList<Key> noRelinKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
constrainedKeys[key_value.key] = 1; constrainedKeys[key_value.key] = 1;
noRelinKeys.push_back(key_value.key); noRelinKeys.push_back(key_value.key);
} }
@ -82,12 +82,12 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
Values values(newTheta); Values values(newTheta);
// Unfortunately, we must be careful here, as some of the smoother values // Unfortunately, we must be careful here, as some of the smoother values
// and/or separator values may have been added previously // and/or separator values may have been added previously
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { for(const Values::ConstKeyValuePair& key_value: smootherValues_) {
if(!isam2_.getLinearizationPoint().exists(key_value.key)) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
values.insert(key_value.key, smootherValues_.at(key_value.key)); values.insert(key_value.key, smootherValues_.at(key_value.key));
} }
} }
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
if(!isam2_.getLinearizationPoint().exists(key_value.key)) { if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
values.insert(key_value.key, separatorValues_.at(key_value.key)); values.insert(key_value.key, separatorValues_.at(key_value.key));
} }
@ -188,15 +188,15 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
// Find all cliques that contain any separator variables // Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques; std::set<ISAM2Clique::shared_ptr> separatorCliques;
BOOST_FOREACH(Key key, separatorValues_.keys()) { for(Key key: separatorValues_.keys()) {
ISAM2Clique::shared_ptr clique = isam2_[key]; ISAM2Clique::shared_ptr clique = isam2_[key];
separatorCliques.insert( clique ); separatorCliques.insert( clique );
} }
// Create the set of clique keys LC: // Create the set of clique keys LC:
std::vector<Key> cliqueKeys; std::vector<Key> cliqueKeys;
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
BOOST_FOREACH(Key key, clique->conditional()->frontals()) { for(Key key: clique->conditional()->frontals()) {
cliqueKeys.push_back(key); cliqueKeys.push_back(key);
} }
} }
@ -204,8 +204,8 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
// Gather all factors that involve only clique keys // Gather all factors that involve only clique keys
std::set<size_t> cliqueFactorSlots; std::set<size_t> cliqueFactorSlots;
BOOST_FOREACH(Key key, cliqueKeys) { for(Key key: cliqueKeys) {
BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { for(size_t slot: isam2_.getVariableIndex()[key]) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
std::set<Key> factorKeys(factor->begin(), factor->end()); std::set<Key> factorKeys(factor->begin(), factor->end());
@ -217,36 +217,36 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
} }
// Remove any factor included in the filter summarization // Remove any factor included in the filter summarization
BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { for(size_t slot: filterSummarizationSlots_) {
cliqueFactorSlots.erase(slot); cliqueFactorSlots.erase(slot);
} }
// Create a factor graph from the identified factors // Create a factor graph from the identified factors
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
BOOST_FOREACH(size_t slot, cliqueFactorSlots) { for(size_t slot: cliqueFactorSlots) {
graph.push_back(isam2_.getFactorsUnsafe().at(slot)); graph.push_back(isam2_.getFactorsUnsafe().at(slot));
} }
// Find the set of children of the separator cliques // Find the set of children of the separator cliques
std::set<ISAM2Clique::shared_ptr> childCliques; std::set<ISAM2Clique::shared_ptr> childCliques;
// Add all of the children // Add all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
childCliques.insert(clique->children.begin(), clique->children.end()); childCliques.insert(clique->children.begin(), clique->children.end());
} }
// Remove any separator cliques that were added because they were children of other separator cliques // Remove any separator cliques that were added because they were children of other separator cliques
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
childCliques.erase(clique); childCliques.erase(clique);
} }
// Augment the factor graph with cached factors from the children // Augment the factor graph with cached factors from the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { for(const ISAM2Clique::shared_ptr& clique: childCliques) {
LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
graph.push_back( factor ); graph.push_back( factor );
} }
// Get the set of separator keys // Get the set of separator keys
KeySet separatorKeys; KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
separatorKeys.insert(key_value.key); separatorKeys.insert(key_value.key);
} }

4
gtsam_unstable/nonlinear/FixedLagSmoother.cpp
View File

@ -36,7 +36,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) {
// Loop through each key and add/update it in the map // Loop through each key and add/update it in the map
BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { for(const KeyTimestampMap::value_type& key_timestamp: timestamps) {
// Check to see if this key already exists in the database // Check to see if this key already exists in the database
KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);
@ -65,7 +65,7 @@ void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps)
/* ************************************************************************* */ /* ************************************************************************* */
void FixedLagSmoother::eraseKeyTimestampMap(const std::set<Key>& keys) { void FixedLagSmoother::eraseKeyTimestampMap(const std::set<Key>& keys) {
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
// Erase the key from the Timestamp->Key map // Erase the key from the Timestamp->Key map
double timestamp = keyTimestampMap_.at(key); double timestamp = keyTimestampMap_.at(key);

28
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -34,12 +34,12 @@ void recursiveMarkAffectedKeys(const Key& key,
!= clique->conditional()->endParents()) { != clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique // Mark the frontal keys of the current clique
BOOST_FOREACH(Key i, clique->conditional()->frontals()) { for(Key i: clique->conditional()->frontals()) {
additionalKeys.insert(i); additionalKeys.insert(i);
} }
// Recursively mark all of the children // Recursively mark all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys); recursiveMarkAffectedKeys(key, child, additionalKeys);
} }
} }
@ -93,7 +93,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
if (debug) { if (debug) {
std::cout << "Marginalizable Keys: "; std::cout << "Marginalizable Keys: ";
BOOST_FOREACH(Key key, marginalizableKeys) { for(Key key: marginalizableKeys) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
@ -116,9 +116,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
// Mark additional keys between the marginalized keys and the leaves // Mark additional keys between the marginalized keys and the leaves
std::set<Key> additionalKeys; std::set<Key> additionalKeys;
BOOST_FOREACH(Key key, marginalizableKeys) { for(Key key: marginalizableKeys) {
ISAM2Clique::shared_ptr clique = isam_[key]; ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys); recursiveMarkAffectedKeys(key, child, additionalKeys);
} }
} }
@ -180,11 +180,11 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
constrainedKeys = FastMap<Key, int>(); constrainedKeys = FastMap<Key, int>();
// Generate ordering constraints so that the marginalizable variables will be eliminated first // Generate ordering constraints so that the marginalizable variables will be eliminated first
// Set all variables to Group1 // Set all variables to Group1
BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { for(const TimestampKeyMap::value_type& timestamp_key: timestampKeyMap_) {
constrainedKeys->operator[](timestamp_key.second) = 1; constrainedKeys->operator[](timestamp_key.second) = 1;
} }
// Set marginalizable variables to Group0 // Set marginalizable variables to Group0
BOOST_FOREACH(Key key, marginalizableKeys) { for(Key key: marginalizableKeys) {
constrainedKeys->operator[](key) = 0; constrainedKeys->operator[](key) = 0;
} }
} }
@ -194,7 +194,7 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) { const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
std::cout << " " << DefaultKeyFormatter(key); std::cout << " " << DefaultKeyFormatter(key);
} }
std::cout << std::endl; std::cout << std::endl;
@ -204,7 +204,7 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
void IncrementalFixedLagSmoother::PrintSymbolicFactor( void IncrementalFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) { const GaussianFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
BOOST_FOREACH(Key key, factor->keys()) { for(Key key: factor->keys()) {
std::cout << " " << DefaultKeyFormatter(key); std::cout << " " << DefaultKeyFormatter(key);
} }
std::cout << " )" << std::endl; std::cout << " )" << std::endl;
@ -214,7 +214,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(
void IncrementalFixedLagSmoother::PrintSymbolicGraph( void IncrementalFixedLagSmoother::PrintSymbolicGraph(
const GaussianFactorGraph& graph, const std::string& label) { const GaussianFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { for(const GaussianFactor::shared_ptr& factor: graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
} }
} }
@ -224,7 +224,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam,
const std::string& label) { const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
if (!isam.roots().empty()) { if (!isam.roots().empty()) {
BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { for(const ISAM2::sharedClique& root: isam.roots()) {
PrintSymbolicTreeHelper(root); PrintSymbolicTreeHelper(root);
} }
} else } else
@ -237,18 +237,18 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
// Print the current clique // Print the current clique
std::cout << indent << "P( "; std::cout << indent << "P( ";
BOOST_FOREACH(Key key, clique->conditional()->frontals()) { for(Key key: clique->conditional()->frontals()) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
if (clique->conditional()->nrParents() > 0) if (clique->conditional()->nrParents() > 0)
std::cout << "| "; std::cout << "| ";
BOOST_FOREACH(Key key, clique->conditional()->parents()) { for(Key key: clique->conditional()->parents()) {
std::cout << DefaultKeyFormatter(key) << " "; std::cout << DefaultKeyFormatter(key) << " ";
} }
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
// Recursively print all of the children // Recursively print all of the children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
PrintSymbolicTreeHelper(child, indent + " "); PrintSymbolicTreeHelper(child, indent + " ");
} }
} }

10
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -27,7 +27,7 @@ LinearizedGaussianFactor::LinearizedGaussianFactor(
: NonlinearFactor(gaussian->keys()) : NonlinearFactor(gaussian->keys())
{ {
// Extract the keys and linearization points // Extract the keys and linearization points
BOOST_FOREACH(const Key& key, gaussian->keys()) { for(const Key& key: gaussian->keys()) {
// extract linearization point // extract linearization point
assert(lin_points.exists(key)); assert(lin_points.exists(key));
this->lin_points_.insert(key, lin_points.at(key)); this->lin_points_.insert(key, lin_points.at(key));
@ -65,7 +65,7 @@ void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& k
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "Nonlinear Keys: "; std::cout << "Nonlinear Keys: ";
BOOST_FOREACH(const Key& key, this->keys()) for(const Key& key: this->keys())
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
std::cout << std::endl; std::cout << std::endl;
@ -105,7 +105,7 @@ LinearizedJacobianFactor::linearize(const Values& c) const {
// Create the 'terms' data structure for the Jacobian constructor // Create the 'terms' data structure for the Jacobian constructor
std::vector<std::pair<Key, Matrix> > terms; std::vector<std::pair<Key, Matrix> > terms;
BOOST_FOREACH(Key key, keys()) { for(Key key: keys()) {
terms.push_back(std::make_pair(key, this->A(key))); terms.push_back(std::make_pair(key, this->A(key)));
} }
@ -120,7 +120,7 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
Vector errorVector = -b(); Vector errorVector = -b();
BOOST_FOREACH(Key key, this->keys()) { for(Key key: this->keys()) {
const Value& newPt = c.at(key); const Value& newPt = c.at(key);
const Value& linPt = lin_points_.at(key); const Value& linPt = lin_points_.at(key);
Vector d = linPt.localCoordinates_(newPt); Vector d = linPt.localCoordinates_(newPt);
@ -162,7 +162,7 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "Nonlinear Keys: "; std::cout << "Nonlinear Keys: ";
BOOST_FOREACH(const Key& key, this->keys()) for(const Key& key: this->keys())
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
std::cout << std::endl; std::cout << std::endl;

18
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -64,18 +64,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
std::set<Key> KeysToKeep; std::set<Key> KeysToKeep;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
KeysToKeep.insert(key_value.key); KeysToKeep.insert(key_value.key);
} // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
BOOST_FOREACH(Key key, keysToMarginalize) { for(Key key: keysToMarginalize) {
KeysToKeep.erase(key); KeysToKeep.erase(key);
} // we removed the keys that we have to marginalize } // we removed the keys that we have to marginalize
Ordering ordering; Ordering ordering;
BOOST_FOREACH(Key key, keysToMarginalize) { for(Key key: keysToMarginalize) {
ordering.push_back(key); ordering.push_back(key);
} // the keys that we marginalize should be at the beginning in the ordering } // the keys that we marginalize should be at the beginning in the ordering
BOOST_FOREACH(Key key, KeysToKeep) { for(Key key: KeysToKeep) {
ordering.push_back(key); ordering.push_back(key);
} }
@ -84,7 +84,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals; NonlinearFactorGraph LinearContainerForGaussianMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
} }
return LinearContainerForGaussianMarginals; return LinearContainerForGaussianMarginals;
@ -439,7 +439,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
// ========================================================== // ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery)); expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { for(const GaussianFactor::shared_ptr& factor: result) {
expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); expectedGraph.push_back(LinearContainerFactor(factor, partialValues));
} }
@ -451,7 +451,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
Values expectedValues = optimalValues; Values expectedValues = optimalValues;
// Check // Check
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
expectedValues.erase(key); expectedValues.erase(key);
} }
@ -1014,7 +1014,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { for(const GaussianFactor::shared_ptr& factor: result) {
expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
} }
@ -1069,7 +1069,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { for(const GaussianFactor::shared_ptr& factor: result) {
expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
} }

4
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -560,14 +560,14 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
KeySet eliminateKeys = linearFactors->keys(); KeySet eliminateKeys = linearFactors->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
eliminateKeys.erase(key_value.key); eliminateKeys.erase(key_value.key);
} }
std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end()); std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end());
GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) { for(const GaussianFactor::shared_ptr& factor: result) {
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
} }

26
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -62,7 +62,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int
// it is the same as the input graph, but we removed the empty factors that may be present in the input graph // it is the same as the input graph, but we removed the empty factors that may be present in the input graph
NonlinearFactorGraph graphForISAM2; NonlinearFactorGraph graphForISAM2;
BOOST_FOREACH(NonlinearFactor::shared_ptr factor, graph){ for(NonlinearFactor::shared_ptr factor: graph){
if(factor) if(factor)
graphForISAM2.push_back(factor); graphForISAM2.push_back(factor);
} }
@ -80,18 +80,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
std::set<Key> KeysToKeep; std::set<Key> KeysToKeep;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
KeysToKeep.insert(key_value.key); KeysToKeep.insert(key_value.key);
} // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
BOOST_FOREACH(Key key, keysToMarginalize) { for(Key key: keysToMarginalize) {
KeysToKeep.erase(key); KeysToKeep.erase(key);
} // we removed the keys that we have to marginalize } // we removed the keys that we have to marginalize
Ordering ordering; Ordering ordering;
BOOST_FOREACH(Key key, keysToMarginalize) { for(Key key: keysToMarginalize) {
ordering.push_back(key); ordering.push_back(key);
} // the keys that we marginalize should be at the beginning in the ordering } // the keys that we marginalize should be at the beginning in the ordering
BOOST_FOREACH(Key key, KeysToKeep) { for(Key key: KeysToKeep) {
ordering.push_back(key); ordering.push_back(key);
} }
@ -101,7 +101,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals; NonlinearFactorGraph LinearContainerForGaussianMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint)); LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
} }
@ -417,7 +417,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
Values expectedValues = optimalValues; Values expectedValues = optimalValues;
// Check // Check
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
expectedValues.erase(key); expectedValues.erase(key);
} }
@ -443,7 +443,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
// ========================================================== // ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery)); expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter, // the linearization point for the linear container is optional, but it is not used in the filter,
// therefore if we add it here it will not pass the test // therefore if we add it here it will not pass the test
// expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
@ -501,7 +501,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
Values expectedValues = optimalValues; Values expectedValues = optimalValues;
// Check // Check
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
expectedValues.erase(key); expectedValues.erase(key);
} }
@ -527,7 +527,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
// ========================================================== // ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery)); expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter, // the linearization point for the linear container is optional, but it is not used in the filter,
// therefore if we add it here it will not pass the test // therefore if we add it here it will not pass the test
// expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues)); // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
@ -543,7 +543,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
Values optimalValues2 = BatchOptimize(newFactors, optimalValues); Values optimalValues2 = BatchOptimize(newFactors, optimalValues);
Values expectedValues2 = optimalValues2; Values expectedValues2 = optimalValues2;
// Check // Check
BOOST_FOREACH(Key key, keysToMove) { for(Key key: keysToMove) {
expectedValues2.erase(key); expectedValues2.erase(key);
} }
Values actualValues2 = filter.calculateEstimate(); Values actualValues2 = filter.calculateEstimate();
@ -1116,7 +1116,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
} }
@ -1167,7 +1167,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
expectedMarginals.push_back(LinearContainerFactor(factor, newValues)); expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
} }

6
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -512,7 +512,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
Values expectedLinearizationPoint = filterSeparatorValues; Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint; Values actualLinearizationPoint;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
} }
CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@ -580,14 +580,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
KeySet allkeys = LinFactorGraph->keys(); KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
} }
std::vector<Key> variables(allkeys.begin(), allkeys.end()); std::vector<Key> variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { for(const GaussianFactor::shared_ptr& factor: *result.second) {
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
} }

6
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
Values expectedLinearizationPoint = filterSeparatorValues; Values expectedLinearizationPoint = filterSeparatorValues;
Values actualLinearizationPoint; Values actualLinearizationPoint;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
} }
CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@ -582,13 +582,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
KeySet allkeys = LinFactorGraph->keys(); KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
std::vector<Key> variables(allkeys.begin(), allkeys.end()); std::vector<Key> variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) { for(const GaussianFactor::shared_ptr& factor: *result.second) {
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues)); expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
} }

33
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -13,7 +13,6 @@
#include <stdexcept> #include <stdexcept>
#include <iostream> #include <iostream>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_array.hpp> #include <boost/shared_array.hpp>
#include <boost/timer.hpp> #include <boost/timer.hpp>
@ -193,7 +192,7 @@ namespace gtsam { namespace partition {
std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
int index1, index2; int index1, index2;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ for(const typename GenericGraph::value_type& factor: graph){
index1 = dictionary[factor->key1.index]; index1 = dictionary[factor->key1.index];
index2 = dictionary[factor->key2.index]; index2 = dictionary[factor->key2.index];
std::cout << "index1: " << index1 << std::endl; std::cout << "index1: " << index1 << std::endl;
@ -222,7 +221,7 @@ namespace gtsam { namespace partition {
sharedInts& adjncy = *ptr_adjncy; sharedInts& adjncy = *ptr_adjncy;
sharedInts& adjwgt = *ptr_adjwgt; sharedInts& adjwgt = *ptr_adjwgt;
int ind_xadj = 0, ind_adjncy = 0; int ind_xadj = 0, ind_adjncy = 0;
BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { for(const NeighborsInfo& info: adjacencyMap) {
*(xadj.get() + ind_xadj) = ind_adjncy; *(xadj.get() + ind_xadj) = ind_adjncy;
std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
@ -334,7 +333,7 @@ namespace gtsam { namespace partition {
<< " " << result.B.size() << std::endl; << " " << result.B.size() << std::endl;
int edgeCut = 0; int edgeCut = 0;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ for(const typename GenericGraph::value_type& factor: graph){
int key1 = factor->key1.index; int key1 = factor->key1.index;
int key2 = factor->key2.index; int key2 = factor->key2.index;
// print keys and their subgraph assignment // print keys and their subgraph assignment
@ -372,19 +371,19 @@ namespace gtsam { namespace partition {
// debug functions // debug functions
void printIsland(const std::vector<size_t>& island) { void printIsland(const std::vector<size_t>& island) {
std::cout << "island: "; std::cout << "island: ";
BOOST_FOREACH(const size_t key, island) for(const size_t key: island)
std::cout << key << " "; std::cout << key << " ";
std::cout << std::endl; std::cout << std::endl;
} }
void printIslands(const std::list<std::vector<size_t> >& islands) { void printIslands(const std::list<std::vector<size_t> >& islands) {
BOOST_FOREACH(const std::vector<std::size_t>& island, islands) for(const std::vector<std::size_t>& island: islands)
printIsland(island); printIsland(island);
} }
void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) { void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
int numCamera = 0, numLandmark = 0; int numCamera = 0, numLandmark = 0;
BOOST_FOREACH(const size_t key, keys) for(const size_t key: keys)
if (int2symbol[key].chr() == 'x') if (int2symbol[key].chr() == 'x')
numCamera++; numCamera++;
else else
@ -403,16 +402,16 @@ namespace gtsam { namespace partition {
std::vector<size_t>& C = partitionResult.C; std::vector<size_t>& C = partitionResult.C;
std::vector<int>& dictionary = workspace.dictionary; std::vector<int>& dictionary = workspace.dictionary;
std::fill(dictionary.begin(), dictionary.end(), -1); std::fill(dictionary.begin(), dictionary.end(), -1);
BOOST_FOREACH(const size_t a, A) for(const size_t a: A)
dictionary[a] = 1; dictionary[a] = 1;
BOOST_FOREACH(const size_t b, B) for(const size_t b: B)
dictionary[b] = 2; dictionary[b] = 2;
if (!C.empty()) if (!C.empty())
throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
// set up landmarks // set up landmarks
size_t i,j; size_t i,j;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { for(const typename GenericGraph::value_type& factor: graph) {
i = factor->key1.index; i = factor->key1.index;
j = factor->key2.index; j = factor->key2.index;
if (dictionary[j] == 0) // if the landmark is already in the separator, continue if (dictionary[j] == 0) // if the landmark is already in the separator, continue
@ -427,7 +426,7 @@ namespace gtsam { namespace partition {
// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; // std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
} }
BOOST_FOREACH(const size_t j, landmarkKeys) { for(const size_t j: landmarkKeys) {
switch(dictionary[j]) { switch(dictionary[j]) {
case 0: C.push_back(j); break; case 0: C.push_back(j); break;
case 1: A.push_back(j); break; case 1: A.push_back(j); break;
@ -456,7 +455,7 @@ namespace gtsam { namespace partition {
// find out all the landmark keys, which are to be eliminated // find out all the landmark keys, which are to be eliminated
cameraKeys.reserve(keys.size()); cameraKeys.reserve(keys.size());
landmarkKeys.reserve(keys.size()); landmarkKeys.reserve(keys.size());
BOOST_FOREACH(const size_t key, keys) { for(const size_t key: keys) {
if((*int2symbol)[key].chr() == 'x') if((*int2symbol)[key].chr() == 'x')
cameraKeys.push_back(key); cameraKeys.push_back(key);
else else
@ -518,11 +517,11 @@ namespace gtsam { namespace partition {
// printNumCamerasLandmarks(result->C, *int2symbol); // printNumCamerasLandmarks(result->C, *int2symbol);
// std::cout << "no. of island: " << islands.size() << "; "; // std::cout << "no. of island: " << islands.size() << "; ";
// std::cout << "island size: "; // std::cout << "island size: ";
// BOOST_FOREACH(const Island& island, islands) // for(const Island& island: islands)
// std::cout << island.size() << " "; // std::cout << island.size() << " ";
// std::cout << std::endl; // std::cout << std::endl;
// BOOST_FOREACH(const Island& island, islands) { // for(const Island& island: islands) {
// printNumCamerasLandmarks(island, int2symbol); // printNumCamerasLandmarks(island, int2symbol);
// } // }
#endif #endif
@ -550,12 +549,12 @@ namespace gtsam { namespace partition {
// generate the node map // generate the node map
std::vector<int>& partitionTable = workspace.partitionTable; std::vector<int>& partitionTable = workspace.partitionTable;
std::fill(partitionTable.begin(), partitionTable.end(), -1); std::fill(partitionTable.begin(), partitionTable.end(), -1);
BOOST_FOREACH(const size_t key, result->C) for(const size_t key: result->C)
partitionTable[key] = 0; partitionTable[key] = 0;
int idx = 0; int idx = 0;
BOOST_FOREACH(const Island& island, islands) { for(const Island& island: islands) {
idx++; idx++;
BOOST_FOREACH(const size_t key, island) { for(const size_t key: island) {
partitionTable[key] = idx; partitionTable[key] = idx;
} }
} }

33
gtsam_unstable/partition/GenericGraph.cpp
View File

@ -6,7 +6,6 @@
* Description: generic graph types used in partitioning * Description: generic graph types used in partitioning
*/ */
#include <iostream> #include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
@ -98,7 +97,7 @@ namespace gtsam { namespace partition {
} }
// erase unused factors // erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase) for(const FactorList::iterator& it: toErase)
factors.erase(it); factors.erase(it);
if (!succeed) break; if (!succeed) break;
@ -107,7 +106,7 @@ namespace gtsam { namespace partition {
list<vector<size_t> > islands; list<vector<size_t> > islands;
map<size_t, vector<size_t> > arrays = dsf.arrays(); map<size_t, vector<size_t> > arrays = dsf.arrays();
size_t key; vector<size_t> array; size_t key; vector<size_t> array;
BOOST_FOREACH(boost::tie(key, array), arrays) for(boost::tie(key, array): arrays)
islands.push_back(array); islands.push_back(array);
return islands; return islands;
} }
@ -116,14 +115,14 @@ namespace gtsam { namespace partition {
/* ************************************************************************* */ /* ************************************************************************* */
void print(const GenericGraph2D& graph, const std::string name) { void print(const GenericGraph2D& graph, const std::string name) {
cout << name << endl; cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) for(const sharedGenericFactor2D& factor_: graph)
cout << factor_->key1.index << " " << factor_->key2.index << endl; cout << factor_->key1.index << " " << factor_->key2.index << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void print(const GenericGraph3D& graph, const std::string name) { void print(const GenericGraph3D& graph, const std::string name) {
cout << name << endl; cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) for(const sharedGenericFactor3D& factor_: graph)
cout << factor_->key1.index << " " << factor_->key2.index << " (" << cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
factor_->key1.type << ", " << factor_->key2.type <<")" << endl; factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
} }
@ -174,7 +173,7 @@ namespace gtsam { namespace partition {
} }
// erase unused factors // erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase) for(const FactorList::iterator& it: toErase)
factors.erase(it); factors.erase(it);
if (!succeed) break; if (!succeed) break;
@ -204,7 +203,7 @@ namespace gtsam { namespace partition {
// compute the constraint number per camera // compute the constraint number per camera
std::fill(nrConstraints.begin(), nrConstraints.end(), 0); std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { for(const sharedGenericFactor3D& factor_: graph) {
const int& key1 = factor_->key1.index; const int& key1 = factor_->key1.index;
const int& key2 = factor_->key2.index; const int& key2 = factor_->key2.index;
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
@ -258,7 +257,7 @@ namespace gtsam { namespace partition {
// check the constraint number of every variable // check the constraint number of every variable
// find the camera and landmark keys // find the camera and landmark keys
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { for(const sharedGenericFactor3D& factor_: graph) {
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
if (workspace.dictionary[factor_->key1.index] != -1) { if (workspace.dictionary[factor_->key1.index] != -1) {
if (factor_->key1.type == NODE_POSE_3D) if (factor_->key1.type == NODE_POSE_3D)
@ -287,7 +286,7 @@ namespace gtsam { namespace partition {
// add singular variables directly as islands // add singular variables directly as islands
if (!singularCameras.empty()) { if (!singularCameras.empty()) {
if (verbose) cout << "singular cameras:"; if (verbose) cout << "singular cameras:";
BOOST_FOREACH(const size_t i, singularCameras) { for(const size_t i: singularCameras) {
islands.push_back(vector<size_t>(1, i)); // <--------------------------- islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " "; if (verbose) cout << i << " ";
} }
@ -295,7 +294,7 @@ namespace gtsam { namespace partition {
} }
if (!singularLandmarks.empty()) { if (!singularLandmarks.empty()) {
if (verbose) cout << "singular landmarks:"; if (verbose) cout << "singular landmarks:";
BOOST_FOREACH(const size_t i, singularLandmarks) { for(const size_t i: singularLandmarks) {
islands.push_back(vector<size_t>(1, i)); // <--------------------------- islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " "; if (verbose) cout << i << " ";
} }
@ -306,10 +305,10 @@ namespace gtsam { namespace partition {
// regenerating islands // regenerating islands
map<size_t, vector<size_t> > labelIslands = dsf.arrays(); map<size_t, vector<size_t> > labelIslands = dsf.arrays();
size_t label; vector<size_t> island; size_t label; vector<size_t> island;
BOOST_FOREACH(boost::tie(label, island), labelIslands) { for(boost::tie(label, island): labelIslands) {
vector<size_t> filteredIsland; // remove singular cameras from array vector<size_t> filteredIsland; // remove singular cameras from array
filteredIsland.reserve(island.size()); filteredIsland.reserve(island.size());
BOOST_FOREACH(const size_t key, island) { for(const size_t key: island) {
if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular
(isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular
(!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined
@ -321,7 +320,7 @@ namespace gtsam { namespace partition {
// sanity check // sanity check
size_t nrKeys = 0; size_t nrKeys = 0;
BOOST_FOREACH(const vector<size_t>& island, islands) for(const vector<size_t>& island: islands)
nrKeys += island.size(); nrKeys += island.size();
if (nrKeys != keys.size()) { if (nrKeys != keys.size()) {
cout << nrKeys << " vs " << keys.size() << endl; cout << nrKeys << " vs " << keys.size() << endl;
@ -363,7 +362,7 @@ namespace gtsam { namespace partition {
// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
vector<int> cameraToCamera(dictionary.size(), -1); vector<int> cameraToCamera(dictionary.size(), -1);
size_t key_i, key_j; size_t key_i, key_j;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { for(const sharedGenericFactor3D& factor_: graph) {
if (factor_->key1.type == NODE_POSE_3D) { if (factor_->key1.type == NODE_POSE_3D) {
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
@ -382,7 +381,7 @@ namespace gtsam { namespace partition {
} }
// sort the landmark keys for the late getNrCommonLandmarks call // sort the landmark keys for the late getNrCommonLandmarks call
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){ for(vector<LandmarkKey> &landmarks: cameraToLandmarks){
if (!landmarks.empty()) if (!landmarks.empty())
std::sort(landmarks.begin(), landmarks.end()); std::sort(landmarks.begin(), landmarks.end());
} }
@ -417,7 +416,7 @@ namespace gtsam { namespace partition {
const vector<int>& dictionary = workspace.dictionary; const vector<int>& dictionary = workspace.dictionary;
vector<bool> isValidCamera(workspace.dictionary.size(), false); vector<bool> isValidCamera(workspace.dictionary.size(), false);
vector<bool> isValidLandmark(workspace.dictionary.size(), false); vector<bool> isValidLandmark(workspace.dictionary.size(), false);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { for(const sharedGenericFactor3D& factor_: graph) {
assert(factor_->key1.type == NODE_POSE_3D); assert(factor_->key1.type == NODE_POSE_3D);
//assert(factor_->key2.type == NODE_LANDMARK_3D); //assert(factor_->key2.type == NODE_LANDMARK_3D);
const size_t& key1 = factor_->key1.index; const size_t& key1 = factor_->key1.index;
@ -463,7 +462,7 @@ namespace gtsam { namespace partition {
} }
// debug info // debug info
BOOST_FOREACH(const size_t key, frontals) { for(const size_t key: frontals) {
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera) if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl; cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
} }

14
gtsam_unstable/partition/NestedDissection-inl.h
View File

@ -70,11 +70,11 @@ namespace gtsam { namespace partition {
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG( boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const { const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
OrderedSymbols frontalKeys; OrderedSymbols frontalKeys;
BOOST_FOREACH(const size_t index, frontals) for(const size_t index: frontals)
frontalKeys.push_back(int2symbol_[index]); frontalKeys.push_back(int2symbol_[index]);
UnorderedSymbols sepKeys; UnorderedSymbols sepKeys;
BOOST_FOREACH(const size_t index, sep) for(const size_t index: sep)
sepKeys.insert(int2symbol_[index]); sepKeys.insert(int2symbol_[index]);
return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent); return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
@ -129,7 +129,7 @@ namespace gtsam { namespace partition {
// make three lists of variables A, B, and C // make three lists of variables A, B, and C
int partition; int partition;
childFrontals.resize(numSubmaps); childFrontals.resize(numSubmaps);
BOOST_FOREACH(const size_t key, keys){ for(const size_t key: keys){
partition = partitionTable[key]; partition = partitionTable[key];
switch (partition) { switch (partition) {
case -1: break; // the separator of the separator variables case -1: break; // the separator of the separator variables
@ -144,13 +144,13 @@ namespace gtsam { namespace partition {
childSeps.reserve(numSubmaps); childSeps.reserve(numSubmaps);
frontalFactors.resize(numSubmaps); frontalFactors.resize(numSubmaps);
frontalUnaryFactors.resize(numSubmaps); frontalUnaryFactors.resize(numSubmaps);
BOOST_FOREACH(typename GenericGraph::value_type factor, fg) for(typename GenericGraph::value_type factor: fg)
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
BOOST_FOREACH(const set<size_t>& childSep, childSeps_) for(const set<size_t>& childSep: childSeps_)
childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end())); childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
// add unary factor to the current cluster or pass it to one of the child clusters // add unary factor to the current cluster or pass it to one of the child clusters
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors) {
partition = partitionTable[unaryFactor_->key.index]; partition = partitionTable[unaryFactor_->key.index];
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
else frontalUnaryFactors[partition-1].push_back(unaryFactor_); else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
@ -164,7 +164,7 @@ namespace gtsam { namespace partition {
NLG sepFactors; NLG sepFactors;
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors)
sepFactors.push_back(fg_[unaryFactor_->index]); sepFactors.push_back(fg_[unaryFactor_->index]);
return sepFactors; return sepFactors;
} }

5
gtsam_unstable/partition/tests/testFindSeparator.cpp
View File

@ -10,7 +10,6 @@
#include <boost/assign/std/set.hpp> // for operator += #include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator += #include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -89,10 +88,10 @@ TEST ( Partition, edgePartitionByMetis )
vector<size_t> A_expected; A_expected += 0, 1; // frontal vector<size_t> A_expected; A_expected += 0, 1; // frontal
vector<size_t> B_expected; B_expected += 2, 3; // frontal vector<size_t> B_expected; B_expected += 2, 3; // frontal
vector<size_t> C_expected; // separator vector<size_t> C_expected; // separator
// BOOST_FOREACH(const size_t a, actual->A) // for(const size_t a: actual->A)
// cout << a << " "; // cout << a << " ";
// cout << endl; // cout << endl;
// BOOST_FOREACH(const size_t b, actual->B) // for(const size_t b: actual->B)
// cout << b << " "; // cout << b << " ";
// cout << endl; // cout << endl;