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

Deprecated all but three constructors.

release/4.3a0
Frank Dellaert 2019-04-04 09:23:02 -04:00
parent 140c666c41
commit 3737474d1e
4 changed files with 120 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/linear/SubgraphSolver.cpp
View File

@ -31,12 +31,12 @@ using namespace std;
namespace gtsam {
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
// Just taking system [A|b]
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters) {
GaussianFactorGraph::shared_ptr Ab1,Ab2;
boost::tie(Ab1, Ab2) = splitGraph(gfg);
boost::tie(Ab1, Ab2) = splitGraph(Ab);
if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
<< " factors" << endl;
@ -46,12 +46,8 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &factorGraph,
const Parameters &parameters, const Ordering& ordering) :
SubgraphSolver(*factorGraph, parameters, ordering) {}
/**************************************************************************************************/
// Taking eliminated tree [R1|c] and constraint graph [A2|b2]
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2,
const Parameters &parameters)
@ -60,42 +56,40 @@ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
// Taking subgraphs [A1|b1] and [A2|b2]
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2,
const Parameters &parameters,
const Ordering &ordering)
: SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
parameters) {}
/**************************************************************************************************/
// deprecated variants
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph &Ab2,
const Parameters &parameters)
: SubgraphSolver(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters_) {}
parameters) {}
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph &Ab2,
const Parameters &parameters,
const Ordering &ordering)
: SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR),
boost::make_shared<GaussianFactorGraph>(Ab2),
parameters_) {}
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2,
const Parameters &parameters,
const Ordering &ordering)
: SubgraphSolver(Ab1->eliminateSequential(ordering, EliminateQR), Ab2,
parameters) {}
: SubgraphSolver(Ab1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters, ordering) {}
#endif
/**************************************************************************************************/
VectorValues SubgraphSolver::optimize() {
VectorValues SubgraphSolver::optimize() const {
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors>(*pc_, pc_->zero(), parameters_);
return pc_->x(ybar);
}
VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
// the initial is ignored in this case ...
return optimize();
}
VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial) {

140
gtsam/linear/SubgraphSolver.h
View File

@ -28,30 +28,34 @@ class GaussianFactorGraph;
class GaussianBayesNet;
class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters {
public:
class GTSAM_EXPORT SubgraphSolverParameters
: public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() :
Base() {
}
void print() const {
Base::print();
}
virtual void print(std::ostream &os) const {
Base::print(os);
}
SubgraphSolverParameters() : Base() {}
void print() const { Base::print(); }
virtual void print(std::ostream &os) const { Base::print(os); }
};
/**
* This class implements the SPCG solver presented in Dellaert et al in IROS'10.
* This class implements the linear SPCG solver presented in Dellaert et al in
* IROS'10.
*
* Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
* \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
* \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
* Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
* Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the
* problem into \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$
* denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
*
* In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
* with the least-squares variation of the conjugate gradient method.
* \f$A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute
* \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
*
* Then we solve a reparametrized problem
* \f$ f(y) = |y|^2 + |A_c R_t^{-1} y -\bar{b_y}|^2 \f$,
* where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
*
* In the matrix form, it is equivalent to solving
* \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$.
* We can solve it with the least-squares variation of the conjugate gradient
* method.
*
* To use it in nonlinear optimization, please see the following example
*
@ -63,69 +67,83 @@ public:
*
* \nosubgrouping
*/
class GTSAM_EXPORT SubgraphSolver: public IterativeSolver {
public:
class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
public:
typedef SubgraphSolverParameters Parameters;
protected:
protected:
Parameters parameters_;
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public:
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public:
/// @name Constructors
/// @{
/**
* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
* Will throw exception if there are ternary factors or higher arity, as we use Kruskal's
* algorithm to split the graph, treating binary factors as edges.
* Given a gaussian factor graph, split it into a spanning tree (A1) + others
* (A2) for SPCG Will throw exception if there are ternary factors or higher
* arity, as we use Kruskal's algorithm to split the graph, treating binary
* factors as edges.
*/
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
const Ordering& ordering);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
const Parameters &parameters, const Ordering& ordering);
const Ordering &ordering);
/**
* The user specifies the subgraph part and the constraints part.
* May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1.
* May throw exception if A1 is underdetermined. An ordering is required to
* eliminate Ab1. We take Ab1 as a const reference, as it will be transformed
* into Rc1, but take Ab2 as a shared pointer as we need to keep it around.
*/
SubgraphSolver(const GaussianFactorGraph &Ab1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering &ordering);
/**
* The same as above, but we assume A1 was solved by caller.
* We take two shared pointers as we keep both around.
*/
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
const Parameters &parameters, const Ordering& ordering);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering& ordering);
/// The same as above, but we assume A1 was solved by caller
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const GaussianFactorGraph &Ab2, const Parameters &parameters);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters);
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters);
/// Destructor
virtual ~SubgraphSolver() {
}
virtual ~SubgraphSolver() {}
/// @}
/// @name Implement interface
/// @{
/// Optimize from zero
VectorValues optimize();
VectorValues optimize() const;
/// Optimize from given initial values
VectorValues optimize(const VectorValues &initial);
/** Interface that IterativeSolver subclasses have to implement */
/// Interface that IterativeSolver subclasses have to implement
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial);
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial) override;
protected:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
const Parameters &parameters, const Ordering &ordering)
: SubgraphSolver(*A, parameters, ordering){};
SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &,
const Parameters &, const Ordering &);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering &ordering)
: SubgraphSolver(*Ab1, Ab2, parameters, ordering) {}
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &,
const GaussianFactorGraph &, const Parameters &);
/// @}
#endif
protected:
/// Split graph using Kruskal algorithm, treating binary factors as edges.
static boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph>>
static boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph>>
splitGraph(const GaussianFactorGraph &gfg);
};
} // namespace gtsam
} // namespace gtsam

12
tests/smallExample.h
View File

@ -614,26 +614,26 @@ inline Ordering planarOrdering(size_t N) {
}
/* ************************************************************************* */
inline std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
inline std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
auto T = boost::make_shared<GaussianFactorGraph>(), C= boost::make_shared<GaussianFactorGraph>();
// Add the x11 constraint to the tree
T.push_back(original[0]);
T->push_back(original[0]);
// Add all horizontal constraints to the tree
size_t i = 1;
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++, i++)
T.push_back(original[i]);
T->push_back(original[i]);
// Add first vertical column of constraints to T, others to C
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++, i++)
if (x == 1)
T.push_back(original[i]);
T->push_back(original[i]);
else
C.push_back(original[i]);
C->push_back(original[i]);
return std::make_pair(T, C);
}

21
tests/testSubgraphSolver.cpp
View File

@ -46,6 +46,13 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
return total_error;
}
/* ************************************************************************* */
TEST( SubgraphSolver, Parameters )
{
LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
LONGS_EQUAL(500, kParameters.maxIterations());
}
/* ************************************************************************* */
TEST( SubgraphSolver, constructor1 )
{
@ -71,12 +78,12 @@ TEST( SubgraphSolver, constructor2 )
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs, so the caller can specify
// the preconditioner (Ab1) and the constraints that are left out (Ab2)
SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering);
SubgraphSolver solver(*Ab1, Ab2, kParameters, kOrdering);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
@ -91,15 +98,15 @@ TEST( SubgraphSolver, constructor3 )
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding kOrdering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
auto Rc1 = Ab1_.eliminateSequential();
auto Rc1 = Ab1->eliminateSequential();
// The third constructor allows the caller to pass an already solved preconditioner Rc1_
// as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
SubgraphSolver solver(Rc1, Ab2_, kParameters);
SubgraphSolver solver(Rc1, Ab2, kParameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}