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

generate ordering and pose config in the initialization stage

release/4.3a0
Kai Ni 2010-01-18 09:24:38 +00:00
parent 18414b1286
commit bbf6cabe9d
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cpp/SubgraphPreconditioner-inl.h
View File

@ -29,12 +29,20 @@ namespace gtsam {
list<Key> keys = predecessorMap2Keys(tree);
// split the graph
Key root = keys.back();
if (verbose_) cout << "generating spanning tree and split the graph ...";
G.template split<Key, Constraint>(tree, T_, C_);
if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl;
// theta_bar = composePoses<Graph, Constraint, Pose, Config> (T_, tree, config[root]);
// 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));
// compose the approximate solution
Key root = keys.back();
theta_bar_ = composePoses<Graph, Constraint, Pose, Config> (T_, tree, config[root]);
}
/* ************************************************************************* */
@ -43,7 +51,7 @@ namespace gtsam {
const Config& theta_bar, const Ordering& ordering) const {
VectorConfig zeros;
BOOST_FOREACH(const string& j, ordering) zeros.insert(j,zero(3));
BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,zero(3));
// build the subgraph PCG system
GaussianFactorGraph Ab1 = T_.linearize(theta_bar);

17
cpp/SubgraphPreconditioner.h
View File

@ -9,6 +9,7 @@
#include "GaussianFactorGraph.h"
#include "GaussianBayesNet.h"
#include "Ordering.h"
namespace gtsam {
@ -59,8 +60,6 @@ namespace gtsam {
void print(const std::string& s = "SubgraphPreconditioner") const;
};
class Ordering;
/**
* A linear system solver using subgraph preconditioning conjugate gradient
*/
@ -76,12 +75,24 @@ namespace gtsam {
const bool verbose_;
const double epsilon_, epsilon_abs_;
/* the ordering derived from the spanning tree */
boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */
boost::shared_ptr<Config> theta_bar_;
NonlinearGraph T_, C_;
public:
SubgraphPCG() {}
// kai: this constructor is for compatible with Factorization
SubgraphPCG() { throw std::runtime_error("SubgraphPCG: this constructor is only for compatibility!");}
SubgraphPCG(const NonlinearGraph& G, const Config& config);
boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<Config> theta_bar() const { return theta_bar_; }
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system