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

Merged in fix/SubgraphSolver (pull request #406) 

Fix/SubgraphSolver

Approved-by: Yong-Dian Jian <ydjian@gmail.com>
release/4.3a0
Frank Dellaert 2019-04-07 16:52:44 +00:00
parent 26e150829a 703b56f9ff
commit 6bf605b624
8 changed files with 207 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam.h
View File

@ -1730,7 +1730,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
};

0
gtsam_unstable/base/DSFMap.h → gtsam/base/DSFMap.h
View File

3
gtsam_unstable/base/tests/testDSFMap.cpp → gtsam/base/tests/testDSFMap.cpp
View File

@ -16,7 +16,7 @@
* @brief unit tests for DSFMap
*/
#include <gtsam_unstable/base/DSFMap.h>
#include <gtsam/base/DSFMap.h>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) {
TEST(DSFMap, sets){
// Create some "matches"
typedef pair<size_t,size_t> Match;
typedef pair<size_t, set<size_t> > key_pair;
list<Match> matches;
matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);

203
gtsam/linear/SubgraphSolver.cpp
View File

@ -22,149 +22,120 @@
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/DSFMap.h>
#include <iostream>
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), ordering_(ordering) {
initialize(gfg);
parameters_(parameters) {
GaussianFactorGraph::shared_ptr Ab1,Ab2;
boost::tie(Ab1, Ab2) = splitGraph(Ab);
if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
<< " factors" << endl;
auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR);
auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(*jfg);
// 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)
: parameters_(parameters) {
auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
// Taking subgraphs [A1|b1] and [A2|b2]
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_,
EliminateQR);
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
EliminateQR);
initialize(Rc1, Ab2);
}
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,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
const GaussianFactorGraph &Ab2,
const Parameters &parameters)
: SubgraphSolver(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters) {}
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph &Ab2,
const Parameters &parameters,
const Ordering &ordering)
: SubgraphSolver(Ab1, boost::make_shared<GaussianFactorGraph>(Ab2),
parameters, ordering) {}
#endif
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(Rc1, Ab2);
}
/**************************************************************************************************/
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();
}
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) {
GaussianFactorGraph::shared_ptr Ab1 =
boost::make_shared<GaussianFactorGraph>(), Ab2 = boost::make_shared<
GaussianFactorGraph>();
boost::tie(Ab1, Ab2) = splitGraph(jfg);
if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
<< " factors" << endl;
GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
EliminateQR);
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2) {
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
const VariableIndex index(jfg);
const size_t n = index.size();
DSFVector D(n);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
size_t t = 0;
for ( const GaussianFactor::shared_ptr &gf: jfg ) {
if (gf->keys().size() > 2) {
throw runtime_error(
"SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
bool augment = false;
/* check whether this factor should be augmented to the "tree" graph */
if (gf->keys().size() == 1)
augment = true;
else {
const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u),
v_root = D.find(v);
if (u_root != v_root) {
t++;
augment = true;
D.merge(u_root, v_root);
}
}
if (augment)
At->push_back(gf);
else
Ac->push_back(gf);
}
return boost::tie(At, Ac);
}
/****************************************************************************/
VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial) {
return VectorValues();
}
/**************************************************************************************************/
// Run Kruskal algorithm to create a spanning tree of factor "edges".
// Edges are not weighted, and will only work if factors are binary.
// Unary factors are ignored for this purpose and added to tree graph.
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
// Create disjoint set forest data structure for Kruskal algorithm
DSFMap<Key> dsf;
// Allocate two output graphs
auto tree = boost::make_shared<GaussianFactorGraph>();
auto constraints = boost::make_shared<GaussianFactorGraph>();
// Loop over all "edges"
for ( const auto &factor: factorGraph ) {
// Fail on > binary factors
const auto& keys = factor->keys();
if (keys.size() > 2) {
throw runtime_error(
"SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
}
// check whether this factor should be augmented to the "tree" graph
if (keys.size() == 1)
tree->push_back(factor);
else if (dsf.find(keys[0]) != dsf.find(keys[1])) {
// We merge two trees joined by this edge if they are still disjoint
tree->push_back(factor);
// Record this fact in DSF
dsf.merge(keys[0], keys[1]);
} else {
// This factor would create a loop, so we add it to non-tree edges...
constraints->push_back(factor);
}
}
return boost::tie(tree, constraints);
}
/****************************************************************************/
} // \namespace gtsam

147
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,72 +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_;
Ordering ordering_;
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public:
/// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
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.
*/
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 specify the subgraph part and the constraint part
* may throw exception if A1 is underdetermined
* The user specifies the subgraph part and the constraints part.
* 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 the A1 is solved before */
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering& ordering);
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:
/// @}
void initialize(const GaussianFactorGraph &jfg);
void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2);
#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
boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph> >
protected:
/// Split graph using Kruskal algorithm, treating binary factors as edges.
static boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph>>
splitGraph(const GaussianFactorGraph &gfg);
};
} // namespace gtsam
} // namespace gtsam

2
gtsam_unstable/timing/timeDSFvariants.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/base/DSFVector.h>
#include <gtsam_unstable/base/DSF.h>
#include <gtsam_unstable/base/DSFMap.h>
#include <gtsam/base/DSFMap.h>
#include <boost/random.hpp>
#include <boost/timer.hpp>

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);
}

60
tests/testSubgraphSolver.cpp
View File

@ -15,26 +15,27 @@
* @author Yong-Dian Jian
**/
#include <CppUnitLite/TestHarness.h>
#if 0
#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
static size_t N = 3;
static SubgraphSolverParameters kParameters;
static auto kOrdering = example::planarOrdering(N);
/* ************************************************************************* */
/** unnormalized error */
@ -45,6 +46,12 @@ 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 )
@ -52,13 +59,11 @@ TEST( SubgraphSolver, constructor1 )
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// The first constructor just takes a factor graph (and parameters)
// The first constructor just takes a factor graph (and kParameters)
// and it will split the graph into A1 and A2, where A1 is a spanning tree
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab, parameters);
SubgraphSolver solver(Ab, kParameters, kOrdering);
VectorValues optimized = solver.optimize(); // does PCG optimization
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
@ -70,16 +75,15 @@ TEST( SubgraphSolver, constructor2 )
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// Get the spanning tree
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)
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab1_, Ab2_, parameters);
// 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);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
@ -91,26 +95,22 @@ TEST( SubgraphSolver, constructor3 )
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// Get the spanning tree and corresponding kOrdering
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 via QR factorization, where R1 is square UT
GaussianBayesNet::shared_ptr Rc1 = //
EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
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
SubgraphSolverParameters parameters;
SubgraphSolver solver(Rc1, Ab2_, parameters);
SubgraphSolver solver(Rc1, Ab2, kParameters);
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
#endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */