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

Removed shared_ptr from Bayes nets and factor graphs

release/4.3a0
Frank Dellaert 2021-12-30 12:12:54 -05:00
parent 2f7ebb4d0c
commit c659336cf8
9 changed files with 106 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/linear/SubgraphBuilder.cpp
View File

@ -446,30 +446,29 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(
} }
/*****************************************************************************/ /*****************************************************************************/
GaussianFactorGraph::shared_ptr buildFactorSubgraph( GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const Subgraph &subgraph, const Subgraph &subgraph,
const bool clone) { const bool clone) {
auto subgraphFactors = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph subgraphFactors;
subgraphFactors->reserve(subgraph.size()); subgraphFactors.reserve(subgraph.size());
for (const auto &e : subgraph) { for (const auto &e : subgraph) {
const auto factor = gfg[e.index]; const auto factor = gfg[e.index];
subgraphFactors->push_back(clone ? factor->clone() : factor); subgraphFactors.push_back(clone ? factor->clone() : factor);
} }
return subgraphFactors; return subgraphFactors;
} }
/**************************************************************************************************/ /**************************************************************************************************/
std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> // std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
splitFactorGraph(const GaussianFactorGraph &factorGraph, const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
const Subgraph &subgraph) {
// Get the subgraph by calling cheaper method // Get the subgraph by calling cheaper method
auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false); auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
// Now, copy all factors then set subGraph factors to zero // Now, copy all factors then set subGraph factors to zero
auto remaining = boost::make_shared<GaussianFactorGraph>(factorGraph); GaussianFactorGraph remaining = factorGraph;
for (const auto &e : subgraph) { for (const auto &e : subgraph) {
remaining->remove(e.index); remaining.remove(e.index);
} }
return std::make_pair(subgraphFactors, remaining); return std::make_pair(subgraphFactors, remaining);

9
gtsam/linear/SubgraphBuilder.h
View File

@ -172,12 +172,13 @@ class GTSAM_EXPORT SubgraphBuilder {
}; };
/** Select the factors in a factor graph according to the subgraph. */ /** Select the factors in a factor graph according to the subgraph. */
boost::shared_ptr<GaussianFactorGraph> buildFactorSubgraph( GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); const Subgraph &subgraph,
const bool clone);
/** Split the graph into a subgraph and the remaining edges. /** Split the graph into a subgraph and the remaining edges.
* Note that the remaining factorgraph has null factors. */ * Note that the remaining factorgraph has null factors. */
std::pair<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> > std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
} // namespace gtsam } // namespace gtsam

50
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -77,16 +77,16 @@ static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
/* ************************************************************************* */ /* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
// Cholesky) // Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors( static GaussianFactorGraph convertToJacobianFactors(
const GaussianFactorGraph &gfg) { const GaussianFactorGraph &gfg) {
auto result = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph result;
for (const auto &factor : gfg) for (const auto &factor : gfg)
if (factor) { if (factor) {
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor); auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if (!jf) { if (!jf) {
jf = boost::make_shared<JacobianFactor>(*factor); jf = boost::make_shared<JacobianFactor>(*factor);
} }
result->push_back(jf); result.push_back(jf);
} }
return result; return result;
} }
@ -96,42 +96,42 @@ SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParam
parameters_(p) {} parameters_(p) {}
/* ************************************************************************* */ /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, SubgraphPreconditioner::SubgraphPreconditioner(const GaussianFactorGraph& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) : const GaussianBayesNet& Rc1, const VectorValues& xbar, const SubgraphPreconditionerParameters &p) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), Ab2_(convertToJacobianFactors(Ab2)), Rc1_(Rc1), xbar_(xbar),
b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) { b2bar_(-Ab2_.gaussianErrors(xbar)), parameters_(p) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// x = xbar + inv(R1)*y // x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + Rc1_->backSubstitute(y); return xbar_ + Rc1_.backSubstitute(y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const { double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y); Errors e(y);
VectorValues x = this->x(y); VectorValues x = this->x(y);
Errors e2 = Ab2()->gaussianErrors(x); Errors e2 = Ab2_.gaussianErrors(x);
return 0.5 * (dot(e, e) + dot(e2,e2)); return 0.5 * (dot(e, e) + dot(e2,e2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ VectorValues x = Rc1_.backSubstitute(y); /* inv(R1)*y */
Errors e = (*Ab2() * x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ Errors e = Ab2_ * x - b2bar_; /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x); VectorValues v = VectorValues::Zero(x);
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ Ab2_.transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + Rc1()->backSubstituteTranspose(v); return y + Rc1_.backSubstituteTranspose(v);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { Errors SubgraphPreconditioner::operator*(const VectorValues &y) const {
Errors e(y); Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */ Errors e2 = Ab2_ * x; /* A2*x */
e.splice(e.end(), e2); e.splice(e.end(), e2);
return e; return e;
} }
@ -147,8 +147,8 @@ void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) c
} }
// Add A2 contribution // Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y VectorValues x = Rc1_.backSubstitute(y); // x=inv(R1)*y
Ab2()->multiplyInPlace(x, ei); // use iterator version Ab2_.multiplyInPlace(x, ei); // use iterator version
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -190,14 +190,14 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
while (it != end) e2.push_back(*(it++)); while (it != end) e2.push_back(*(it++));
VectorValues x = VectorValues::Zero(y); // x = 0 VectorValues x = VectorValues::Zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 Ab2_.transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x y += alpha * Rc1_.backSubstituteTranspose(x); // y += alpha*inv(R1')*x
} }
/* ************************************************************************* */ /* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const { void SubgraphPreconditioner::print(const std::string& s) const {
cout << s << endl; cout << s << endl;
Ab2_->print(); Ab2_.print();
} }
/*****************************************************************************/ /*****************************************************************************/
@ -205,7 +205,7 @@ void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
assert(x.size() == y.size()); assert(x.size() == y.size());
/* back substitute */ /* back substitute */
for (const auto &cg : boost::adaptors::reverse(*Rc1_)) { for (const auto &cg : boost::adaptors::reverse(Rc1_)) {
/* collect a subvector of x that consists of the parents of cg (S) */ /* collect a subvector of x that consists of the parents of cg (S) */
const KeyVector parentKeys(cg->beginParents(), cg->endParents()); const KeyVector parentKeys(cg->beginParents(), cg->endParents());
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
@ -228,7 +228,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
std::copy(y.data(), y.data() + y.rows(), x.data()); std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */ /* in place back substitute */
for (const auto &cg : *Rc1_) { for (const auto &cg : Rc1_) {
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys); const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
const Vector solFrontal = const Vector solFrontal =
@ -261,10 +261,10 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
keyInfo_ = keyInfo; keyInfo_ = keyInfo;
/* build factor subgraph */ /* build factor subgraph */
GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true); auto gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
/* factorize and cache BayesNet */ /* factorize and cache BayesNet */
Rc1_ = gfg_subgraph->eliminateSequential(); Rc1_ = gfg_subgraph.eliminateSequential();
} }
/*****************************************************************************/ /*****************************************************************************/

25
gtsam/linear/SubgraphPreconditioner.h
View File

@ -19,6 +19,8 @@
#include <gtsam/linear/SubgraphBuilder.h> #include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -53,16 +55,12 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr; typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors;
private: private:
sharedFG Ab2_; GaussianFactorGraph Ab2_;
sharedBayesNet Rc1_; GaussianBayesNet Rc1_;
sharedValues xbar_; ///< A1 \ b1 VectorValues xbar_; ///< A1 \ b1
sharedErrors b2bar_; ///< A2*xbar - b2 Errors b2bar_; ///< A2*xbar - b2
KeyInfo keyInfo_; KeyInfo keyInfo_;
SubgraphPreconditionerParameters parameters_; SubgraphPreconditionerParameters parameters_;
@ -77,7 +75,7 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1 * @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1 * @param xbar: the solution to R1*x=c1
*/ */
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar, SubgraphPreconditioner(const GaussianFactorGraph& Ab2, const GaussianBayesNet& Rc1, const VectorValues& xbar,
const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
~SubgraphPreconditioner() override {} ~SubgraphPreconditioner() override {}
@ -86,13 +84,13 @@ namespace gtsam {
void print(const std::string& s = "SubgraphPreconditioner") const; void print(const std::string& s = "SubgraphPreconditioner") const;
/** Access Ab2 */ /** Access Ab2 */
const sharedFG& Ab2() const { return Ab2_; } const GaussianFactorGraph& Ab2() const { return Ab2_; }
/** Access Rc1 */ /** Access Rc1 */
const sharedBayesNet& Rc1() const { return Rc1_; } const GaussianBayesNet& Rc1() const { return Rc1_; }
/** Access b2bar */ /** Access b2bar */
const sharedErrors b2bar() const { return b2bar_; } const Errors b2bar() const { return b2bar_; }
/** /**
* Add zero-mean i.i.d. Gaussian prior terms to each variable * Add zero-mean i.i.d. Gaussian prior terms to each variable
@ -104,8 +102,7 @@ namespace gtsam {
/* A zero VectorValues with the structure of xbar */ /* A zero VectorValues with the structure of xbar */
VectorValues zero() const { VectorValues zero() const {
assert(xbar_); return VectorValues::Zero(xbar_);
return VectorValues::Zero(*xbar_);
} }
/** /**

18
gtsam/linear/SubgraphSolver.cpp
View File

@ -34,24 +34,24 @@ namespace gtsam {
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
const Parameters &parameters, const Ordering& ordering) : const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters) { parameters_(parameters) {
GaussianFactorGraph::shared_ptr Ab1,Ab2; GaussianFactorGraph Ab1, Ab2;
std::tie(Ab1, Ab2) = splitGraph(Ab); std::tie(Ab1, Ab2) = splitGraph(Ab);
if (parameters_.verbosity()) if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() cout << "Split A into (A1) " << Ab1.size() << " and (A2) " << Ab2.size()
<< " factors" << endl; << " factors" << endl;
auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR); auto Rc1 = Ab1.eliminateSequential(ordering, EliminateQR);
auto xbar = boost::make_shared<VectorValues>(Rc1->optimize()); auto xbar = Rc1.optimize();
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
/**************************************************************************************************/ /**************************************************************************************************/
// Taking eliminated tree [R1|c] and constraint graph [A2|b2] // Taking eliminated tree [R1|c] and constraint graph [A2|b2]
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, SubgraphSolver::SubgraphSolver(const GaussianBayesNet &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2, const GaussianFactorGraph &Ab2,
const Parameters &parameters) const Parameters &parameters)
: parameters_(parameters) { : parameters_(parameters) {
auto xbar = boost::make_shared<VectorValues>(Rc1->optimize()); auto xbar = Rc1.optimize();
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
@ -59,7 +59,7 @@ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
// Taking subgraphs [A1|b1] and [A2|b2] // Taking subgraphs [A1|b1] and [A2|b2]
// delegate up // delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const GaussianFactorGraph &Ab2,
const Parameters &parameters, const Parameters &parameters,
const Ordering &ordering) const Ordering &ordering)
: SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2, : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
@ -78,7 +78,7 @@ VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
return VectorValues(); return VectorValues();
} }
/**************************************************************************************************/ /**************************************************************************************************/
pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> // pair<GaussianFactorGraph, GaussianFactorGraph> //
SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
/* identify the subgraph structure */ /* identify the subgraph structure */

11
gtsam/linear/SubgraphSolver.h
View File

@ -99,15 +99,13 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
* eliminate Ab1. We take Ab1 as a const reference, as it will be transformed * 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. * into Rc1, but take Ab2 as a shared pointer as we need to keep it around.
*/ */
SubgraphSolver(const GaussianFactorGraph &Ab1, SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering &ordering); const Parameters &parameters, const Ordering &ordering);
/** /**
* The same as above, but we assume A1 was solved by caller. * The same as above, but we assume A1 was solved by caller.
* We take two shared pointers as we keep both around. * We take two shared pointers as we keep both around.
*/ */
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, SubgraphSolver(const GaussianBayesNet &Rc1, const GaussianFactorGraph &Ab2,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters); const Parameters &parameters);
/// Destructor /// Destructor
@ -131,9 +129,8 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
/// @{ /// @{
/// Split graph using Kruskal algorithm, treating binary factors as edges. /// Split graph using Kruskal algorithm, treating binary factors as edges.
std::pair < boost::shared_ptr<GaussianFactorGraph>, std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
boost::shared_ptr<GaussianFactorGraph> > splitGraph( const GaussianFactorGraph &gfg);
const GaussianFactorGraph &gfg);
/// @} /// @}
}; };

15
tests/smallExample.h
View File

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

61
tests/testSubgraphPreconditioner.cpp
View File

@ -77,8 +77,8 @@ TEST(SubgraphPreconditioner, planarGraph) {
DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue
// Check that xtrue is optimal // Check that xtrue is optimal
GaussianBayesNet::shared_ptr R1 = A.eliminateSequential(); GaussianBayesNet R1 = A.eliminateSequential();
VectorValues actual = R1->optimize(); VectorValues actual = R1.optimize();
EXPECT(assert_equal(xtrue, actual)); EXPECT(assert_equal(xtrue, actual));
} }
@ -90,14 +90,14 @@ TEST(SubgraphPreconditioner, splitOffPlanarTree) {
boost::tie(A, xtrue) = planarGraph(3); boost::tie(A, xtrue) = planarGraph(3);
// Get the spanning tree and constraints, and check their sizes // Get the spanning tree and constraints, and check their sizes
GaussianFactorGraph::shared_ptr T, C; GaussianFactorGraph T, C;
boost::tie(T, C) = splitOffPlanarTree(3, A); boost::tie(T, C) = splitOffPlanarTree(3, A);
LONGS_EQUAL(9, T->size()); LONGS_EQUAL(9, T.size());
LONGS_EQUAL(4, C->size()); LONGS_EQUAL(4, C.size());
// Check that the tree can be solved to give the ground xtrue // Check that the tree can be solved to give the ground xtrue
GaussianBayesNet::shared_ptr R1 = T->eliminateSequential(); GaussianBayesNet R1 = T.eliminateSequential();
VectorValues xbar = R1->optimize(); VectorValues xbar = R1.optimize();
EXPECT(assert_equal(xtrue, xbar)); EXPECT(assert_equal(xtrue, xbar));
} }
@ -110,31 +110,29 @@ TEST(SubgraphPreconditioner, system) {
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and remaining graph // Get the spanning tree and remaining graph
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
// Eliminate the spanning tree to build a prior // Eliminate the spanning tree to build a prior
const Ordering ord = planarOrdering(N); const Ordering ord = planarOrdering(N);
auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1 auto Rc1 = Ab1.eliminateSequential(ord); // R1*x-c1
VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 VectorValues xbar = Rc1.optimize(); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system // Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared( const SubgraphPreconditioner system(Ab2, Rc1, xbar);
new VectorValues(xbar)); // TODO: horrible
const SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Get corresponding matrices for tests. Add dummy factors to Ab2 to make // Get corresponding matrices for tests. Add dummy factors to Ab2 to make
// sure it works with the ordering. // sure it works with the ordering.
Ordering ordering = Rc1->ordering(); // not ord in general! Ordering ordering = Rc1.ordering(); // not ord in general!
Ab2->add(key(1, 1), Z_2x2, Z_2x1); Ab2.add(key(1, 1), Z_2x2, Z_2x1);
Ab2->add(key(1, 2), Z_2x2, Z_2x1); Ab2.add(key(1, 2), Z_2x2, Z_2x1);
Ab2->add(key(1, 3), Z_2x2, Z_2x1); Ab2.add(key(1, 3), Z_2x2, Z_2x1);
Matrix A, A1, A2; Matrix A, A1, A2;
Vector b, b1, b2; Vector b, b1, b2;
std::tie(A, b) = Ab.jacobian(ordering); std::tie(A, b) = Ab.jacobian(ordering);
std::tie(A1, b1) = Ab1->jacobian(ordering); std::tie(A1, b1) = Ab1.jacobian(ordering);
std::tie(A2, b2) = Ab2->jacobian(ordering); std::tie(A2, b2) = Ab2.jacobian(ordering);
Matrix R1 = Rc1->matrix(ordering).first; Matrix R1 = Rc1.matrix(ordering).first;
Matrix Abar(13 * 2, 9 * 2); Matrix Abar(13 * 2, 9 * 2);
Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2); Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
Abar.bottomRows(8) = A2.topRows(8) * R1.inverse(); Abar.bottomRows(8) = A2.topRows(8) * R1.inverse();
@ -151,7 +149,7 @@ TEST(SubgraphPreconditioner, system) {
y1[key(3, 3)] = Vector2(1.0, -1.0); y1[key(3, 3)] = Vector2(1.0, -1.0);
// Check backSubstituteTranspose works with R1 // Check backSubstituteTranspose works with R1
VectorValues actual = Rc1->backSubstituteTranspose(y1); VectorValues actual = Rc1.backSubstituteTranspose(y1);
Vector expected = R1.transpose().inverse() * vec(y1); Vector expected = R1.transpose().inverse() * vec(y1);
EXPECT(assert_equal(expected, vec(actual))); EXPECT(assert_equal(expected, vec(actual)));
@ -230,7 +228,7 @@ TEST(SubgraphSolver, Solves) {
system.build(Ab, keyInfo, lambda); system.build(Ab, keyInfo, lambda);
// Create a perturbed (non-zero) RHS // Create a perturbed (non-zero) RHS
const auto xbar = system.Rc1()->optimize(); // merely for use in zero below const auto xbar = system.Rc1().optimize(); // merely for use in zero below
auto values_y = VectorValues::Zero(xbar); auto values_y = VectorValues::Zero(xbar);
auto it = values_y.begin(); auto it = values_y.begin();
it->second.setConstant(100); it->second.setConstant(100);
@ -238,13 +236,13 @@ TEST(SubgraphSolver, Solves) {
it->second.setConstant(-100); it->second.setConstant(-100);
// Solve the VectorValues way // Solve the VectorValues way
auto values_x = system.Rc1()->backSubstitute(values_y); auto values_x = system.Rc1().backSubstitute(values_y);
// Solve the matrix way, this really just checks BN::backSubstitute // Solve the matrix way, this really just checks BN::backSubstitute
// This only works with Rc1 ordering, not with keyInfo ! // This only works with Rc1 ordering, not with keyInfo !
// TODO(frank): why does this not work with an arbitrary ordering? // TODO(frank): why does this not work with an arbitrary ordering?
const auto ord = system.Rc1()->ordering(); const auto ord = system.Rc1().ordering();
const Matrix R1 = system.Rc1()->matrix(ord).first; const Matrix R1 = system.Rc1().matrix(ord).first;
auto ord_y = values_y.vector(ord); auto ord_y = values_y.vector(ord);
auto vector_x = R1.inverse() * ord_y; auto vector_x = R1.inverse() * ord_y;
EXPECT(assert_equal(vector_x, values_x.vector(ord))); EXPECT(assert_equal(vector_x, values_x.vector(ord)));
@ -261,7 +259,7 @@ TEST(SubgraphSolver, Solves) {
// Test that transposeSolve does implement x = R^{-T} y // Test that transposeSolve does implement x = R^{-T} y
// We do this by asserting it gives same answer as backSubstituteTranspose // We do this by asserting it gives same answer as backSubstituteTranspose
auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y); auto values_x2 = system.Rc1().backSubstituteTranspose(values_y);
Vector solveT_x = Vector::Zero(N); Vector solveT_x = Vector::Zero(N);
system.transposeSolve(vector_y, solveT_x); system.transposeSolve(vector_y, solveT_x);
EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
@ -277,18 +275,15 @@ TEST(SubgraphPreconditioner, conjugateGradients) {
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree // Get the spanning tree
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
// Eliminate the spanning tree to build a prior // Eliminate the spanning tree to build a prior
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianBayesNet Rc1 = Ab1.eliminateSequential(); // R1*x-c1
Ab1->eliminateSequential(); // R1*x-c1 VectorValues xbar = Rc1.optimize(); // xbar = inv(R1)*c1
VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system // Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared( SubgraphPreconditioner system(Ab2, Rc1, xbar);
new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1 // Create zero config y0 and perturbed config y1
VectorValues y0 = VectorValues::Zero(xbar); VectorValues y0 = VectorValues::Zero(xbar);

14
tests/testSubgraphSolver.cpp
View File

@ -68,10 +68,10 @@ TEST( SubgraphSolver, splitFactorGraph )
auto subgraph = builder(Ab); auto subgraph = builder(Ab);
EXPECT_LONGS_EQUAL(9, subgraph.size()); EXPECT_LONGS_EQUAL(9, subgraph.size());
GaussianFactorGraph::shared_ptr Ab1, Ab2; GaussianFactorGraph Ab1, Ab2;
std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph); std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph);
EXPECT_LONGS_EQUAL(9, Ab1->size()); EXPECT_LONGS_EQUAL(9, Ab1.size());
EXPECT_LONGS_EQUAL(13, Ab2->size()); EXPECT_LONGS_EQUAL(13, Ab2.size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -99,12 +99,12 @@ TEST( SubgraphSolver, constructor2 )
std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree // Get the spanning tree
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs, so the caller can specify // The second constructor takes two factor graphs, so the caller can specify
// the preconditioner (Ab1) and the constraints that are left out (Ab2) // 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(); VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
} }
@ -119,11 +119,11 @@ TEST( SubgraphSolver, constructor3 )
std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
// Get the spanning tree and corresponding kOrdering // Get the spanning tree and corresponding kOrdering
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT // 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_ // 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 // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before