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

I changed the name of SubgraphPCG to SubgraphSolver and put it in its own compilation unit

release/4.3a0
Frank Dellaert 2010-03-12 19:19:21 +00:00
parent f217a5bd8a
commit 00ac961c8a
7 changed files with 87 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Makefile.am
View File

@ -115,7 +115,7 @@ testErrors_SOURCES = testErrors.cpp
testErrors_LDADD = libgtsam.la
# Iterative Methods
headers += iterative-inl.h SubgraphPreconditioner-inl.h
headers += iterative-inl.h SubgraphSolver.h SubgraphSolver-inl.h
sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
testIterative_SOURCES = testIterative.cpp

2
cpp/Pose2SLAMOptimizer.cpp
View File

@ -8,7 +8,7 @@
#include "Pose2SLAMOptimizer.h"
#include "pose2SLAM.h"
#include "dataset.h"
#include "SubgraphPreconditioner-inl.h"
#include "SubgraphSolver-inl.h"
using namespace std;
namespace gtsam {

4
cpp/Pose2SLAMOptimizer.h
View File

@ -12,7 +12,7 @@
#include "pose2SLAM.h"
#include "Ordering.h"
#include "SubgraphPreconditioner.h"
#include "SubgraphSolver.h"
namespace gtsam {
@ -31,7 +31,7 @@ namespace gtsam {
boost::shared_ptr<Pose2Config> theta_;
/** Non-linear solver */
typedef SubgraphPCG<Pose2Graph, Pose2Config> SPCG_Solver;
typedef SubgraphSolver<Pose2Graph, Pose2Config> SPCG_Solver;
SPCG_Solver solver_;
/** Linear Solver */

62
cpp/SubgraphPreconditioner.h
View File

@ -4,8 +4,7 @@
* @author: Frank Dellaert
*/
#ifndef SUBGRAPHPRECONDITIONER_H_
#define SUBGRAPHPRECONDITIONER_H_
#pragma once
#include "GaussianFactorGraph.h"
#include "GaussianBayesNet.h"
@ -91,61 +90,4 @@ namespace gtsam {
void print(const std::string& s = "SubgraphPreconditioner") const;
};
/**
* A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> VectorConfig
*/
template<class G, class T>
class SubgraphPCG {
private:
typedef typename T::Key Key;
typedef typename G::Constraint Constraint;
typedef typename G::Pose Pose;
// TODO not hardcode
static const size_t maxIterations_=100;
static const bool verbose_=false;
static const double epsilon_=1e-4, epsilon_abs_=1e-5;
/* the ordering derived from the spanning tree */
boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */
boost::shared_ptr<T> theta_bar_;
G T_, C_;
public:
SubgraphPCG() {}
SubgraphPCG(const G& g, const T& theta0);
void initialize(const G& g, const T& theta0);
boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<T> theta_bar() const { return theta_bar_; }
/**
* linearize the non-linear graph around the current config and build the subgraph preconditioner systme
*/
boost::shared_ptr<SubgraphPreconditioner> linearize(const G& g, const T& theta_bar) const;
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system
*/
VectorConfig optimize(SubgraphPreconditioner& system) const;
};
template<class G, class T> const size_t SubgraphPCG<G,T>::maxIterations_;
template<class G, class T> const bool SubgraphPCG<G,T>::verbose_;
template<class G, class T> const double SubgraphPCG<G,T>::epsilon_;
template<class G, class T> const double SubgraphPCG<G,T>::epsilon_abs_;
} // nsamespace gtsam
#endif /* SUBGRAPHPRECONDITIONER_H_ */
} // namespace gtsam

12
cpp/SubgraphPreconditioner-inl.h → cpp/SubgraphSolver-inl.h
View File

@ -1,5 +1,5 @@
/*
* SubgraphPreconditioner-inl.h
* SubgraphSolver-inl.h
*
* Created on: Jan 17, 2010
* Author: nikai
@ -9,7 +9,7 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include "SubgraphPreconditioner.h"
#include "SubgraphSolver.h"
#include "graph-inl.h"
#include "iterative-inl.h"
@ -21,13 +21,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class G, class T>
SubgraphPCG<G, T>::SubgraphPCG(const G& g, const T& theta0) {
SubgraphSolver<G, T>::SubgraphSolver(const G& g, const T& theta0) {
initialize(g,theta0);
}
/* ************************************************************************* */
template<class G, class T>
void SubgraphPCG<G, T>::initialize(const G& g, const T& theta0) {
void SubgraphSolver<G, T>::initialize(const G& g, const T& theta0) {
// generate spanning tree
PredecessorMap<Key> tree = g.template findMinimumSpanningTree<Key, Constraint>();
@ -51,7 +51,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class G, class T>
boost::shared_ptr<SubgraphPreconditioner> SubgraphPCG<G, T>::linearize(const G& g, const T& theta_bar) const {
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<G, T>::linearize(const G& g, const T& theta_bar) const {
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
#ifdef TIMING
@ -69,7 +69,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class G, class T>
VectorConfig SubgraphPCG<G, T>::optimize(SubgraphPreconditioner& system) const {
VectorConfig SubgraphSolver<G, T>::optimize(SubgraphPreconditioner& system) const {
VectorConfig zeros = system.zero();
// Solve the subgraph PCG

71
cpp/SubgraphSolver.h Normal file
View File

@ -0,0 +1,71 @@
/*
* SubgraphSolver.h
* Created on: Dec 31, 2009
* @author: Frank Dellaert
*/
#pragma once
#include "GaussianFactorGraph.h"
#include "GaussianBayesNet.h"
#include "Ordering.h"
#include "SubgraphPreconditioner.h"
namespace gtsam {
/**
* A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> VectorConfig
*/
template<class G, class T>
class SubgraphSolver {
private:
typedef typename T::Key Key;
typedef typename G::Constraint Constraint;
typedef typename G::Pose Pose;
// TODO not hardcode
static const size_t maxIterations_=100;
static const bool verbose_=false;
static const double epsilon_=1e-4, epsilon_abs_=1e-5;
/* the ordering derived from the spanning tree */
boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */
boost::shared_ptr<T> theta_bar_;
G T_, C_;
public:
SubgraphSolver() {}
SubgraphSolver(const G& g, const T& theta0);
void initialize(const G& g, const T& theta0);
boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<T> theta_bar() const { return theta_bar_; }
/**
* linearize the non-linear graph around the current config and build the subgraph preconditioner systme
*/
boost::shared_ptr<SubgraphPreconditioner> linearize(const G& g, const T& theta_bar) const;
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system
*/
VectorConfig optimize(SubgraphPreconditioner& system) const;
};
template<class G, class T> const size_t SubgraphSolver<G,T>::maxIterations_;
template<class G, class T> const bool SubgraphSolver<G,T>::verbose_;
template<class G, class T> const double SubgraphSolver<G,T>::epsilon_;
template<class G, class T> const double SubgraphSolver<G,T>::epsilon_abs_;
} // nsamespace gtsam

8
cpp/testNonlinearOptimizer.cpp
View File

@ -27,7 +27,7 @@ using namespace boost;
// template definitions
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "SubgraphPreconditioner-inl.h"
#include "SubgraphSolver-inl.h"
using namespace gtsam;
@ -202,10 +202,10 @@ TEST( NonlinearOptimizer, Factorization )
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SubgraphPCG )
TEST( NonlinearOptimizer, SubgraphSolver )
{
using namespace pose2SLAM;
typedef SubgraphPCG<Graph, Config> Solver;
typedef SubgraphSolver<Graph, Config> Solver;
typedef NonlinearOptimizer<Graph, Config, SubgraphPreconditioner, Solver> Optimizer;
// Create a graph
@ -220,7 +220,7 @@ TEST( NonlinearOptimizer, SubgraphPCG )
// Create solver and optimizer
Optimizer::shared_solver solver
(new SubgraphPCG<Graph, Config> (*graph, *config));
(new SubgraphSolver<Graph, Config> (*graph, *config));
Optimizer optimizer(graph, config, solver);
// Optimize !!!!