prefer auto in range for loops
parent
285ebd7dbd
commit
4fb718a943
8
gtsam.h
8
gtsam.h
|
@ -220,9 +220,9 @@ class FactorIndexSet {
|
||||||
void clear();
|
void clear();
|
||||||
|
|
||||||
// structure specific methods
|
// structure specific methods
|
||||||
void insert(size_t factorIdx);
|
void insert(size_t factorIndex);
|
||||||
bool erase(size_t factorIdx); // returns true if value was removed
|
bool erase(size_t factorIndex); // returns true if value was removed
|
||||||
bool count(size_t factorIdx) const; // returns true if value exists
|
bool count(size_t factorIndex) const; // returns true if value exists
|
||||||
};
|
};
|
||||||
|
|
||||||
// Actually a vector<FactorIndex>
|
// Actually a vector<FactorIndex>
|
||||||
|
@ -239,7 +239,7 @@ class FactorIndices {
|
||||||
size_t at(size_t i) const;
|
size_t at(size_t i) const;
|
||||||
size_t front() const;
|
size_t front() const;
|
||||||
size_t back() const;
|
size_t back() const;
|
||||||
void push_back(size_t factorIdx) const;
|
void push_back(size_t factorIndex) const;
|
||||||
};
|
};
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
// base
|
// base
|
||||||
|
|
|
@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
|
||||||
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
||||||
for(KeyMap::value_type key_factors: index_) {
|
for(KeyMap::value_type key_factors: index_) {
|
||||||
cout << "var " << keyFormatter(key_factors.first) << ":";
|
cout << "var " << keyFormatter(key_factors.first) << ":";
|
||||||
for(const FactorIndex factor: key_factors.second)
|
for(const auto index: key_factors.second)
|
||||||
cout << " " << factor;
|
cout << " " << index;
|
||||||
cout << "\n";
|
cout << "\n";
|
||||||
}
|
}
|
||||||
cout.flush();
|
cout.flush();
|
||||||
|
@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
|
||||||
// run over variables, which will be hyper-edges.
|
// run over variables, which will be hyper-edges.
|
||||||
for(KeyMap::value_type key_factors: index_) {
|
for(KeyMap::value_type key_factors: index_) {
|
||||||
// every variable is a hyper-edge covering its factors
|
// every variable is a hyper-edge covering its factors
|
||||||
for(const FactorIndex factor: key_factors.second)
|
for(const auto index: key_factors.second)
|
||||||
os << (factor+1) << " "; // base 1
|
os << (index+1) << " "; // base 1
|
||||||
os << "\n";
|
os << "\n";
|
||||||
}
|
}
|
||||||
os << flush;
|
os << flush;
|
||||||
|
|
|
@ -113,7 +113,7 @@ FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const {
|
||||||
}
|
}
|
||||||
if (debug) cout << "Affected factors are: ";
|
if (debug) cout << "Affected factors are: ";
|
||||||
if (debug) {
|
if (debug) {
|
||||||
for (const FactorIndex index : indices) {
|
for (const auto index : indices) {
|
||||||
cout << index << " ";
|
cout << index << " ";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -586,7 +586,7 @@ ISAM2Result ISAM2::update(
|
||||||
// Remove the removed factors
|
// Remove the removed factors
|
||||||
NonlinearFactorGraph removeFactors;
|
NonlinearFactorGraph removeFactors;
|
||||||
removeFactors.reserve(removeFactorIndices.size());
|
removeFactors.reserve(removeFactorIndices.size());
|
||||||
for (FactorIndex index : removeFactorIndices) {
|
for (const auto index : removeFactorIndices) {
|
||||||
removeFactors.push_back(nonlinearFactors_[index]);
|
removeFactors.push_back(nonlinearFactors_[index]);
|
||||||
nonlinearFactors_.remove(index);
|
nonlinearFactors_.remove(index);
|
||||||
if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
|
if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
|
||||||
|
@ -934,8 +934,8 @@ void ISAM2::marginalizeLeaves(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Create factor graph from factor indices
|
// Create factor graph from factor indices
|
||||||
for (FactorIndex i : factorsFromMarginalizedInClique_step1) {
|
for (const auto index: factorsFromMarginalizedInClique_step1) {
|
||||||
graph.push_back(nonlinearFactors_[i]->linearize(theta_));
|
graph.push_back(nonlinearFactors_[index]->linearize(theta_));
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reeliminate the linear graph to get the marginal and discard the
|
// Reeliminate the linear graph to get the marginal and discard the
|
||||||
|
@ -1008,10 +1008,10 @@ void ISAM2::marginalizeLeaves(
|
||||||
// Remove the factors to remove that have been summarized in the newly-added
|
// Remove the factors to remove that have been summarized in the newly-added
|
||||||
// marginal factors
|
// marginal factors
|
||||||
NonlinearFactorGraph removedFactors;
|
NonlinearFactorGraph removedFactors;
|
||||||
for (FactorIndex i : factorIndicesToRemove) {
|
for (const auto index: factorIndicesToRemove) {
|
||||||
removedFactors.push_back(nonlinearFactors_[i]);
|
removedFactors.push_back(nonlinearFactors_[index]);
|
||||||
nonlinearFactors_.remove(i);
|
nonlinearFactors_.remove(index);
|
||||||
if (params_.cacheLinearizedFactors) linearFactors_.remove(i);
|
if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
|
||||||
}
|
}
|
||||||
variableIndex_.remove(factorIndicesToRemove.begin(),
|
variableIndex_.remove(factorIndicesToRemove.begin(),
|
||||||
factorIndicesToRemove.end(), removedFactors);
|
factorIndicesToRemove.end(), removedFactors);
|
||||||
|
|
|
@ -175,19 +175,19 @@ private:
|
||||||
|
|
||||||
// collect all factors involving this key in the original graph
|
// collect all factors involving this key in the original graph
|
||||||
DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
|
DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
|
||||||
for(size_t factorIdx: varIndex[key]) {
|
for(size_t factorIndex: varIndex[key]) {
|
||||||
star->push_back(graph.at(factorIdx));
|
star->push_back(graph.at(factorIndex));
|
||||||
|
|
||||||
// accumulate unary factors
|
// accumulate unary factors
|
||||||
if (graph.at(factorIdx)->size() == 1) {
|
if (graph.at(factorIndex)->size() == 1) {
|
||||||
if (!prodOfUnaries)
|
if (!prodOfUnaries)
|
||||||
prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>(
|
prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>(
|
||||||
graph.at(factorIdx));
|
graph.at(factorIndex));
|
||||||
else
|
else
|
||||||
prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
|
prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
|
||||||
*prodOfUnaries
|
*prodOfUnaries
|
||||||
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
|
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
|
||||||
graph.at(factorIdx))));
|
graph.at(factorIndex))));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue