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

Split off builder, now also used with same parameters in SubGraphSolver

release/4.3a0
Frank Dellaert 2019-04-10 22:30:21 -04:00
parent d0a73b42fb
commit b6abcb04f2
9 changed files with 856 additions and 743 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

545
gtsam/linear/SubgraphBuilder.cpp Normal file
View File

@ -0,0 +1,545 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SubgraphBuilder.cpp
* @date Dec 31, 2009
* @author Frank Dellaert, Yong-Dian Jian
*/
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphBuilder.h>
#include <boost/algorithm/string.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <numeric> // accumulate
#include <queue>
#include <set>
#include <stdexcept>
#include <vector>
using std::cout;
using std::endl;
using std::ostream;
using std::vector;
namespace gtsam {
/*****************************************************************************/
/* sort the container and return permutation index with default comparator */
template <typename Container>
static vector<size_t> sort_idx(const Container &src) {
typedef typename Container::value_type T;
const size_t n = src.size();
vector<std::pair<size_t, T> > tmp;
tmp.reserve(n);
for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
/* sort */
std::stable_sort(tmp.begin(), tmp.end());
/* copy back */
vector<size_t> idx;
idx.reserve(n);
for (size_t i = 0; i < n; i++) {
idx.push_back(tmp[i].first);
}
return idx;
}
/*****************************************************************************/
static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
/* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
if (sum == 0.0) {
throw std::runtime_error("Weight vector has no non-zero weights");
}
/* make a normalized and accumulated version of the weight vector */
const size_t m = weight.size();
vector<double> cdf;
cdf.reserve(m);
for (size_t i = 0; i < m; ++i) {
cdf.push_back(weight[i] / sum);
}
vector<double> acc(m);
std::partial_sum(cdf.begin(), cdf.end(), acc.begin());
/* iid sample n times */
vector<size_t> samples;
samples.reserve(n);
const double denominator = (double)RAND_MAX;
for (size_t i = 0; i < n; ++i) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
const auto it = std::lower_bound(acc.begin(), acc.end(), value);
const size_t index = it - acc.begin();
samples.push_back(index);
}
return samples;
}
/*****************************************************************************/
static vector<size_t> UniqueSampler(const vector<double> &weight,
const size_t n) {
const size_t m = weight.size();
if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size");
vector<size_t> samples;
size_t count = 0;
vector<bool> touched(m, false);
while (count < n) {
vector<size_t> localIndices;
localIndices.reserve(n - count);
vector<double> localWeights;
localWeights.reserve(n - count);
/* collect data */
for (size_t i = 0; i < m; ++i) {
if (!touched[i]) {
localIndices.push_back(i);
localWeights.push_back(weight[i]);
}
}
/* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n - count);
for (const size_t &index : samples) {
if (touched[index] == false) {
touched[index] = true;
samples.push_back(index);
if (++count >= n) break;
}
}
}
return samples;
}
/****************************************************************************/
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
for (const size_t &index : indices) {
const Edge e {index,1.0};
edges_.push_back(e);
}
}
/****************************************************************************/
vector<size_t> Subgraph::edgeIndices() const {
vector<size_t> eid;
eid.reserve(size());
for (const Edge &edge : edges_) {
eid.push_back(edge.index);
}
return eid;
}
/****************************************************************************/
void Subgraph::save(const std::string &fn) const {
std::ofstream os(fn.c_str());
boost::archive::text_oarchive oa(os);
oa << *this;
os.close();
}
/****************************************************************************/
Subgraph Subgraph::load(const std::string &fn) {
std::ifstream is(fn.c_str());
boost::archive::text_iarchive ia(is);
Subgraph subgraph;
ia >> subgraph;
is.close();
return subgraph;
}
/****************************************************************************/
ostream &operator<<(ostream &os, const Subgraph::Edge &edge) {
if (edge.weight != 1.0)
os << edge.index << "(" << std::setprecision(2) << edge.weight << ")";
else
os << edge.index;
return os;
}
/****************************************************************************/
ostream &operator<<(ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
for (const auto &e : subgraph.edges()) {
os << e << ", ";
}
return os;
}
/*****************************************************************************/
void SubgraphBuilderParameters::print() const { print(cout); }
/***************************************************************************************/
void SubgraphBuilderParameters::print(ostream &os) const {
os << "SubgraphBuilderParameters" << endl
<< "skeleton: " << skeletonTranslator(skeletonType) << endl
<< "skeleton weight: " << skeletonWeightTranslator(skeletonWeight)
<< endl
<< "augmentation weight: "
<< augmentationWeightTranslator(augmentationWeight) << endl;
}
/*****************************************************************************/
ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
SubgraphBuilderParameters::Skeleton
SubgraphBuilderParameters::skeletonTranslator(const std::string &src) {
std::string s = src;
boost::algorithm::to_upper(s);
if (s == "NATURALCHAIN")
return NATURALCHAIN;
else if (s == "BFS")
return BFS;
else if (s == "KRUSKAL")
return KRUSKAL;
throw std::invalid_argument(
"SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
if (s == NATURALCHAIN)
return "NATURALCHAIN";
else if (s == BFS)
return "BFS";
else if (s == KRUSKAL)
return "KRUSKAL";
else
return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::SkeletonWeight
SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
std::string s = src;
boost::algorithm::to_upper(s);
if (s == "EQUAL")
return EQUAL;
else if (s == "RHS")
return RHS_2NORM;
else if (s == "LHS")
return LHS_FNORM;
else if (s == "RANDOM")
return RANDOM;
throw std::invalid_argument(
"SubgraphBuilderParameters::skeletonWeightTranslator undefined string " +
s);
return EQUAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonWeightTranslator(
SkeletonWeight w) {
if (w == EQUAL)
return "EQUAL";
else if (w == RHS_2NORM)
return "RHS";
else if (w == LHS_FNORM)
return "LHS";
else if (w == RANDOM)
return "RANDOM";
else
return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight
SubgraphBuilderParameters::augmentationWeightTranslator(
const std::string &src) {
std::string s = src;
boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
throw std::invalid_argument(
"SubgraphBuilder::Parameters::augmentationWeightTranslator undefined "
"string " +
s);
return SKELETON;
}
/****************************************************************/
std::string SubgraphBuilderParameters::augmentationWeightTranslator(
AugmentationWeight w) {
if (w == SKELETON) return "SKELETON";
// else if ( w == STRETCH ) return "STRETCH";
// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
else
return "UNKNOWN";
}
/****************************************************************/
vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeletonType) {
case SubgraphBuilderParameters::NATURALCHAIN:
return natural_chain(gfg);
break;
case SubgraphBuilderParameters::BFS:
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, weights);
break;
default:
std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
vector<size_t> unaryFactorIndices;
size_t index = 0;
for (const auto &factor : gfg) {
if (factor->size() == 1) {
unaryFactorIndices.push_back(index);
}
index++;
}
return unaryFactorIndices;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::natural_chain(
const GaussianFactorGraph &gfg) const {
vector<size_t> chainFactorIndices;
size_t index = 0;
for (const GaussianFactor::shared_ptr &gf : gfg) {
if (gf->size() == 2) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index);
}
index++;
}
return chainFactorIndices;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0];
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
vector<size_t> bfsFactorIndices;
bfsFactorIndices.reserve(n - 1);
/* Initialize */
std::queue<size_t> q;
q.push(seed);
std::set<size_t> flags;
flags.insert(seed);
/* traversal */
while (!q.empty()) {
const size_t head = q.front();
q.pop();
for (const size_t index : variableIndex[head]) {
const GaussianFactor &gf = *gfg[index];
for (const size_t key : gf.keys()) {
if (flags.count(key) == 0) {
q.push(key);
flags.insert(key);
bfsFactorIndices.push_back(index);
}
}
}
}
return bfsFactorIndices;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> sortedIndices = sort_idx(weights);
/* initialize buffer */
vector<size_t> treeIndices;
treeIndices.reserve(n - 1);
// container for acsendingly sorted edges
DSFVector dsf(n);
size_t count = 0;
double sum = 0.0;
for (const size_t index : sortedIndices) {
const GaussianFactor &gf = *gfg[index];
const auto keys = gf.keys();
if (keys.size() != 2) continue;
const size_t u = ordering.find(keys[0])->second,
v = ordering.find(keys[1])->second;
if (dsf.find(u) != dsf.find(v)) {
dsf.merge(u, v);
treeIndices.push_back(index);
sum += weights[index];
if (++count == n - 1) break;
}
}
return treeIndices;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::sample(const vector<double> &weights,
const size_t t) const {
return UniqueSampler(weights, t);
}
/****************************************************************/
Subgraph SubgraphBuilder::operator()(
const GaussianFactorGraph &gfg) const {
const auto &p = parameters_;
const auto inverse_ordering = Ordering::Natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), m = gfg.size();
// Make sure the subgraph preconditioner does not include more than half of
// the edges beyond the spanning tree, or we might as well solve the whole
// thing.
size_t numExtraEdges = n * p.augmentationFactor;
const size_t numRemaining = m - (n - 1);
numExtraEdges = std::min(numExtraEdges, numRemaining / 2);
// Calculate weights
vector<double> weights = this->weights(gfg);
// Build spanning tree.
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
if (tree.size() != n - 1) {
throw std::runtime_error(
"SubgraphBuilder::operator() failure: tree.size() != n-1");
}
// Downweight the tree edges to zero.
for (const size_t index : tree) {
weights[index] = 0.0;
}
/* decide how many edges to augment */
vector<size_t> offTree = sample(weights, numExtraEdges);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
return Subgraph(subgraph);
}
/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(
const GaussianFactorGraph &gfg) const {
const size_t m = gfg.size();
Weights weight;
weight.reserve(m);
for (const GaussianFactor::shared_ptr &gf : gfg) {
switch (parameters_.skeletonWeight) {
case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0);
break;
case SubgraphBuilderParameters::RHS_2NORM: {
if (JacobianFactor::shared_ptr jf =
boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
weight.push_back(jf->getb().norm());
} else if (HessianFactor::shared_ptr hf =
boost::dynamic_pointer_cast<HessianFactor>(gf)) {
weight.push_back(hf->linearTerm().norm());
}
} break;
case SubgraphBuilderParameters::LHS_FNORM: {
if (JacobianFactor::shared_ptr jf =
boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
} else if (HessianFactor::shared_ptr hf =
boost::dynamic_pointer_cast<HessianFactor>(gf)) {
weight.push_back(std::sqrt(hf->information().squaredNorm()));
}
} break;
case SubgraphBuilderParameters::RANDOM:
weight.push_back(std::rand() % 100 + 1.0);
break;
default:
throw std::invalid_argument(
"SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
return weight;
}
/*****************************************************************************/
GaussianFactorGraph::shared_ptr buildFactorSubgraph(
const GaussianFactorGraph &gfg, const Subgraph &subgraph,
const bool clone) {
auto subgraphFactors = boost::make_shared<GaussianFactorGraph>();
subgraphFactors->reserve(subgraph.size());
for (const auto &e : subgraph) {
const auto factor = gfg[e.index];
subgraphFactors->push_back(clone ? factor->clone(): factor);
}
return subgraphFactors;
}
/**************************************************************************************************/
std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
// Get the subgraph by calling cheaper method
auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
// Now, copy all factors then set subGraph factors to zero
auto remaining = boost::make_shared<GaussianFactorGraph>(factorGraph);
for (const auto &e : subgraph) {
remaining->remove(e.index);
}
return std::make_pair(subgraphFactors, remaining);
}
/*****************************************************************************/
} // namespace gtsam

182
gtsam/linear/SubgraphBuilder.h Normal file
View File

@ -0,0 +1,182 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SubgraphBuilder.h
* @date Dec 31, 2009
* @author Frank Dellaert, Yong-Dian Jian
*/
#pragma once
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
struct PreconditionerParameters;
/**************************************************************************/
class GTSAM_EXPORT Subgraph {
public:
struct GTSAM_EXPORT Edge {
size_t index; /* edge id */
double weight; /* edge weight */
inline bool isUnitWeight() const { return (weight == 1.0); }
friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_NVP(index);
ar &BOOST_SERIALIZATION_NVP(weight);
}
};
typedef std::vector<Edge> Edges;
typedef std::vector<size_t> EdgeIndices;
typedef Edges::iterator iterator;
typedef Edges::const_iterator const_iterator;
protected:
Edges edges_; /* index to the factors */
public:
Subgraph() {}
Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
Subgraph(const Edges &edges) : edges_(edges) {}
Subgraph(const std::vector<size_t> &indices);
inline const Edges &edges() const { return edges_; }
inline size_t size() const { return edges_.size(); }
EdgeIndices edgeIndices() const;
iterator begin() { return edges_.begin(); }
const_iterator begin() const { return edges_.begin(); }
iterator end() { return edges_.end(); }
const_iterator end() const { return edges_.end(); }
void save(const std::string &fn) const;
static Subgraph load(const std::string &fn);
friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_NVP(edges_);
}
};
/****************************************************************************/
struct GTSAM_EXPORT SubgraphBuilderParameters {
typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
enum Skeleton {
/* augmented tree */
NATURALCHAIN = 0, /* natural ordering of the graph */
BFS, /* breadth-first search tree */
KRUSKAL, /* maximum weighted spanning tree */
} skeletonType;
enum SkeletonWeight { /* how to weigh the graph edges */
EQUAL = 0, /* every block edge has equal weight */
RHS_2NORM, /* use the 2-norm of the rhs */
LHS_FNORM, /* use the frobenius norm of the lhs */
RANDOM, /* bounded random edge weight */
} skeletonWeight;
enum AugmentationWeight { /* how to weigh the graph edges */
SKELETON = 0, /* use the same weights in building
the skeleton */
// STRETCH, /* stretch in the
// laplacian sense */ GENERALIZED_STRETCH /*
// the generalized stretch defined in
// jian2013iros */
} augmentationWeight;
/// factor multiplied with n, yields number of extra edges.
double augmentationFactor;
SubgraphBuilderParameters()
: skeletonType(KRUSKAL),
skeletonWeight(RANDOM),
augmentationWeight(SKELETON),
augmentationFactor(1.0) {}
virtual ~SubgraphBuilderParameters() {}
/* for serialization */
void print() const;
virtual void print(std::ostream &os) const;
friend std::ostream &operator<<(std::ostream &os,
const PreconditionerParameters &p);
static Skeleton skeletonTranslator(const std::string &s);
static std::string skeletonTranslator(Skeleton s);
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
static std::string skeletonWeightTranslator(SkeletonWeight w);
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
static std::string augmentationWeightTranslator(AugmentationWeight w);
};
/*****************************************************************************/
class GTSAM_EXPORT SubgraphBuilder {
public:
typedef SubgraphBuilder Base;
typedef std::vector<double> Weights;
SubgraphBuilder(
const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: parameters_(p) {}
virtual ~SubgraphBuilder() {}
virtual Subgraph operator()(const GaussianFactorGraph &jfg) const;
private:
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) const;
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) const;
std::vector<size_t> sample(const std::vector<double> &weights,
const size_t t) const;
Weights weights(const GaussianFactorGraph &gfg) const;
SubgraphBuilderParameters parameters_;
};
/** Select the factors in a factor graph according to the subgraph. */
boost::shared_ptr<GaussianFactorGraph> buildFactorSubgraph(
const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
/** Split the graph into a subgraph and the remaining edges.
* Note that the remaining factorgraph has null factors. */
std::pair<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> >
splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
} // namespace gtsam

548
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -1,6 +1,6 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -12,45 +12,22 @@
/** /**
* @file SubgraphPreconditioner.cpp * @file SubgraphPreconditioner.cpp
* @date Dec 31, 2009 * @date Dec 31, 2009
* @author: Frank Dellaert * @author Frank Dellaert, Yong-Dian Jian
*/ */
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/range/adaptor/reversed.hpp> #include <boost/range/adaptor/reversed.hpp>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <list>
#include <map>
#include <numeric> // accumulate
#include <queue>
#include <set>
#include <stdexcept> #include <stdexcept>
#include <string>
#include <utility>
#include <vector>
using std::cout; using std::cout;
using std::endl; using std::endl;
@ -60,422 +37,60 @@ using std::ostream;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { const KeyVector &keys) {
/* a cache of starting index and dim */
vector<std::pair<size_t, size_t> > cache;
cache.reserve(3);
/* figure out dimension by traversing the keys */
size_t dim = 0;
for (const Key &key : keys) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.emplace_back(entry.start, entry.dim);
dim += entry.dim;
}
/* use the cache to fill the result */
Vector result(dim);
size_t index = 0;
for (const auto &p : cache) {
result.segment(index, p.second) = src.segment(p.first, p.second);
index += p.second;
}
return result;
}
/*****************************************************************************/
static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
const KeyVector &keys, Vector &dst) {
size_t index = 0;
for (const Key &key : keys) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
const size_t keyDim = entry.dim;
dst.segment(entry.start, keyDim) = src.segment(index, keyDim);
index += keyDim;
}
}
/* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
// Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(
const GaussianFactorGraph &gfg) {
auto result = boost::make_shared<GaussianFactorGraph>(); auto result = boost::make_shared<GaussianFactorGraph>();
for (const auto &factor : gfg) { for (const auto &factor : gfg)
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor); if (factor) {
if( !jf ) { auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
jf = boost::make_shared<JacobianFactor>(*factor); if (!jf) {
} jf = boost::make_shared<JacobianFactor>(*factor);
result->push_back(jf);
}
return result;
}
/*****************************************************************************/
static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
/* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
if (sum==0.0) {
throw std::runtime_error("Weight vector has no non-zero weights");
}
/* make a normalized and accumulated version of the weight vector */
const size_t m = weight.size();
vector<double> cdf; cdf.reserve(m);
for ( size_t i = 0 ; i < m ; ++i ) {
cdf.push_back(weight[i]/sum);
}
vector<double> acc(m);
std::partial_sum(cdf.begin(),cdf.end(),acc.begin());
/* iid sample n times */
vector<size_t> result; result.reserve(n);
const double denominator = (double)RAND_MAX;
for ( size_t i = 0 ; i < n ; ++i ) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
const auto it = std::lower_bound(acc.begin(), acc.end(), value);
const size_t index = it - acc.begin();
result.push_back(index);
}
return result;
}
/*****************************************************************************/
static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n) {
const size_t m = weight.size();
if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size");
vector<size_t> result;
size_t count = 0;
vector<bool> touched(m, false);
while ( count < n ) {
vector<size_t> localIndices; localIndices.reserve(n-count);
vector<double> localWeights; localWeights.reserve(n-count);
/* collect data */
for ( size_t i = 0 ; i < m ; ++i ) {
if ( !touched[i] ) {
localIndices.push_back(i);
localWeights.push_back(weight[i]);
} }
result->push_back(jf);
} }
/* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n-count);
for ( const size_t &index: samples ) {
if ( touched[index] == false ) {
touched[index] = true ;
result.push_back(index);
if ( ++count >= n ) break;
}
}
}
return result; return result;
} }
/****************************************************************************/
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
for ( const size_t &index: indices ) {
edges_.emplace_back(index, 1.0);
}
}
/****************************************************************************/
vector<size_t> Subgraph::edgeIndices() const {
vector<size_t> eid; eid.reserve(size());
for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_);
}
return eid;
}
/****************************************************************************/
void Subgraph::save(const std::string &fn) const {
std::ofstream os(fn.c_str());
boost::archive::text_oarchive oa(os);
oa << *this;
os.close();
}
/****************************************************************************/
Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
std::ifstream is(fn.c_str());
boost::archive::text_iarchive ia(is);
Subgraph::shared_ptr subgraph(new Subgraph());
ia >> *subgraph;
is.close();
return subgraph;
}
/****************************************************************************/
ostream &operator<<(ostream &os, const SubgraphEdge &edge) {
if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else
os << edge.index() ;
return os;
}
/****************************************************************************/
ostream &operator<<(ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ;
}
return os;
}
/*****************************************************************************/
void SubgraphBuilderParameters::print() const {
print(cout);
}
/***************************************************************************************/
void SubgraphBuilderParameters::print(ostream &os) const {
os << "SubgraphBuilderParameters" << endl
<< "skeleton: " << skeletonTranslator(skeleton_) << endl
<< "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl
<< "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
std::string s = src; boost::algorithm::to_upper(s);
if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL;
throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
if ( s == NATURALCHAIN ) return "NATURALCHAIN";
else if ( s == BFS ) return "BFS";
else if ( s == KRUSKAL ) return "KRUSKAL";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "EQUAL") return EQUAL;
else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM;
throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
return EQUAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
if ( w == EQUAL ) return "EQUAL";
else if ( w == RHS_2NORM ) return "RHS";
else if ( w == LHS_FNORM ) return "LHS";
else if ( w == RANDOM ) return "RANDOM";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight
SubgraphBuilderParameters::augmentationWeightTranslator(
const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
return SKELETON;
}
/****************************************************************/
std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
if ( w == SKELETON ) return "SKELETON";
// else if ( w == STRETCH ) return "STRETCH";
// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
else return "UNKNOWN";
}
/****************************************************************/
vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN:
return natural_chain(gfg);
break;
case SubgraphBuilderParameters::BFS:
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, weights);
break;
default:
std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t index = 0;
for (const auto &factor : gfg) {
if ( factor->size() == 1 ) {
result.push_back(index);
}
index++;
}
return result;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t index = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 )
result.push_back(index);
}
index++;
}
return result;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0];
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
vector<size_t> result;
result.reserve(n-1);
/* Initialize */
std::queue<size_t> q;
q.push(seed);
std::set<size_t> flags;
flags.insert(seed);
/* traversal */
while ( !q.empty() ) {
const size_t head = q.front(); q.pop();
for ( const size_t index: variableIndex[head] ) {
const GaussianFactor &gf = *gfg[index];
for ( const size_t key: gf.keys() ) {
if ( flags.count(key) == 0 ) {
q.push(key);
flags.insert(key);
result.push_back(index);
}
}
}
}
return result;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> sortedIndices = sort_idx(weights) ;
/* initialize buffer */
vector<size_t> result;
result.reserve(n-1);
// container for acsendingly sorted edges
DSFVector dsf(n);
size_t count = 0 ; double sum = 0.0 ;
for (const size_t index: sortedIndices) {
const GaussianFactor &gf = *gfg[index];
const auto keys = gf.keys();
if ( keys.size() != 2 ) continue;
const size_t u = ordering.find(keys[0])->second,
v = ordering.find(keys[1])->second;
if ( dsf.find(u) != dsf.find(v) ) {
dsf.merge(u, v) ;
result.push_back(index) ;
sum += weights[index] ;
if ( ++count == n-1 ) break ;
}
}
return result;
}
/****************************************************************/
vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
return UniqueSampler(weights, t);
}
/****************************************************************/
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
const auto &p = parameters_;
const auto inverse_ordering = Ordering::Natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), m = gfg.size();
// Make sure the subgraph preconditioner does not include more than half of
// the edges beyond the spanning tree, or we might as well solve the whole thing.
size_t numExtraEdges = n * p.complexity_;
const size_t numRemaining = m - (n - 1);
numExtraEdges = std::min(numExtraEdges, numRemaining/2);
// Calculate weights
vector<double> weights = this->weights(gfg);
// Build spanning tree.
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
if ( tree.size() != n-1 ) {
throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1");
}
// Downweight the tree edges to zero.
for ( const size_t index: tree ) {
weights[index] = 0.0;
}
/* decide how many edges to augment */
vector<size_t> offTree = sample(weights, numExtraEdges);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
return boost::make_shared<Subgraph>(subgraph);
}
/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
{
const size_t m = gfg.size() ;
Weights weight; weight.reserve(m);
for(const GaussianFactor::shared_ptr &gf: gfg ) {
switch ( parameters_.skeletonWeight_ ) {
case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0);
break;
case SubgraphBuilderParameters::RHS_2NORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(jf->getb().norm());
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(hf->linearTerm().norm());
}
}
break;
case SubgraphBuilderParameters::LHS_FNORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(std::sqrt(hf->information().squaredNorm()));
}
}
break;
case SubgraphBuilderParameters::RANDOM:
weight.push_back(std::rand()%100 + 1.0);
break;
default:
throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
return weight;
}
/* ************************************************************************* */ /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
parameters_(p) {} parameters_(p) {}
@ -640,67 +255,18 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda) void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{ {
/* identify the subgraph structure */ /* identify the subgraph structure */
const SubgraphBuilder builder(parameters_.builderParams_); const SubgraphBuilder builder(parameters_.builderParams);
Subgraph::shared_ptr subgraph = builder(gfg); auto subgraph = builder(gfg);
keyInfo_ = keyInfo; keyInfo_ = keyInfo;
/* build factor subgraph */ /* build factor subgraph */
GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
/* factorize and cache BayesNet */ /* factorize and cache BayesNet */
Rc1_ = gfg_subgraph->eliminateSequential(); Rc1_ = gfg_subgraph->eliminateSequential();
} }
/*****************************************************************************/ /*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
const KeyVector &keys) {
/* a cache of starting index and dim */
vector<std::pair<size_t, size_t> > cache;
cache.reserve(3);
/* figure out dimension by traversing the keys */
size_t dim = 0;
for (const Key &key : keys) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.emplace_back(entry.start, entry.dim);
dim += entry.dim;
}
/* use the cache to fill the result */
Vector result(dim);
size_t index = 0;
for (const auto &p : cache) {
result.segment(index, p.second) = src.segment(p.first, p.second);
index += p.second;
}
return result;
}
/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
size_t index = 0;
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
const size_t keyDim = entry.dim;
dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ;
index += keyDim;
}
}
/*****************************************************************************/
GaussianFactorGraph::shared_ptr buildFactorSubgraph(
const GaussianFactorGraph &gfg, const Subgraph &subgraph,
const bool clone) {
auto result = boost::make_shared<GaussianFactorGraph>();
result->reserve(subgraph.size());
for ( const SubgraphEdge &e: subgraph ) {
const size_t index = e.index();
if ( clone ) result->push_back(gfg[index]->clone());
else result->push_back(gfg[index]);
}
return result;
}
} // nsamespace gtsam } // nsamespace gtsam

187
gtsam/linear/SubgraphPreconditioner.h
View File

@ -1,6 +1,6 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -17,30 +17,16 @@
#pragma once #pragma once
#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <map> #include <map>
#include <utility>
#include <vector>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam { namespace gtsam {
@ -49,142 +35,11 @@ namespace gtsam {
class GaussianFactorGraph; class GaussianFactorGraph;
class VectorValues; class VectorValues;
struct GTSAM_EXPORT SubgraphEdge {
size_t index_; /* edge id */
double weight_; /* edge weight */
SubgraphEdge() : index_(0), weight_(1.0) {}
SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
inline size_t index() const { return index_; }
inline double weight() const { return weight_; }
inline bool isUnitWeight() const { return (weight_ == 1.0); }
friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(weight_);
}
};
/**************************************************************************/
class GTSAM_EXPORT Subgraph {
public:
typedef boost::shared_ptr<Subgraph> shared_ptr;
typedef std::vector<shared_ptr> vector_shared_ptr;
typedef std::vector<SubgraphEdge> Edges;
typedef std::vector<size_t> EdgeIndices;
typedef Edges::iterator iterator;
typedef Edges::const_iterator const_iterator;
protected:
Edges edges_; /* index to the factors */
public:
Subgraph() {}
Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
Subgraph(const Edges &edges) : edges_(edges) {}
Subgraph(const std::vector<size_t> &indices) ;
inline const Edges& edges() const { return edges_; }
inline size_t size() const { return edges_.size(); }
EdgeIndices edgeIndices() const;
iterator begin() { return edges_.begin(); }
const_iterator begin() const { return edges_.begin(); }
iterator end() { return edges_.end(); }
const_iterator end() const { return edges_.end(); }
void save(const std::string &fn) const;
static shared_ptr load(const std::string &fn);
friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(edges_);
}
};
/****************************************************************************/
struct GTSAM_EXPORT SubgraphBuilderParameters {
public:
typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
enum Skeleton {
/* augmented tree */
NATURALCHAIN = 0, /* natural ordering of the graph */
BFS, /* breadth-first search tree */
KRUSKAL, /* maximum weighted spanning tree */
} skeleton_ ;
enum SkeletonWeight { /* how to weigh the graph edges */
EQUAL = 0, /* every block edge has equal weight */
RHS_2NORM, /* use the 2-norm of the rhs */
LHS_FNORM, /* use the frobenius norm of the lhs */
RANDOM, /* bounded random edge weight */
} skeletonWeight_ ;
enum AugmentationWeight { /* how to weigh the graph edges */
SKELETON = 0, /* use the same weights in building the skeleton */
// STRETCH, /* stretch in the laplacian sense */
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
} augmentationWeight_ ;
double complexity_; /* factor multiplied with n, yields number of extra edges. */
SubgraphBuilderParameters()
: skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
virtual ~SubgraphBuilderParameters() {}
/* for serialization */
void print() const ;
virtual void print(std::ostream &os) const ;
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
static Skeleton skeletonTranslator(const std::string &s);
static std::string skeletonTranslator(Skeleton s);
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
static std::string skeletonWeightTranslator(SkeletonWeight w);
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
static std::string augmentationWeightTranslator(AugmentationWeight w);
};
/*****************************************************************************/
class GTSAM_EXPORT SubgraphBuilder {
public:
typedef SubgraphBuilder Base;
typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
typedef std::vector<double> Weights;
SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: parameters_(p) {}
virtual ~SubgraphBuilder() {}
virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
private:
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
Weights weights(const GaussianFactorGraph &gfg) const;
SubgraphBuilderParameters parameters_;
};
/*******************************************************************************************/
struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr; typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: Base(), builderParams_(p) {} : builderParams(p) {}
virtual ~SubgraphPreconditionerParameters() {} SubgraphBuilderParameters builderParams;
SubgraphBuilderParameters builderParams_;
}; };
/** /**
@ -300,38 +155,4 @@ namespace gtsam {
/*****************************************************************************/ /*****************************************************************************/
}; };
/* get subvectors */
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
/* set subvectors */
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
/* sort the container and return permutation index with default comparator */
template <typename Container>
std::vector<size_t> sort_idx(const Container &src)
{
typedef typename Container::value_type T;
const size_t n = src.size() ;
std::vector<std::pair<size_t,T> > tmp;
tmp.reserve(n);
for ( size_t i = 0 ; i < n ; i++ )
tmp.push_back(std::make_pair(i, src[i]));
/* sort */
std::stable_sort(tmp.begin(), tmp.end()) ;
/* copy back */
std::vector<size_t> idx; idx.reserve(n);
for ( size_t i = 0 ; i < n ; i++ ) {
idx.push_back(tmp[i].first) ;
}
return idx;
}
} // namespace gtsam } // namespace gtsam

50
gtsam/linear/SubgraphSolver.cpp
View File

@ -18,13 +18,12 @@
*/ */
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative-inl.h> #include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/base/DSFMap.h>
#include <iostream>
using namespace std; using namespace std;
@ -36,7 +35,7 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
const Parameters &parameters, const Ordering& ordering) : const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters) { parameters_(parameters) {
GaussianFactorGraph::shared_ptr Ab1,Ab2; GaussianFactorGraph::shared_ptr Ab1,Ab2;
boost::tie(Ab1, Ab2) = splitGraph(Ab); std::tie(Ab1, Ab2) = splitGraph(Ab);
if (parameters_.verbosity()) if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
<< " factors" << endl; << " factors" << endl;
@ -91,49 +90,20 @@ VectorValues SubgraphSolver::optimize() const {
} }
VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda, const KeyInfo &keyInfo, const map<Key, Vector> &lambda,
const VectorValues &initial) { const VectorValues &initial) {
return VectorValues(); return VectorValues();
} }
/**************************************************************************************************/ /**************************************************************************************************/
// Run Kruskal algorithm to create a spanning tree of factor "edges". pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
// Edges are not weighted, and will only work if factors are binary.
// Unary factors are ignored for this purpose and added to tree graph.
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
// Create disjoint set forest data structure for Kruskal algorithm /* identify the subgraph structure */
DSFMap<Key> dsf; const SubgraphBuilder builder(parameters_.builderParams);
auto subgraph = builder(factorGraph);
// Allocate two output graphs /* build factor subgraph */
auto tree = boost::make_shared<GaussianFactorGraph>(); return splitFactorGraph(factorGraph, subgraph);
auto constraints = boost::make_shared<GaussianFactorGraph>();
// Loop over all "edges"
for ( const auto &factor: factorGraph ) {
// Fail on > binary factors
const auto& keys = factor->keys();
if (keys.size() > 2) {
throw runtime_error(
"SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
// check whether this factor should be augmented to the "tree" graph
if (keys.size() == 1)
tree->push_back(factor);
else if (dsf.find(keys[0]) != dsf.find(keys[1])) {
// We merge two trees joined by this edge if they are still disjoint
tree->push_back(factor);
// Record this fact in DSF
dsf.merge(keys[0], keys[1]);
} else {
// This factor would create a loop, so we add it to non-tree edges...
constraints->push_back(factor);
}
}
return boost::tie(tree, constraints);
} }
/****************************************************************************/ /****************************************************************************/

41
gtsam/linear/SubgraphSolver.h
View File

@ -20,6 +20,10 @@
#pragma once #pragma once
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/SubgraphBuilder.h>
#include <map>
#include <utility> // pair
namespace gtsam { namespace gtsam {
@ -28,13 +32,15 @@ class GaussianFactorGraph;
class GaussianBayesNet; class GaussianBayesNet;
class SubgraphPreconditioner; class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters struct GTSAM_EXPORT SubgraphSolverParameters
: public ConjugateGradientParameters { : public ConjugateGradientParameters {
public: SubgraphBuilderParameters builderParams;
typedef ConjugateGradientParameters Base; explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
SubgraphSolverParameters() : Base() {} : builderParams(p) {}
void print() const { Base::print(); } void print() const { Base::print(); }
virtual void print(std::ostream &os) const { Base::print(os); } virtual void print(std::ostream &os) const {
Base::print(os);
}
}; };
/** /**
@ -115,10 +121,19 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
VectorValues optimize() const; VectorValues optimize() const;
/// Interface that IterativeSolver subclasses have to implement /// Interface that IterativeSolver subclasses have to implement
virtual VectorValues optimize(const GaussianFactorGraph &gfg, VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda, const std::map<Key, Vector> &lambda,
const VectorValues &initial) override; const VectorValues &initial) override;
/// @}
/// @name Implement interface
/// @{
/// Split graph using Kruskal algorithm, treating binary factors as edges.
std::pair < boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph> > splitGraph(
const GaussianFactorGraph &gfg);
/// @} /// @}
@ -127,7 +142,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
/// @{ /// @{
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A, SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
const Parameters &parameters, const Ordering &ordering) const Parameters &parameters, const Ordering &ordering)
: SubgraphSolver(*A, parameters, ordering){}; : SubgraphSolver(*A, parameters, ordering) {}
SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &, SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &,
const Parameters &, const Ordering &); const Parameters &, const Ordering &);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
@ -138,12 +153,6 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
const GaussianFactorGraph &, const Parameters &); const GaussianFactorGraph &, const Parameters &);
/// @} /// @}
#endif #endif
protected:
/// Split graph using Kruskal algorithm, treating binary factors as edges.
static boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph>>
splitGraph(const GaussianFactorGraph &gfg);
}; };
} // namespace gtsam } // namespace gtsam

9
tests/smallExample.h
View File

@ -26,7 +26,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
namespace gtsam { namespace gtsam {
@ -131,7 +130,7 @@ namespace example {
* -x11-x21-x31 * -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry. * with x11 clamped at (1,1), and others related by 2D odometry.
*/ */
// inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N); // inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
/* /*
* Create canonical ordering for planar graph that also works for tree * Create canonical ordering for planar graph that also works for tree
@ -399,7 +398,7 @@ inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
inline GaussianFactorGraph createSmoother(int T) { inline GaussianFactorGraph createSmoother(int T) {
NonlinearFactorGraph nlfg; NonlinearFactorGraph nlfg;
Values poses; Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); std::tie(nlfg, poses) = createNonlinearSmoother(T);
return *nlfg.linearize(poses); return *nlfg.linearize(poses);
} }
@ -563,7 +562,7 @@ inline Symbol key(size_t x, size_t y) {
} // \namespace impl } // \namespace impl
/* ************************************************************************* */ /* ************************************************************************* */
inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) { inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
using namespace impl; using namespace impl;
// create empty graph // create empty graph
@ -601,7 +600,7 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
// linearize around zero // linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros); boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);
return boost::make_tuple(*gfg, xtrue); return std::make_pair(*gfg, xtrue);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
tests/testSubgraphPreconditioner.cpp
View File

@ -218,7 +218,7 @@ TEST(SubgraphSolver, Solves) {
SubgraphPreconditioner system; SubgraphPreconditioner system;
// We test on three different graphs // We test on three different graphs
const auto Ab1 = planarGraph(3).get<0>(); const auto Ab1 = planarGraph(3).first;
const auto Ab2 = read("toy3D"); const auto Ab2 = read("toy3D");
const auto Ab3 = read("randomGrid3D"); const auto Ab3 = read("randomGrid3D");

35
tests/testSubgraphSolver.cpp
View File

@ -15,18 +15,19 @@
* @author Yong-Dian Jian * @author Yong-Dian Jian
**/ **/
#include <gtsam/linear/SubgraphSolver.h>
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h> #include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> #include <boost/assign/std/list.hpp>
using namespace boost::assign; using namespace boost::assign;
@ -53,13 +54,33 @@ TEST( SubgraphSolver, Parameters )
LONGS_EQUAL(500, kParameters.maxIterations()); LONGS_EQUAL(500, kParameters.maxIterations());
} }
/* ************************************************************************* */
TEST( SubgraphSolver, splitFactorGraph )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
SubgraphBuilderParameters params;
params.augmentationFactor = 0.0;
SubgraphBuilder builder(params);
auto subgraph = builder(Ab);
EXPECT_LONGS_EQUAL(9, subgraph.size());
GaussianFactorGraph::shared_ptr Ab1, Ab2;
std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph);
EXPECT_LONGS_EQUAL(9, Ab1->size());
EXPECT_LONGS_EQUAL(13, Ab2->size());
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphSolver, constructor1 ) TEST( SubgraphSolver, constructor1 )
{ {
// Build a planar graph // Build a planar graph
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
VectorValues xtrue; VectorValues xtrue;
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// The first constructor just takes a factor graph (and kParameters) // The first constructor just takes a factor graph (and kParameters)
// and it will split the graph into A1 and A2, where A1 is a spanning tree // and it will split the graph into A1 and A2, where A1 is a spanning tree
@ -75,11 +96,11 @@ TEST( SubgraphSolver, constructor2 )
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
VectorValues xtrue; VectorValues xtrue;
size_t N = 3; size_t N = 3;
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree // Get the spanning tree
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs, so the caller can specify // The second constructor takes two factor graphs, so the caller can specify
// the preconditioner (Ab1) and the constraints that are left out (Ab2) // the preconditioner (Ab1) and the constraints that are left out (Ab2)
@ -95,11 +116,11 @@ TEST( SubgraphSolver, constructor3 )
GaussianFactorGraph Ab; GaussianFactorGraph Ab;
VectorValues xtrue; VectorValues xtrue;
size_t N = 3; size_t N = 3;
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding kOrdering // Get the spanning tree and corresponding kOrdering
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
auto Rc1 = Ab1->eliminateSequential(); auto Rc1 = Ab1->eliminateSequential();