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

150
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -52,17 +52,21 @@
#include <utility> #include <utility>
#include <vector> #include <vector>
using namespace std; using std::cout;
using std::endl;
using std::vector;
using std::ostream;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); auto result = boost::make_shared<GaussianFactorGraph>();
for(const GaussianFactor::shared_ptr &gf: gfg) { for (const auto &factor : gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf); auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if( !jf ) { 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); 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 */ /* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); 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; vector<size_t> result;
size_t count = 0; size_t count = 0;
std::vector<bool> touched(m, false); vector<bool> touched(m, false);
while ( count < n ) { while ( count < n ) {
std::vector<size_t> localIndices; localIndices.reserve(n-count); vector<size_t> localIndices; localIndices.reserve(n-count);
std::vector<double> localWeights; localWeights.reserve(n-count); vector<double> localWeights; localWeights.reserve(n-count);
/* collect data */ /* collect data */
for ( size_t i = 0 ; i < m ; ++i ) { 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()); edges_.reserve(indices.size());
for ( const size_t &idx: indices ) { 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 { vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size()); vector<size_t> eid; eid.reserve(size());
for ( const SubgraphEdge &edge: edges_ ) { for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_); 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 ) if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else 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; os << "Subgraph" << endl;
for ( const SubgraphEdge &e: subgraph.edges() ) { for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ; os << e << ", " ;
@ -212,7 +216,7 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato
if (s == "NATURALCHAIN") return NATURALCHAIN; if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS; else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL; 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; return KRUSKAL;
} }
@ -231,7 +235,7 @@ SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWei
else if (s == "RHS") return RHS_2NORM; else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM; else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM; 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; 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); std::string s = src; boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON; if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH; // else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_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; 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_; const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) { switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN: case SubgraphBuilderParameters::NATURALCHAIN:
@ -276,18 +284,18 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
return kruskal(gfg, ordering, w); return kruskal(gfg, ordering, w);
break; break;
default: default:
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break; break;
} }
return vector<size_t>(); return vector<size_t>();
} }
/****************************************************************/ /****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ; vector<size_t> result ;
size_t idx = 0; size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) { for (const auto &factor : gfg) {
if ( gf->size() == 1 ) { if ( factor->size() == 1 ) {
result.push_back(idx); result.push_back(idx);
} }
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 { vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ; vector<size_t> result ;
size_t idx = 0; size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) { for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) { 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); const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */ /* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0]; 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(); const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */ /* each vertex has self as the predecessor */
std::vector<size_t> result; vector<size_t> result;
result.reserve(n-1); result.reserve(n-1);
/* Initialize */ /* 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 VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size(); const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(w) ; 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); result.reserve(n-1);
// container for acsendingly sorted edges // container for acsendingly sorted edges
DSFVector D(n) ; DSFVector dsf(n);
size_t count = 0 ; double sum = 0.0 ; size_t count = 0 ; double sum = 0.0 ;
for (const size_t id: idx) { for (const size_t id: idx) {
const GaussianFactor &gf = *gfg[id]; const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue; const auto keys = gf.keys();
const size_t u = ordering.find(gf.keys()[0])->second, if ( keys.size() != 2 ) continue;
u_root = D.find(u), const size_t u = ordering.find(keys[0])->second,
v = ordering.find(gf.keys()[1])->second, v = ordering.find(keys[1])->second;
v_root = D.find(v) ; if ( dsf.find(u) != dsf.find(v) ) {
if ( u_root != v_root ) { dsf.merge(u, v) ;
D.merge(u_root, v_root) ;
result.push_back(id) ; result.push_back(id) ;
sum += w[id] ; sum += w[id] ;
if ( ++count == n-1 ) break ; 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); return uniqueSampler(weights, t);
} }
@ -395,7 +404,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
/* sanity check */ /* sanity check */
if ( tree.size() != n-1 ) { 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 */ /* 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 */ /* 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); vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end()); subgraph.insert(subgraph.end(), tree.begin(), tree.end());
@ -450,7 +459,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
break; break;
default: default:
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break; break;
} }
} }
@ -495,7 +504,6 @@ VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y); Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */ Errors e2 = *Ab2() * x; /* A2*x */
@ -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 */ /* copy first */
assert(x.size() == y.size());
std::copy(y.data(), y.data() + y.rows(), x.data()); std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */ /* 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) */ /* 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 xParent = getSubvector(
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); 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 */ /* 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 */ /* 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 */ /* copy first */
assert(x.size() == y.size());
std::copy(y.data(), y.data() + y.rows(), x.data()); std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */ /* in place back substitute */
for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) { for (const auto &cg : *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); const Vector rhsFrontal = getSubvector(
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal); x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal); const Vector solFrontal =
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
rhsFrontal);
// Check for indeterminant solution // 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 */ /* 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 */ /* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
KeyInfo::const_iterator it2 = keyInfo_.find(*it); const KeyInfoEntry &info = keyInfo_.find(*it)->second;
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; 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) { Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
/* a cache of starting index and dim */ /* 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; Cache cache;
/* figure out dimension by traversing the keys */ /* figure out dimension by traversing the keys */
size_t d = 0; size_t d = 0;
for ( const Key &key: keys ) { for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second; 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(); d += entry.dim();
} }
@ -668,10 +684,10 @@ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &ke
} }
/*****************************************************************************/ /*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph> GaussianFactorGraph::shared_ptr buildFactorSubgraph(
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { const GaussianFactorGraph &gfg, const Subgraph &subgraph,
const bool clone) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); auto result = boost::make_shared<GaussianFactorGraph>();
result->reserve(subgraph.size()); result->reserve(subgraph.size());
for ( const SubgraphEdge &e: subgraph ) { for ( const SubgraphEdge &e: subgraph ) {
const size_t idx = e.index(); const size_t idx = e.index();