I changed the name of SubgraphPCG to SubgraphSolver and put it in its own compilation unit
parent
f217a5bd8a
commit
00ac961c8a
|
|
@ -115,7 +115,7 @@ testErrors_SOURCES = testErrors.cpp
|
||||||
testErrors_LDADD = libgtsam.la
|
testErrors_LDADD = libgtsam.la
|
||||||
|
|
||||||
# Iterative Methods
|
# 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
|
sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
|
||||||
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
|
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
|
||||||
testIterative_SOURCES = testIterative.cpp
|
testIterative_SOURCES = testIterative.cpp
|
||||||
|
|
|
||||||
|
|
@ -8,7 +8,7 @@
|
||||||
#include "Pose2SLAMOptimizer.h"
|
#include "Pose2SLAMOptimizer.h"
|
||||||
#include "pose2SLAM.h"
|
#include "pose2SLAM.h"
|
||||||
#include "dataset.h"
|
#include "dataset.h"
|
||||||
#include "SubgraphPreconditioner-inl.h"
|
#include "SubgraphSolver-inl.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
|
||||||
|
|
@ -12,7 +12,7 @@
|
||||||
|
|
||||||
#include "pose2SLAM.h"
|
#include "pose2SLAM.h"
|
||||||
#include "Ordering.h"
|
#include "Ordering.h"
|
||||||
#include "SubgraphPreconditioner.h"
|
#include "SubgraphSolver.h"
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -31,7 +31,7 @@ namespace gtsam {
|
||||||
boost::shared_ptr<Pose2Config> theta_;
|
boost::shared_ptr<Pose2Config> theta_;
|
||||||
|
|
||||||
/** Non-linear solver */
|
/** Non-linear solver */
|
||||||
typedef SubgraphPCG<Pose2Graph, Pose2Config> SPCG_Solver;
|
typedef SubgraphSolver<Pose2Graph, Pose2Config> SPCG_Solver;
|
||||||
SPCG_Solver solver_;
|
SPCG_Solver solver_;
|
||||||
|
|
||||||
/** Linear Solver */
|
/** Linear Solver */
|
||||||
|
|
|
||||||
|
|
@ -4,8 +4,7 @@
|
||||||
* @author: Frank Dellaert
|
* @author: Frank Dellaert
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef SUBGRAPHPRECONDITIONER_H_
|
#pragma once
|
||||||
#define SUBGRAPHPRECONDITIONER_H_
|
|
||||||
|
|
||||||
#include "GaussianFactorGraph.h"
|
#include "GaussianFactorGraph.h"
|
||||||
#include "GaussianBayesNet.h"
|
#include "GaussianBayesNet.h"
|
||||||
|
|
@ -91,61 +90,4 @@ namespace gtsam {
|
||||||
void print(const std::string& s = "SubgraphPreconditioner") const;
|
void print(const std::string& s = "SubgraphPreconditioner") const;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
} // 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 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_ */
|
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
/*
|
/*
|
||||||
* SubgraphPreconditioner-inl.h
|
* SubgraphSolver-inl.h
|
||||||
*
|
*
|
||||||
* Created on: Jan 17, 2010
|
* Created on: Jan 17, 2010
|
||||||
* Author: nikai
|
* Author: nikai
|
||||||
|
|
@ -9,7 +9,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <boost/tuple/tuple.hpp>
|
#include <boost/tuple/tuple.hpp>
|
||||||
#include "SubgraphPreconditioner.h"
|
#include "SubgraphSolver.h"
|
||||||
|
|
||||||
#include "graph-inl.h"
|
#include "graph-inl.h"
|
||||||
#include "iterative-inl.h"
|
#include "iterative-inl.h"
|
||||||
|
|
@ -21,13 +21,13 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class G, class T>
|
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);
|
initialize(g,theta0);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class G, class T>
|
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
|
// generate spanning tree
|
||||||
PredecessorMap<Key> tree = g.template findMinimumSpanningTree<Key, Constraint>();
|
PredecessorMap<Key> tree = g.template findMinimumSpanningTree<Key, Constraint>();
|
||||||
|
|
@ -51,7 +51,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class G, class T>
|
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 Ab1 = T_.linearize(theta_bar);
|
||||||
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
|
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
|
||||||
#ifdef TIMING
|
#ifdef TIMING
|
||||||
|
|
@ -69,7 +69,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class G, class T>
|
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();
|
VectorConfig zeros = system.zero();
|
||||||
|
|
||||||
// Solve the subgraph PCG
|
// Solve the subgraph PCG
|
||||||
|
|
@ -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
|
||||||
|
|
@ -27,7 +27,7 @@ using namespace boost;
|
||||||
// template definitions
|
// template definitions
|
||||||
#include "NonlinearFactorGraph-inl.h"
|
#include "NonlinearFactorGraph-inl.h"
|
||||||
#include "NonlinearOptimizer-inl.h"
|
#include "NonlinearOptimizer-inl.h"
|
||||||
#include "SubgraphPreconditioner-inl.h"
|
#include "SubgraphSolver-inl.h"
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
|
|
@ -202,10 +202,10 @@ TEST( NonlinearOptimizer, Factorization )
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( NonlinearOptimizer, SubgraphPCG )
|
TEST( NonlinearOptimizer, SubgraphSolver )
|
||||||
{
|
{
|
||||||
using namespace pose2SLAM;
|
using namespace pose2SLAM;
|
||||||
typedef SubgraphPCG<Graph, Config> Solver;
|
typedef SubgraphSolver<Graph, Config> Solver;
|
||||||
typedef NonlinearOptimizer<Graph, Config, SubgraphPreconditioner, Solver> Optimizer;
|
typedef NonlinearOptimizer<Graph, Config, SubgraphPreconditioner, Solver> Optimizer;
|
||||||
|
|
||||||
// Create a graph
|
// Create a graph
|
||||||
|
|
@ -220,7 +220,7 @@ TEST( NonlinearOptimizer, SubgraphPCG )
|
||||||
|
|
||||||
// Create solver and optimizer
|
// Create solver and optimizer
|
||||||
Optimizer::shared_solver solver
|
Optimizer::shared_solver solver
|
||||||
(new SubgraphPCG<Graph, Config> (*graph, *config));
|
(new SubgraphSolver<Graph, Config> (*graph, *config));
|
||||||
Optimizer optimizer(graph, config, solver);
|
Optimizer optimizer(graph, config, solver);
|
||||||
|
|
||||||
// Optimize !!!!
|
// Optimize !!!!
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue