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

Cleanup

release/4.3a0
Frank Dellaert 2019-04-04 23:01:32 -04:00
parent 334c85a298
commit 6b637bda9e
1 changed files with 87 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -52,17 +52,21 @@
#include <utility>
#include <vector>
using namespace std;
using std::cout;
using std::endl;
using std::vector;
using std::ostream;
namespace gtsam {
/* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
for(const GaussianFactor::shared_ptr &gf: gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
auto result = boost::make_shared<GaussianFactorGraph>();
for (const auto &factor : gfg) {
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
jf = boost::make_shared<JacobianFactor>(*factor);
}
result->push_back(jf);
}
@ -70,7 +74,7 @@ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFa
}
/*****************************************************************************/
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
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);
@ -107,10 +111,10 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
vector<size_t> result;
size_t count = 0;
std::vector<bool> touched(m, false);
vector<bool> touched(m, false);
while ( count < n ) {
std::vector<size_t> localIndices; localIndices.reserve(n-count);
std::vector<double> localWeights; localWeights.reserve(n-count);
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 ) {
@ -134,16 +138,16 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
}
/****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) {
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
for ( const size_t &idx: indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0));
edges_.emplace_back(idx, 1.0);
}
}
/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size());
vector<size_t> Subgraph::edgeIndices() const {
vector<size_t> eid; eid.reserve(size());
for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_);
}
@ -169,7 +173,7 @@ Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
ostream &operator<<(ostream &os, const SubgraphEdge &edge) {
if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else
@ -178,7 +182,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
ostream &operator<<(ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ;
@ -212,7 +216,7 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato
if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL;
throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
@ -231,7 +235,7 @@ SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWei
else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM;
throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
return EQUAL;
}
@ -245,12 +249,14 @@ std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
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 invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
return SKELETON;
}
@ -263,7 +269,9 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &w) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN:
@ -276,18 +284,18 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
return kruskal(gfg, ordering, w);
break;
default:
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 1 ) {
for (const auto &factor : gfg) {
if ( factor->size() == 1 ) {
result.push_back(idx);
}
idx++;
@ -296,8 +304,8 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) {
@ -311,7 +319,7 @@ std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gf
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
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];
@ -319,7 +327,7 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
std::vector<size_t> result;
vector<size_t> result;
result.reserve(n-1);
/* Initialize */
@ -347,7 +355,9 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &w) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(w) ;
@ -357,18 +367,17 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
result.reserve(n-1);
// container for acsendingly sorted edges
DSFVector D(n) ;
DSFVector dsf(n);
size_t count = 0 ; double sum = 0.0 ;
for (const size_t id: idx) {
const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second,
u_root = D.find(u),
v = ordering.find(gf.keys()[1])->second,
v_root = D.find(v) ;
if ( u_root != v_root ) {
D.merge(u_root, v_root) ;
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(id) ;
sum += w[id] ;
if ( ++count == n-1 ) break ;
@ -378,7 +387,7 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
return uniqueSampler(weights, t);
}
@ -395,7 +404,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
/* sanity check */
if ( tree.size() != n-1 ) {
throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
}
/* down weight the tree edges to zero */
@ -404,7 +413,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
}
/* decide how many edges to augment */
std::vector<size_t> offTree = sample(w, t);
vector<size_t> offTree = sample(w, t);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
@ -450,7 +459,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
break;
default:
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
@ -484,21 +493,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const {
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
Errors e = (*Ab2() * x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x);
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + Rc1()->backSubstituteTranspose(v);
}
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */
Errors e2 = *Ab2() * x; /* A2*x */
e.splice(e.end(), e2);
return e;
}
@ -568,47 +576,55 @@ void SubgraphPreconditioner::print(const std::string& s) const {
}
/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
{
void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
/* copy first */
assert(x.size() == y.size());
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
for (auto cg: boost::adaptors::reverse(*Rc1_)) {
for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
/* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const Vector xParent = getSubvector(
x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(
x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
rhsFrontal - cg->get_S() * xParent);
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
setSubvector(solFrontal, keyInfo_,
KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
}
}
/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
{
void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
/* copy first */
assert(x.size() == y.size());
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
for (const auto &cg : *Rc1_) {
const Vector rhsFrontal = getSubvector(
x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const Vector solFrontal =
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
rhsFrontal);
// Check for indeterminant solution
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
if (solFrontal.hasNaN())
throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
setSubvector(solFrontal, keyInfo_,
KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
/* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
KeyInfo::const_iterator it2 = keyInfo_.find(*it);
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
const KeyInfoEntry &info = keyInfo_.find(*it)->second;
Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
}
}
@ -634,14 +650,14 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
/* a cache of starting index and dim */
typedef vector<pair<size_t, size_t> > Cache;
typedef vector<std::pair<size_t, size_t> > Cache;
Cache cache;
/* figure out dimension by traversing the keys */
size_t d = 0;
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair(entry.colstart(), entry.dim()));
cache.emplace_back(entry.colstart(), entry.dim());
d += entry.dim();
}
@ -668,10 +684,10 @@ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &ke
}
/*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
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 idx = e.index();