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

Fix the subgraph preconditioner solver

release/4.3a0
Yong-Dian Jian 2010-10-18 19:29:16 +00:00
parent 989ab9a94f
commit e9b93fe007
4 changed files with 46 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
linear/SubgraphPreconditioner.cpp
View File

@ -41,30 +41,31 @@ namespace gtsam {
return x;
}
SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const {
SubgraphPreconditioner result = *this ;
result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ;
return result ;
}
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
// Errors e;
//
// // Use BayesNet order to add y contributions in order
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// e.push_back(y[j]); // append y
// }
//
// // Add A2 contribution
// VectorValues x = this->x(y);
// Errors e2 = Ab2_->errors(x);
// e.splice(e.end(), e2);
//
// return 0.5 * dot(e, e);
Errors e(y);
// Add A2 contribution
Errors e(y);
VectorValues x = this->x(y);
Errors e2 = Ab2_->errors(x);
return 0.5 * (dot(e, e) + dot(e2,e2));
}
@ -107,21 +108,19 @@ namespace gtsam {
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
// Errors::iterator ei = e.begin();
//
// // Use BayesNet order to add y contributions in order
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// *ei = y[j]; // append y
// ei++;
// }
//
// // Add A2 contribution
// VectorValues x = y; // TODO avoid ?
// gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
// Ab2_->multiplyInPlace(x,ei); // use iterator version
Errors::iterator ei = e.begin();
for ( int i = 0 ; i < y.size() ; ++i, ++ei ) {
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i];
}
@ -152,7 +151,7 @@ namespace gtsam {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for ( int i = 0 ; i < y.size() ; ++i, ++it )
for ( Index i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
@ -170,12 +169,11 @@ namespace gtsam {
// const Vector& ej = *(it++);
// axpy(alpha,ej,y[j]);
// }
//
// // get A2 part
// transposeMultiplyAdd2(alpha,it,e.end(),y);
Errors::const_iterator it = e.begin();
for ( int i = 0 ; i < y.size() ; ++i, ++it ) {
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it;
axpy(alpha,ei,y[i]) ;
}

6
linear/SubgraphPreconditioner.h
View File

@ -63,6 +63,12 @@ namespace gtsam {
// Vector b2() const { return Ab2_->rhsVector(); }
// VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); }
/**
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const;

26
linear/SubgraphSolver-inl.h
View File

@ -21,9 +21,8 @@
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
using namespace std;
@ -41,36 +40,37 @@ namespace gtsam {
void SubgraphSolver<Graph, Values>::initialize(const Graph& G, const Values& theta0) {
// generate spanning tree
PredecessorMap<Key> tree = G.template findMinimumSpanningTree<Key, Constraint>();
list<Key> keys = predecessorMap2Keys(tree);
PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<Graph, Key, Constraint>(G);
// split the graph
if (verbose_) cout << "generating spanning tree and split the graph ...";
G.template split<Key, Constraint>(tree, T_, C_);
// G.template split<Key, Constraint>(tree, T_, C_);
gtsam::split<Graph,Key,Constraint>(G, tree, T_, C_) ;
if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl;
// make the ordering
list<Symbol> symbols;
symbols.resize(keys.size());
std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol<Key>);
ordering_ = boost::shared_ptr<Ordering>(new Ordering(symbols));
list<Key> keys = predecessorMap2Keys(tree);
ordering_ = boost::shared_ptr<Ordering>(new Ordering(list<Symbol>(keys.begin(), keys.end())));
// Add a HardConstraint to the root, otherwise the root will be singular
Key root = keys.back();
T_.addHardConstraint(root, theta0[root]);
// compose the approximate solution
Key root = keys.back();
theta_bar_ = composePoses<Graph, Constraint, Pose, Values> (T_, tree, theta0[root]);
}
/* ************************************************************************* */
template<class Graph, class Values>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Values>::linearize(const Graph& G, const Values& theta_bar) const {
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar, *ordering_);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_);
#ifdef TIMING
SubgraphPreconditioner::sharedBayesNet Rc1;
SubgraphPreconditioner::sharedValues xbar;
#else
GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_);
SubgraphPreconditioner::sharedBayesNet Rc1 = Inference::Eliminate(sacrificialAb1) ;
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
#endif
// TODO: there does not seem to be a good reason to have Ab1_

9
linear/SubgraphSolver.h
View File

@ -19,8 +19,8 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -67,6 +67,7 @@ namespace gtsam {
*/
boost::shared_ptr<SubgraphPreconditioner> linearize(const Graph& G, const Values& theta_bar) const;
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system
@ -76,6 +77,12 @@ namespace gtsam {
boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
}
/** expmap the Values given the stored Ordering */
Values expmap(const Values& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_);
}
};
template<class Graph, class Values> const size_t SubgraphSolver<Graph,Values>::maxIterations_;