remove all adaptors
parent
755da00e51
commit
773d4975e6
|
@ -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) {
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
/* ************************************************************************** */
|
/* ************************************************************************** */
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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());
|
||||||
|
|
|
@ -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)
|
||||||
{
|
{
|
||||||
|
|
|
@ -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)));
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
||||||
|
|
|
@ -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));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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 {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
|
|
Loading…
Reference in New Issue