remove all adaptors

release/4.3a0
kartik arcot 2023-01-19 15:58:17 -08:00
parent 755da00e51
commit 773d4975e6
21 changed files with 96 additions and 94 deletions

View File

@ -178,10 +178,10 @@ int main(int argc, char** argv) {
initial.insert(i, predictedPose); initial.insert(i, predictedPose);
// Check if there are range factors to be added // Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) { while (k < K && t >= std::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]); size_t j = std::get<1>(triples[k]);
Symbol landmark_key('L', j); Symbol landmark_key('L', j);
double range = boost::get<2>(triples[k]); double range = std::get<2>(triples[k]);
newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>( newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
i, landmark_key, range, rangeNoise); i, landmark_key, range, rangeNoise);
if (initializedLandmarks.count(landmark_key) == 0) { if (initializedLandmarks.count(landmark_key) == 0) {

View File

@ -49,8 +49,6 @@
#include <boost/archive/binary_oarchive.hpp> #include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp> #include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp> #include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/serialization/export.hpp>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>

View File

@ -268,5 +268,4 @@ namespace gtsam {
// traits // traits
template <> template <>
struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {}; struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {};
} // namespace gtsam } // namespace gtsam

View File

@ -58,8 +58,9 @@ DiscreteValues DiscreteBayesNet::sample() const {
DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const { DiscreteValues DiscreteBayesNet::sample(DiscreteValues result) const {
// sample each node in turn in topological sort order (parents first) // sample each node in turn in topological sort order (parents first)
for (auto conditional : boost::adaptors::reverse(*this)) for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
conditional->sampleInPlace(&result); (*it)->sampleInPlace(&result);
}
return result; return result;
} }

View File

@ -20,6 +20,7 @@
#include <gtsam/discrete/DiscreteLookupDAG.h> #include <gtsam/discrete/DiscreteLookupDAG.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <iterator>
#include <string> #include <string>
#include <utility> #include <utility>
@ -118,8 +119,10 @@ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet(
DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const { DiscreteValues DiscreteLookupDAG::argmax(DiscreteValues result) const {
// Argmax each node in turn in topological sort order (parents first). // Argmax each node in turn in topological sort order (parents first).
for (auto lookupTable : boost::adaptors::reverse(*this)) for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
lookupTable->argmaxInPlace(&result); // dereference to get the sharedFactor to the lookup table
(*it)->argmaxInPlace(&result);
}
return result; return result;
} }
/* ************************************************************************** */ /* ************************************************************************** */

View File

@ -56,7 +56,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
os << "\n"; os << "\n";
// Reverse order as typically Bayes nets stored in reverse topological sort. // Reverse order as typically Bayes nets stored in reverse topological sort.
for (auto conditional : boost::adaptors::reverse(*this)) { for (auto it = std::make_reverse_iterator(this->end());
it != std::make_reverse_iterator(this->begin()); ++it) {
const auto& conditional = *it;
auto frontals = conditional->frontals(); auto frontals = conditional->frontals();
const Key me = frontals.front(); const Key me = frontals.front();
auto parents = conditional->parents(); auto parents = conditional->parents();

View File

@ -20,8 +20,8 @@
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/range/adaptor/reversed.hpp>
#include <fstream> #include <fstream>
#include <iterator>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -50,11 +50,11 @@ namespace gtsam {
VectorValues solution = given; VectorValues solution = given;
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
// solve each node in reverse topological sort order (parents first) // solve each node in reverse topological sort order (parents first)
for (auto cg : boost::adaptors::reverse(*this)) { for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
// i^th part of R*x=y, x=inv(R)*y // i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi => // (Rii*xi + R_i*x(i+1:))./si = yi =>
// xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) // xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
solution.insert(cg->solve(solution)); solution.insert((*it)->solve(solution));
} }
return solution; return solution;
} }
@ -69,8 +69,8 @@ namespace gtsam {
std::mt19937_64* rng) const { std::mt19937_64* rng) const {
VectorValues result(given); VectorValues result(given);
// sample each node in reverse topological sort order (parents first) // sample each node in reverse topological sort order (parents first)
for (auto cg : boost::adaptors::reverse(*this)) { for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
const VectorValues sampled = cg->sample(result, rng); const VectorValues sampled = (*it)->sample(result, rng);
result.insert(sampled); result.insert(sampled);
} }
return result; return result;
@ -131,8 +131,8 @@ namespace gtsam {
VectorValues result; VectorValues result;
// TODO this looks pretty sketchy. result is passed as the parents argument // TODO this looks pretty sketchy. result is passed as the parents argument
// as it's filled up by solving the gaussian conditionals. // as it's filled up by solving the gaussian conditionals.
for (auto cg: boost::adaptors::reverse(*this)) { for (auto it = std::make_reverse_iterator(end()); it != std::make_reverse_iterator(begin()); ++it) {
result.insert(cg->solveOtherRHS(result, rhs)); result.insert((*it)->solveOtherRHS(result, rhs));
} }
return result; return result;
} }

View File

@ -31,18 +31,12 @@
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <sstream> #include <sstream>
#include <limits> #include <limits>
#include "gtsam/base/Vector.h"
using namespace std; using namespace std;
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
}
namespace gtsam { namespace gtsam {
@ -144,12 +138,20 @@ namespace {
DenseIndex _getSizeHF(const Vector& m) { DenseIndex _getSizeHF(const Vector& m) {
return m.size(); return m.size();
} }
std::vector<DenseIndex> _getSizeHFVec(const std::vector<Vector>& m) {
std::vector<DenseIndex> dims;
for (const Vector& v : m) {
dims.push_back(v.size());
}
return dims;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const KeyVector& js, HessianFactor::HessianFactor(const KeyVector& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) : const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { GaussianFactor(js), info_(_getSizeHFVec(gs), true) {
// Get the number of variables // Get the number of variables
size_t variable_count = js.size(); size_t variable_count = js.size();

View File

@ -32,10 +32,6 @@
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/array.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/adaptor/map.hpp>
#include <cmath> #include <cmath>
#include <sstream> #include <sstream>
@ -227,10 +223,10 @@ void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
gttic(allocate); gttic(allocate);
Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size()); Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Copy keys in order
// Get variable keys std::transform(orderedSlots.begin(), orderedSlots.end(),
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin(),
Base::keys_.begin()); [](const VariableSlots::const_iterator& it) {return it->first;});
gttoc(allocate); gttoc(allocate);
// Loop over slots in combined factor and copy blocks from source factors // Loop over slots in combined factor and copy blocks from source factors

View File

@ -25,7 +25,6 @@
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <stdexcept> #include <stdexcept>
@ -205,7 +204,8 @@ void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
assert(x.size() == y.size()); assert(x.size() == y.size());
/* back substitute */ /* back substitute */
for (const auto &cg : boost::adaptors::reverse(Rc1_)) { for (auto it = std::make_reverse_iterator(Rc1_.end()); it != std::make_reverse_iterator(Rc1_.begin()); ++it) {
auto& cg = *it;
/* collect a subvector of x that consists of the parents of cg (S) */ /* collect a subvector of x that consists of the parents of cg (S) */
const KeyVector parentKeys(cg->beginParents(), cg->endParents()); const KeyVector parentKeys(cg->beginParents(), cg->endParents());
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());

View File

@ -20,16 +20,11 @@
#include <boost/bind/bind.hpp> #include <boost/bind/bind.hpp>
#include <boost/range/numeric.hpp> #include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
using boost::adaptors::transformed;
using boost::accumulate;
/* ************************************************************************ */ /* ************************************************************************ */
VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
{ {

View File

@ -26,7 +26,6 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/range/iterator_range.hpp> #include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -131,7 +130,11 @@ TEST(JacobianFactor, constructors_and_accessors)
blockMatrix(1) = terms[1].second; blockMatrix(1) = terms[1].second;
blockMatrix(2) = terms[2].second; blockMatrix(2) = terms[2].second;
blockMatrix(3) = b; blockMatrix(3) = b;
JacobianFactor actual(terms | boost::adaptors::map_keys, blockMatrix, noise); // get a vector of keys from the terms
vector<Key> keys;
for (const auto& term : terms)
keys.push_back(term.first);
JacobianFactor actual(keys, blockMatrix, noise);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));

View File

@ -21,12 +21,9 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/range/adaptor/map.hpp>
#include <sstream> #include <sstream>
using namespace std; using namespace std;
using boost::adaptors::map_keys;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -28,13 +28,6 @@
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h> #include <gtsam/linear/GaussianEliminationTree.h>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/copy.hpp>
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
} // namespace br
#include <algorithm> #include <algorithm>
#include <limits> #include <limits>
#include <string> #include <string>

View File

@ -176,9 +176,11 @@ void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams,
gttic(recalculateBatch); gttic(recalculateBatch);
gttic(add_keys); gttic(add_keys);
br::copy(variableIndex_ | br::map_keys,
std::inserter(*affectedKeysSet, affectedKeysSet->end()));
// copy the keys from the variableIndex_ to the affectedKeysSet
for (const auto& [key, _] : variableIndex_) {
affectedKeysSet->insert(key);
}
// Removed unused keys: // Removed unused keys:
VariableIndex affectedFactorsVarIndex = variableIndex_; VariableIndex affectedFactorsVarIndex = variableIndex_;

View File

@ -22,11 +22,10 @@
#include <gtsam/symbolic/SymbolicBayesTree.h> #include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h> #include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
#include <boost/range/adaptor/indirected.hpp>
using boost::adaptors::indirected;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <iterator>
#include <type_traits>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -34,6 +33,24 @@ using namespace gtsam::symbol_shorthand;
static bool debug = false; static bool debug = false;
// Given a vector of shared pointers infer the type of the pointed-to objects
template<typename T>
using PointedToType = std::decay_t<decltype(**declval<T>().begin())>;
// Given a vector of shared pointers infer the type of the pointed-to objects
template<typename T>
using ValuesVector = std::vector<PointedToType<T>>;
// Return a vector of dereferenced values
template<typename T>
ValuesVector<T> deref(const T& v) {
ValuesVector<T> result;
for (auto& t : v)
result.push_back(*t);
return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesTree, clear) { TEST(SymbolicBayesTree, clear) {
SymbolicBayesTree bayesTree = asiaBayesTree; SymbolicBayesTree bayesTree = asiaBayesTree;
@ -111,8 +128,7 @@ TEST(BayesTree, removePath) {
bayesTree.removePath(bayesTree[_C_], &bn, &orphans); bayesTree.removePath(bayesTree[_C_], &bn, &orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
CHECK(assert_container_equal(expectedOrphans | indirected, CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
orphans | indirected));
bayesTree = bayesTreeOrig; bayesTree = bayesTreeOrig;
@ -127,8 +143,7 @@ TEST(BayesTree, removePath) {
bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2); bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph factors2(bn2);
CHECK(assert_equal(expected2, factors2)); CHECK(assert_equal(expected2, factors2));
CHECK(assert_container_equal(expectedOrphans2 | indirected, CHECK(assert_container_equal(deref(expectedOrphans2), deref(orphans2)));
orphans2 | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -147,8 +162,7 @@ TEST(BayesTree, removePath2) {
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_T_], SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_T_],
bayesTree[_X_]}; bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans | indirected, CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
orphans | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -167,8 +181,7 @@ TEST(BayesTree, removePath3) {
expected.emplace_shared<SymbolicFactor>(_T_, _E_, _L_); expected.emplace_shared<SymbolicFactor>(_T_, _E_, _L_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]}; SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans | indirected, CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
orphans | indirected));
} }
void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, void getAllCliques(const SymbolicBayesTree::sharedClique& subtree,
@ -249,8 +262,7 @@ TEST(BayesTree, removeTop) {
CHECK(assert_equal(expected, bn)); CHECK(assert_equal(expected, bn));
SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]}; SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans | indirected, CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
orphans | indirected));
// Try removeTop again with a factor that should not change a thing // Try removeTop again with a factor that should not change a thing
// std::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_)); // std::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
@ -261,8 +273,7 @@ TEST(BayesTree, removeTop) {
SymbolicFactorGraph expected2; SymbolicFactorGraph expected2;
CHECK(assert_equal(expected2, factors2)); CHECK(assert_equal(expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2; SymbolicBayesTree::Cliques expectedOrphans2;
CHECK(assert_container_equal(expectedOrphans2 | indirected, CHECK(assert_container_equal(deref(expectedOrphans2), deref(orphans2)));
orphans2 | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -286,8 +297,7 @@ TEST(BayesTree, removeTop2) {
CHECK(assert_equal(expected, bn)); CHECK(assert_equal(expected, bn));
SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]}; SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_S_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans | indirected, CHECK(assert_container_equal(deref(expectedOrphans), deref(orphans)));
orphans | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -11,7 +11,6 @@
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <boost/range/adaptor/map.hpp>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
@ -47,7 +46,7 @@ class LoopyBelief {
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
cout << s << ":" << endl; cout << s << ":" << endl;
star->print("Star graph: "); star->print("Star graph: ");
for (Key key : correctedBeliefIndices | boost::adaptors::map_keys) { for (const auto& [key, _] : correctedBeliefIndices) {
cout << "Belief factor index for " << key << ": " cout << "Belief factor index for " << key << ": "
<< correctedBeliefIndices.at(key) << endl; << correctedBeliefIndices.at(key) << endl;
} }
@ -71,7 +70,7 @@ class LoopyBelief {
/// print /// print
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
cout << s << ":" << endl; cout << s << ":" << endl;
for (Key key : starGraphs_ | boost::adaptors::map_keys) { for (const auto& [key, _] : starGraphs_) {
starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); starGraphs_.at(key).print((boost::format("Node %d:") % key).str());
} }
} }
@ -85,7 +84,7 @@ class LoopyBelief {
DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph());
std::map<Key, std::map<Key, DiscreteFactor::shared_ptr> > allMessages; std::map<Key, std::map<Key, DiscreteFactor::shared_ptr> > allMessages;
// Eliminate each star graph // Eliminate each star graph
for (Key key : starGraphs_ | boost::adaptors::map_keys) { for (const auto& [key, _] : starGraphs_) {
// cout << "***** Node " << key << "*****" << endl; // cout << "***** Node " << key << "*****" << endl;
// initialize belief to the unary factor from the original graph // initialize belief to the unary factor from the original graph
DecisionTreeFactor::shared_ptr beliefAtKey; DecisionTreeFactor::shared_ptr beliefAtKey;
@ -94,8 +93,7 @@ class LoopyBelief {
std::map<Key, DiscreteFactor::shared_ptr> messages; std::map<Key, DiscreteFactor::shared_ptr> messages;
// eliminate each neighbor in this star graph one by one // eliminate each neighbor in this star graph one by one
for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | for (const auto& [neighbor, _] : starGraphs_.at(key).correctedBeliefIndices) {
boost::adaptors::map_keys) {
DiscreteFactorGraph subGraph; DiscreteFactorGraph subGraph;
for (size_t factor : starGraphs_.at(key).varIndex_[neighbor]) { for (size_t factor : starGraphs_.at(key).varIndex_[neighbor]) {
subGraph.push_back(starGraphs_.at(key).star->at(factor)); subGraph.push_back(starGraphs_.at(key).star->at(factor));
@ -143,11 +141,10 @@ class LoopyBelief {
// Update corrected beliefs // Update corrected beliefs
VariableIndex beliefFactors(*beliefs); VariableIndex beliefFactors(*beliefs);
for (Key key : starGraphs_ | boost::adaptors::map_keys) { for (const auto& [key, _] : starGraphs_) {
std::map<Key, DiscreteFactor::shared_ptr> messages = allMessages[key]; std::map<Key, DiscreteFactor::shared_ptr> messages = allMessages[key];
for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | for (const auto& [neighbor, _] : starGraphs_.at(key).correctedBeliefIndices) {
boost::adaptors::map_keys) { DecisionTreeFactor correctedBelief =
DecisionTreeFactor correctedBelief =
(*std::dynamic_pointer_cast<DecisionTreeFactor>( (*std::dynamic_pointer_cast<DecisionTreeFactor>(
beliefs->at(beliefFactors[key].front()))) / beliefs->at(beliefFactors[key].front()))) /
(*std::dynamic_pointer_cast<DecisionTreeFactor>( (*std::dynamic_pointer_cast<DecisionTreeFactor>(
@ -175,7 +172,7 @@ class LoopyBelief {
const std::map<Key, DiscreteKey>& allDiscreteKeys) const { const std::map<Key, DiscreteKey>& allDiscreteKeys) const {
StarGraphs starGraphs; StarGraphs starGraphs;
VariableIndex varIndex(graph); ///< access to all factors of each node VariableIndex varIndex(graph); ///< access to all factors of each node
for (Key key : varIndex | boost::adaptors::map_keys) { for (const auto& [key, _] : varIndex) {
// initialize to multiply with other unary factors later // initialize to multiply with other unary factors later
DecisionTreeFactor::shared_ptr prodOfUnaries; DecisionTreeFactor::shared_ptr prodOfUnaries;

View File

@ -179,9 +179,9 @@ int main(int argc, char** argv) {
landmarkEstimates.insert(i, predictedPose); landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added // Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) { while (k < K && t >= std::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]); size_t j = std::get<1>(triples[k]);
double range = boost::get<2>(triples[k]); double range = std::get<2>(triples[k]);
if (i > start) { if (i > start) {
if (smart && totalCount < minK) { if (smart && totalCount < minK) {
try { try {

View File

@ -160,9 +160,9 @@ int main(int argc, char** argv) {
landmarkEstimates.insert(i, predictedPose); landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added // Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) { while (k < K && t >= std::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]); size_t j = std::get<1>(triples[k]);
double range = boost::get<2>(triples[k]); double range = std::get<2>(triples[k]);
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise); RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise);
// Throw out obvious outliers based on current landmark estimates // Throw out obvious outliers based on current landmark estimates
Vector error = factor.unwhitenedError(landmarkEstimates); Vector error = factor.unwhitenedError(landmarkEstimates);

View File

@ -66,7 +66,7 @@ GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const {
// create factor ||x||^2 and add to the graph // create factor ||x||^2 and add to the graph
const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap(); const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
for (Key key : constrainedKeyDim | boost::adaptors::map_keys) { for (const auto& [key, _] : constrainedKeyDim) {
size_t dim = constrainedKeyDim.at(key); size_t dim = constrainedKeyDim.at(key);
initGraph->push_back( initGraph->push_back(
JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
@ -107,4 +107,4 @@ InequalityFactorGraph LPInitSolver::addSlackVariableToInequalities(
return slackInequalities; return slackInequalities;
} }
} }

View File

@ -27,7 +27,6 @@
#include <boost/archive/binary_oarchive.hpp> #include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp> #include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp> #include <boost/serialization/export.hpp>
#include <boost/range/adaptor/reversed.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -225,9 +224,14 @@ int main(int argc, char *argv[]) {
try { try {
Marginals marginals(graph, values); Marginals marginals(graph, values);
int i=0; int i=0;
for (Key key1: boost::adaptors::reverse(values.keys())) { // Assign the keyvector to a named variable
auto keys = values.keys();
// Iterate over it in reverse
for (auto it1 = keys.rbegin(); it1 != keys.rend(); ++it1) {
Key key1 = *it1;
int j=0; int j=0;
for (Key key2: boost::adaptors::reverse(values.keys())) { for (auto it2 = keys.rbegin(); it2 != keys.rend(); ++it2) {
Key key2 = *it2;
if(i != j) { if(i != j) {
gttic_(jointMarginalInformation); gttic_(jointMarginalInformation);
KeyVector keys(2); KeyVector keys(2);