Added Bayes Net and Subgraph preconditioners to gtsam (developed in CitySLAM project)
Frank Dellaert
parent
730f4a546f
commit
a1d14ba2ae
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* BayesNetPreconditioner.cpp
|
||||
* Created on: Dec 31, 2009
|
||||
* @Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include "BayesNetPreconditioner.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
BayesNetPreconditioner::BayesNetPreconditioner(const GaussianFactorGraph& Ab,
|
||||
const GaussianBayesNet& Rd) :
|
||||
Ab_(Ab), Rd_(Rd) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// R*x = y by solving x=inv(R)*y
|
||||
VectorConfig BayesNetPreconditioner::backSubstitute(const VectorConfig& y) const {
|
||||
return gtsam::backSubstitute(Rd_, y);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gy=inv(L)*gx by solving L*gy=gx.
|
||||
VectorConfig BayesNetPreconditioner::backSubstituteTranspose(
|
||||
const VectorConfig& gx) const {
|
||||
return gtsam::backSubstituteTranspose(Rd_, gx);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double BayesNetPreconditioner::error(const VectorConfig& y) const {
|
||||
return Ab_.error(x(y));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gradient is inv(R')*A'*(A*inv(R)*y-b),
|
||||
VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const {
|
||||
VectorConfig gx = Ab_ ^ Ab_.errors(x(y));
|
||||
return gtsam::backSubstituteTranspose(Rd_, gx);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Apply operator *
|
||||
Errors BayesNetPreconditioner::operator*(const VectorConfig& y) const {
|
||||
return Ab_ * x(y);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Apply operator inv(R')*A'*e
|
||||
VectorConfig BayesNetPreconditioner::operator^(const Errors& e) const {
|
||||
VectorConfig x = Ab_ ^ e; // x = A'*e2
|
||||
return gtsam::backSubstituteTranspose(Rd_, x);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/*
|
||||
* BayesNetPreconditioner.h
|
||||
* Created on: Dec 31, 2009
|
||||
* @Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifndef BAYESNETPRECONDITIONER_H_
|
||||
#define BAYESNETPRECONDITIONER_H_
|
||||
|
||||
#include "GaussianFactorGraph.h"
|
||||
#include "GaussianBayesNet.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Upper-triangular preconditioner R for the system |A*x-b|^2
|
||||
* The new system will be |A*inv(R)*y-b|^2, i.e., R*x=y
|
||||
* This class can solve for x=inv(R)*y by back-substituting R*x=y
|
||||
* and also apply the chain rule gy=inv(R')*gx by solving R'*gy=gx.
|
||||
* This is not used currently, just to debug operators below
|
||||
*/
|
||||
class BayesNetPreconditioner {
|
||||
|
||||
// The original system
|
||||
const GaussianFactorGraph& Ab_;
|
||||
|
||||
// The preconditioner
|
||||
const GaussianBayesNet& Rd_;
|
||||
|
||||
public:
|
||||
|
||||
/** Constructor */
|
||||
BayesNetPreconditioner(const GaussianFactorGraph& Ab,
|
||||
const GaussianBayesNet& Rd);
|
||||
|
||||
// R*x = y by solving x=inv(R)*y
|
||||
VectorConfig backSubstitute(const VectorConfig& y) const;
|
||||
|
||||
// gy=inv(L)*gx by solving L*gy=gx.
|
||||
VectorConfig backSubstituteTranspose(const VectorConfig& gx) const;
|
||||
|
||||
/* x = inv(R)*y */
|
||||
inline VectorConfig x(const VectorConfig& y) const {
|
||||
return backSubstitute(y);
|
||||
}
|
||||
|
||||
/* error, given y */
|
||||
double error(const VectorConfig& y) const;
|
||||
|
||||
/** gradient */
|
||||
VectorConfig gradient(const VectorConfig& y) const;
|
||||
|
||||
/** Apply operator A */
|
||||
Errors operator*(const VectorConfig& y) const;
|
||||
|
||||
/** Apply operator A' */
|
||||
VectorConfig operator^(const Errors& e) const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif /* BAYESNETPRECONDITIONER_H_ */
|
||||
|
|
@ -96,9 +96,9 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp
|
|||
testBinaryBayesNet_LDADD = libgtsam.la
|
||||
|
||||
# Gaussian inference
|
||||
headers += GaussianFactorSet.h iterative-inl.h
|
||||
sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp iterative.cpp
|
||||
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testIterative
|
||||
headers += GaussianFactorSet.h
|
||||
sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
|
||||
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet
|
||||
testVectorConfig_SOURCES = testVectorConfig.cpp
|
||||
testVectorConfig_LDADD = libgtsam.la
|
||||
testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp
|
||||
|
|
@ -109,8 +109,17 @@ testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp
|
|||
testGaussianConditional_LDADD = libgtsam.la
|
||||
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
|
||||
testGaussianBayesNet_LDADD = libgtsam.la
|
||||
|
||||
# Iterative Methods
|
||||
headers += iterative-inl.h
|
||||
sources += iterative.cpp BayesNetPreconditioner.cpp subgraphPreconditioner.cpp
|
||||
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
|
||||
testIterative_SOURCES = $(example) testIterative.cpp
|
||||
testIterative_LDADD = libgtsam.la
|
||||
testBayesNetPreconditioner_SOURCES = $(example) testBayesNetPreconditioner.cpp
|
||||
testBayesNetPreconditioner_LDADD = libgtsam.la
|
||||
testSubgraphPreconditioner_SOURCES = $(example) testSubgraphPreconditioner.cpp
|
||||
testSubgraphPreconditioner_LDADD = libgtsam.la
|
||||
|
||||
# not the correct way, I'm sure: Kai ?
|
||||
timeGaussianFactor: timeGaussianFactor.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,101 @@
|
|||
/*
|
||||
* SubgraphPreconditioner.cpp
|
||||
* Created on: Dec 31, 2009
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include "SubgraphPreconditioner.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
SubgraphPreconditioner::SubgraphPreconditioner(const GaussianBayesNet& Rc1,
|
||||
const GaussianFactorGraph& Ab2, const VectorConfig& xbar) :
|
||||
Rc1_(Rc1), Ab2_(Ab2), xbar_(xbar), b2bar_(Ab2_.errors(xbar)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// x = xbar + inv(R1)*y
|
||||
VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const {
|
||||
return xbar_ + gtsam::backSubstitute(Rc1_, y);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SubgraphPreconditioner::error(const VectorConfig& y) const {
|
||||
|
||||
Errors e;
|
||||
|
||||
// Use BayesNet order to add y contributions in order
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
|
||||
const string& j = cg->key();
|
||||
e.push_back(y[j]); // append y
|
||||
}
|
||||
|
||||
// Add A2 contribution
|
||||
VectorConfig x = this->x(y);
|
||||
Errors e2 = Ab2_.errors(x);
|
||||
e.splice(e.end(), e2);
|
||||
|
||||
return 0.5 * dot(e, e);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
|
||||
VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const {
|
||||
VectorConfig x = this->x(y); // x = inv(R1)*y
|
||||
VectorConfig gx2 = Ab2_ ^ Ab2_.errors(x);
|
||||
VectorConfig gy2 = gtsam::backSubstituteTranspose(Rc1_, gx2); // inv(R1')*gx2
|
||||
return y + gy2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
|
||||
Errors SubgraphPreconditioner::operator*(const VectorConfig& y) const {
|
||||
|
||||
Errors e;
|
||||
|
||||
// Use BayesNet order to add y contributions in order
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
|
||||
const string& j = cg->key();
|
||||
e.push_back(y[j]); // append y
|
||||
}
|
||||
|
||||
// Add A2 contribution
|
||||
VectorConfig x = gtsam::backSubstitute(Rc1_, y); // x=inv(R1)*y
|
||||
Errors e2 = Ab2_ * x; // A2*x
|
||||
e.splice(e.end(), e2);
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
|
||||
VectorConfig SubgraphPreconditioner::operator^(const Errors& e) const {
|
||||
|
||||
VectorConfig y1;
|
||||
|
||||
// Use BayesNet order to remove y contributions in order
|
||||
Errors::const_iterator it = e.begin();
|
||||
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
|
||||
const string& j = cg->key();
|
||||
const Vector& ej = *(it++);
|
||||
y1.insert(j,ej);
|
||||
}
|
||||
|
||||
// create e2 with what's left of e
|
||||
Errors e2;
|
||||
while (it != e.end())
|
||||
e2.push_back(*(it++));
|
||||
|
||||
// get A2 part,
|
||||
VectorConfig x = Ab2_ ^ e2; // x = A2'*e2
|
||||
VectorConfig y2 = gtsam::backSubstituteTranspose(Rc1_, x); // inv(R1')*x;
|
||||
|
||||
return y1 + y2;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // nsamespace gtsam
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
/*
|
||||
* SubgraphPreconditioner.h
|
||||
* Created on: Dec 31, 2009
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifndef SUBGRAPHPRECONDITIONER_H_
|
||||
#define SUBGRAPHPRECONDITIONER_H_
|
||||
|
||||
#include "GaussianFactorGraph.h"
|
||||
#include "GaussianBayesNet.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Subgraph conditioner class, as explained in the RSS 2010 submission.
|
||||
* Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
|
||||
* We solve R1*x=c1, and make the substitution y=R1*x-c1.
|
||||
* To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2.
|
||||
* Then solve for yhat using CG, and solve for xhat = system.x(yhat).
|
||||
*/
|
||||
class SubgraphPreconditioner {
|
||||
|
||||
private:
|
||||
|
||||
const GaussianBayesNet& Rc1_;
|
||||
|
||||
const GaussianFactorGraph& Ab2_;
|
||||
const VectorConfig& xbar_;
|
||||
const Errors b2bar_; /** b2 - A2*xbar */
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param Rc1: the Bayes Net R1*x=c1
|
||||
* @param Ab2: the Graph A2*x=b2
|
||||
* @param xbar: the solution to R1*x=c1
|
||||
*/
|
||||
SubgraphPreconditioner(const GaussianBayesNet& Rc1,
|
||||
const GaussianFactorGraph& Ab2, const VectorConfig& xbar);
|
||||
|
||||
/* x = xbar + inv(R1)*y */
|
||||
VectorConfig x(const VectorConfig& y) const;
|
||||
|
||||
/* error, given y */
|
||||
double error(const VectorConfig& y) const;
|
||||
|
||||
/** gradient */
|
||||
VectorConfig gradient(const VectorConfig& y) const;
|
||||
|
||||
/** Apply operator A */
|
||||
Errors operator*(const VectorConfig& y) const;
|
||||
|
||||
/** Apply operator A' */
|
||||
VectorConfig operator^(const Errors& e) const;
|
||||
};
|
||||
|
||||
} // nsamespace gtsam
|
||||
|
||||
#endif /* SUBGRAPHPRECONDITIONER_H_ */
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/**
|
||||
* @file testBayesNetConditioner.cpp
|
||||
* @brief Unit tests for BayesNetConditioner
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "Ordering.h"
|
||||
#include "smallExample.h"
|
||||
#include "BayesNetPreconditioner.h"
|
||||
#include "iterative-inl.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesNetPreconditioner, operators )
|
||||
{
|
||||
// Build a simple Bayes net
|
||||
// small Bayes Net x <- y, x=2D, y=1D
|
||||
// 1 2 3 x1 0
|
||||
// 0 1 2 * x2 = 0
|
||||
// 0 0 1 x3 1
|
||||
|
||||
// Create a scalar Gaussian on y
|
||||
GaussianBayesNet bn = scalarGaussian("y", 1, 0.1);
|
||||
|
||||
// Add a conditional node with one parent |Rx+Sy-d|
|
||||
Matrix R11 = Matrix_(2, 2, 1.0, 2.0, 0.0, 1.0), S12 = Matrix_(2, 1, 3.0, 2.0);
|
||||
Vector d = zero(2);
|
||||
Vector sigmas = Vector_(2, 0.1, 0.1);
|
||||
push_front(bn, "x", d, R11, "y", S12, sigmas);
|
||||
|
||||
// Create Precondioner class
|
||||
GaussianFactorGraph dummy;
|
||||
BayesNetPreconditioner P(dummy,bn);
|
||||
|
||||
// inv(R1)*d should equal solution [1;-2;1]
|
||||
VectorConfig D;
|
||||
D.insert("x", d);
|
||||
D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma
|
||||
VectorConfig expected1;
|
||||
expected1.insert("x", Vector_(2, 1.0, -2.0));
|
||||
expected1.insert("y", Vector_(1, 1.0));
|
||||
VectorConfig actual1 = P.backSubstitute(D);
|
||||
CHECK(assert_equal(expected1,actual1));
|
||||
|
||||
// inv(R1')*ones should equal ?
|
||||
VectorConfig ones;
|
||||
ones.insert("x", Vector_(2, 1.0, 1.0));
|
||||
ones.insert("y", Vector_(1, 1.0));
|
||||
VectorConfig expected2;
|
||||
expected2.insert("x", Vector_(2, 0.1, -0.1));
|
||||
expected2.insert("y", Vector_(1, 0.0));
|
||||
VectorConfig actual2 = P.backSubstituteTranspose(ones);
|
||||
CHECK(assert_equal(expected2,actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesNetPreconditioner, conjugateGradients )
|
||||
{
|
||||
// Build a planar graph
|
||||
GaussianFactorGraph Ab;
|
||||
VectorConfig xtrue;
|
||||
size_t N = 3;
|
||||
boost::tie(Ab, xtrue) = 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);
|
||||
|
||||
// Eliminate the spanning tree to build a prior
|
||||
Ordering ordering = planarOrdering(N);
|
||||
GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1
|
||||
VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1
|
||||
|
||||
// Create BayesNet-preconditioned system
|
||||
BayesNetPreconditioner system(Ab,Rc1);
|
||||
|
||||
// Create zero config y0 and perturbed config y1
|
||||
VectorConfig y0;
|
||||
Vector z2 = zero(2);
|
||||
BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2);
|
||||
|
||||
VectorConfig y1 = y0;
|
||||
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
|
||||
VectorConfig x1 = system.x(y1);
|
||||
|
||||
// Solve using PCG
|
||||
bool verbose = false;
|
||||
double epsilon = 1e-6; // had to crank this down !!!
|
||||
size_t maxIterations = 100;
|
||||
VectorConfig actual_y = gtsam::conjugateGradients<BayesNetPreconditioner,
|
||||
VectorConfig, Errors>(system, y1, verbose, epsilon, maxIterations);
|
||||
VectorConfig actual_x = system.x(actual_y);
|
||||
CHECK(assert_equal(xtrue,actual_x));
|
||||
|
||||
// Compare with non preconditioned version:
|
||||
VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon,
|
||||
maxIterations);
|
||||
CHECK(assert_equal(xtrue,actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,212 @@
|
|||
/**
|
||||
* @file testSubgraphConditioner.cpp
|
||||
* @brief Unit tests for SubgraphPreconditioner
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "numericalDerivative.h"
|
||||
#include "Ordering.h"
|
||||
#include "smallExample.h"
|
||||
#include "SubgraphPreconditioner.h"
|
||||
#include "iterative-inl.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SubgraphPreconditioner, planarGraph )
|
||||
{
|
||||
// Check planar graph construction
|
||||
GaussianFactorGraph A;
|
||||
VectorConfig xtrue;
|
||||
boost::tie(A, xtrue) = planarGraph(3);
|
||||
LONGS_EQUAL(13,A.size());
|
||||
LONGS_EQUAL(9,xtrue.size());
|
||||
DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue
|
||||
|
||||
// Check canonical ordering
|
||||
Ordering expected, ordering = planarOrdering(3);
|
||||
expected += "x33", "x23", "x13", "x32", "x22", "x12", "x31", "x21", "x11";
|
||||
CHECK(assert_equal(expected,ordering));
|
||||
|
||||
// Check that xtrue is optimal
|
||||
GaussianBayesNet R1 = A.eliminate(ordering);
|
||||
VectorConfig actual = optimize(R1);
|
||||
CHECK(assert_equal(xtrue,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SubgraphPreconditioner, splitOffPlanarTree )
|
||||
{
|
||||
// Build a planar graph
|
||||
GaussianFactorGraph A;
|
||||
VectorConfig xtrue;
|
||||
boost::tie(A, xtrue) = planarGraph(3);
|
||||
|
||||
// Get the spanning tree and constraints, and check their sizes
|
||||
GaussianFactorGraph T, C;
|
||||
boost::tie(T, C) = splitOffPlanarTree(3, A);
|
||||
LONGS_EQUAL(9,T.size());
|
||||
LONGS_EQUAL(4,C.size());
|
||||
|
||||
// Check that the tree can be solved to give the ground xtrue
|
||||
Ordering ordering = planarOrdering(3);
|
||||
GaussianBayesNet R1 = T.eliminate(ordering);
|
||||
VectorConfig xbar = optimize(R1);
|
||||
CHECK(assert_equal(xtrue,xbar));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double error(const VectorConfig& x) {
|
||||
// Build a planar graph
|
||||
GaussianFactorGraph Ab;
|
||||
VectorConfig xtrue;
|
||||
size_t N = 3;
|
||||
boost::tie(Ab, xtrue) = 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);
|
||||
|
||||
// Eliminate the spanning tree to build a prior
|
||||
Ordering ordering = planarOrdering(N);
|
||||
GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1
|
||||
VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1
|
||||
|
||||
SubgraphPreconditioner system(Rc1, Ab2, xbar);
|
||||
return system.error(x);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SubgraphPreconditioner, system )
|
||||
{
|
||||
// Build a planar graph
|
||||
GaussianFactorGraph Ab;
|
||||
VectorConfig xtrue;
|
||||
size_t N = 3;
|
||||
boost::tie(Ab, xtrue) = 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);
|
||||
|
||||
// Eliminate the spanning tree to build a prior
|
||||
Ordering ordering = planarOrdering(N);
|
||||
GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1
|
||||
VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1
|
||||
|
||||
// Create Subgraph-preconditioned system
|
||||
SubgraphPreconditioner system(Rc1, Ab2, xbar);
|
||||
|
||||
// Create zero config
|
||||
VectorConfig zeros;
|
||||
Vector z2 = zero(2);
|
||||
BOOST_FOREACH(const string& j, ordering) zeros.insert(j,z2);
|
||||
|
||||
// Set up y0 as all zeros
|
||||
VectorConfig y0 = zeros;
|
||||
|
||||
// y1 = perturbed y0
|
||||
VectorConfig y1 = zeros;
|
||||
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
|
||||
|
||||
// Check corresponding x values
|
||||
VectorConfig expected_x1 = xtrue, x1 = system.x(y1);
|
||||
expected_x1.getReference("x23") = Vector_(2, 2.01, 2.99);
|
||||
expected_x1.getReference("x33") = Vector_(2, 3.01, 2.99);
|
||||
CHECK(assert_equal(xtrue, system.x(y0)));
|
||||
CHECK(assert_equal(expected_x1,system.x(y1)));
|
||||
|
||||
// Check errors
|
||||
DOUBLES_EQUAL(0,Ab.error(xtrue),1e-9);
|
||||
DOUBLES_EQUAL(3,Ab.error(x1),1e-9);
|
||||
DOUBLES_EQUAL(0,system.error(y0),1e-9);
|
||||
DOUBLES_EQUAL(3,system.error(y1),1e-9);
|
||||
|
||||
// Test gradient in x
|
||||
VectorConfig expected_gx0 = zeros;
|
||||
VectorConfig expected_gx1 = zeros;
|
||||
CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue)));
|
||||
expected_gx1.getReference("x13") = Vector_(2, -100., 100.);
|
||||
expected_gx1.getReference("x22") = Vector_(2, -100., 100.);
|
||||
expected_gx1.getReference("x23") = Vector_(2, 200., -200.);
|
||||
expected_gx1.getReference("x32") = Vector_(2, -100., 100.);
|
||||
expected_gx1.getReference("x33") = Vector_(2, 100., -100.);
|
||||
CHECK(assert_equal(expected_gx1,Ab.gradient(x1)));
|
||||
|
||||
// Test gradient in y
|
||||
VectorConfig expected_gy0 = zeros;
|
||||
VectorConfig expected_gy1 = zeros;
|
||||
expected_gy1.getReference("x13") = Vector_(2, 2., -2.);
|
||||
expected_gy1.getReference("x22") = Vector_(2, -2., 2.);
|
||||
expected_gy1.getReference("x23") = Vector_(2, 3., -3.);
|
||||
expected_gy1.getReference("x32") = Vector_(2, -1., 1.);
|
||||
expected_gy1.getReference("x33") = Vector_(2, 1., -1.);
|
||||
CHECK(assert_equal(expected_gy0,system.gradient(y0)));
|
||||
CHECK(assert_equal(expected_gy1,system.gradient(y1)));
|
||||
|
||||
// Check it numerically for good measure
|
||||
// TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
|
||||
Vector numerical_g1 = numericalGradient<VectorConfig> (error, y1, 0.001);
|
||||
Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
|
||||
3., -3., 0., 0., -1., 1., 1., -1.);
|
||||
CHECK(assert_equal(expected_g1,numerical_g1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SubgraphPreconditioner, conjugateGradients )
|
||||
{
|
||||
// Build a planar graph
|
||||
GaussianFactorGraph Ab;
|
||||
VectorConfig xtrue;
|
||||
size_t N = 3;
|
||||
boost::tie(Ab, xtrue) = 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);
|
||||
|
||||
// Eliminate the spanning tree to build a prior
|
||||
Ordering ordering = planarOrdering(N);
|
||||
GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1
|
||||
VectorConfig xbar = optimize(Rc1); // xbar = inv(R1)*c1
|
||||
|
||||
// Create Subgraph-preconditioned system
|
||||
SubgraphPreconditioner system(Rc1, Ab2, xbar);
|
||||
|
||||
// Create zero config y0 and perturbed config y1
|
||||
VectorConfig y0;
|
||||
Vector z2 = zero(2);
|
||||
BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2);
|
||||
|
||||
VectorConfig y1 = y0;
|
||||
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
|
||||
VectorConfig x1 = system.x(y1);
|
||||
|
||||
// Solve for the remaining constraints using PCG
|
||||
bool verbose = false;
|
||||
double epsilon = 1e-3;
|
||||
size_t maxIterations = 100;
|
||||
VectorConfig actual = gtsam::conjugateGradients<SubgraphPreconditioner,
|
||||
VectorConfig, Errors>(system, y1, verbose, epsilon, maxIterations);
|
||||
CHECK(assert_equal(y0,actual));
|
||||
|
||||
// Compare with non preconditioned version:
|
||||
VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon,
|
||||
maxIterations);
|
||||
CHECK(assert_equal(xtrue,actual2,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue