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

Merged in fix/SubgraphPreconditioner (pull request #407) 

Fix/SubgraphPreconditioner
release/4.3a0
Frank Dellaert 2019-04-08 21:21:36 +00:00
parent 0b4f8da22e 913262b27d
commit 60b2c3b157
9 changed files with 3995 additions and 280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3414
examples/Data/randomGrid3D.xml Normal file
View File

File diff suppressed because it is too large Load Diff

169
examples/Data/toy3D.xml Normal file
View File

@ -0,0 +1,169 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="17">
<graph class_id="0" tracking_level="0" version="0">
<Base class_id="1" tracking_level="0" version="0">
<factors_ class_id="2" tracking_level="0" version="0">
<count>2</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>
<count>2</count>
<item_version>0</item_version>
<item>0</item>
<item>1</item>
</keys_>
</Base>
</Base>
<Ab_ class_id="8" tracking_level="0" version="0">
<matrix_ class_id="9" tracking_level="0" version="0">
<rows>9</rows>
<cols>7</cols>
<data>
<item>1.22427709071730959e+01</item>
<item>1.51514613104920048e+01</item>
<item>3.60934366857813060e+00</item>
<item>-1.18407259026383116e+01</item>
<item>7.84826117220921216e+00</item>
<item>1.23509165494819051e+01</item>
<item>-6.09875015991639735e+00</item>
<item>6.16547190708139126e-01</item>
<item>3.94972084922329048e+00</item>
<item>-4.89208482920378174e+00</item>
<item>3.02091647632478866e+00</item>
<item>-8.95328692238917512e+00</item>
<item>7.89831607220345955e+00</item>
<item>-2.36793602009719084e+00</item>
<item>1.48517612051941725e+01</item>
<item>-3.97284286249233731e-01</item>
<item>-1.95744531643153863e+01</item>
<item>-3.85954855417462017e+00</item>
<item>4.79268277145419042e+00</item>
<item>-9.01707953629520453e+00</item>
<item>1.37848069005841385e+01</item>
<item>1.04829326688375950e+01</item>
<item>-5.00630568442241675e+00</item>
<item>4.70463561852773182e+00</item>
<item>-1.59179134598689274e+01</item>
<item>-2.04767784956723942e+00</item>
<item>9.54135497908261954e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>-0.00000000000000000e+00</item>
<item>-1.76201757312015372e+00</item>
<item>1.98634190821282672e+01</item>
<item>1.52966546661624236e+00</item>
<item>1.94817649567575373e+01</item>
<item>1.39684693294110307e+00</item>
<item>4.30228460420588288e+00</item>
<item>1.76201757312015372e+00</item>
<item>-1.98634190821282672e+01</item>
<item>-1.52966546661624236e+00</item>
<item>-0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>4.16606867942304948e+00</item>
<item>1.86906420801308037e+00</item>
<item>-1.94717865319198360e+01</item>
<item>-1.94817649567575373e+01</item>
<item>-1.39684693294110307e+00</item>
<item>-4.30228460420588288e+00</item>
<item>-4.16606867942304948e+00</item>
<item>-1.86906420801308037e+00</item>
<item>1.94717865319198360e+01</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00891462904330531e+01</item>
<item>-1.08132497987816869e+01</item>
<item>8.66487736568128497e+00</item>
<item>2.88370015604634311e+01</item>
<item>1.89391698948574643e+01</item>
<item>2.12398960190661290e+00</item>
<item>1.22150946112124039e+01</item>
<item>-2.33658532501596561e+01</item>
<item>1.51576204760307363e+01</item>
</data>
</matrix_>
<variableColOffsets_>
<count>4</count>
<item_version>0</item_version>
<item>0</item>
<item>3</item>
<item>6</item>
<item>7</item>
</variableColOffsets_>
<rowStart_>0</rowStart_>
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_ class_id="11" tracking_level="0" version="1">
<px class_id="-1"></px>
</model_>
</px>
</item>
<item>
<px class_id_reference="4" object_id="_1">
<Base>
<Base>
<keys_>
<count>2</count>
<item_version>0</item_version>
<item>0</item>
<item>1</item>
</keys_>
</Base>
</Base>
<Ab_>
<matrix_>
<rows>3</rows>
<cols>7</cols>
<data>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
</data>
</matrix_>
<variableColOffsets_>
<count>4</count>
<item_version>0</item_version>
<item>0</item>
<item>3</item>
<item>6</item>
<item>7</item>
</variableColOffsets_>
<rowStart_>0</rowStart_>
<rowEnd_>3</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
</px>
</item>
</factors_>
</Base>
</graph>
</boost_serialization>

42
gtsam/base/numericalDerivative.h
View File

@ -67,28 +67,29 @@ struct FixedSizeMatrix {
}
/**
* Numerically compute gradient of scalar function
* @brief Numerically compute gradient of scalar function
* @return n-dimensional gradient computed via central differencing
* Class X is the input argument
* The class X needs to have dim, expmap, logmap
* int N is the dimension of the X input value if variable dimension type but known at test time
*/
template<class X>
typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
double delta = 1e-5) {
template <class X, int N = traits<X>::dimension>
typename Eigen::Matrix<double, N, 1> numericalGradient(
boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
static const int N = traits<X>::dimension;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
typedef typename traits<X>::TangentVector TangentX;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX d;
typename traits<X>::TangentVector d;
d.setZero();
Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
Eigen::Matrix<double,N,1> g;
g.setZero();
for (int j = 0; j < N; j++) {
d(j) = delta;
double hxplus = h(traits<X>::Retract(x, d));
@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
* @param delta increment for numerical derivative
* Class Y is the output argument
* Class X is the input argument
* int N is the dimension of the X input value if variable dimension type but known at test time
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X>
template <class Y, class X, int N = traits<X>::dimension>
// TODO Should compute fixed-size matrix
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
typedef traits<Y> TraitsY;
typedef typename TraitsY::TangentVector TangentY;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
static const int N = traits<X>::dimension;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
typedef traits<X> TraitsX;
typedef typename TraitsX::TangentVector TangentX;
// get value at x, and corresponding chart
const Y hx = h(x);
// Bit of a hack for now to find number of rows
const TangentY zeroY = TraitsY::Local(hx, hx);
const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
const size_t m = zeroY.size();
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx;
Eigen::Matrix<double, N, 1> dx;
dx.setZero();
// Fill in Jacobian H
@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
const double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) {
dx(j) = delta;
const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = -delta;
const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
}

28
gtsam/linear/Preconditioner.h
View File

@ -70,21 +70,20 @@ public:
Preconditioner() {}
virtual ~Preconditioner() {}
/* Computation Interfaces */
/*
* Abstract interface for raw vectors. VectorValues is a speed bottleneck
* and Yong-Dian has profiled preconditioners (outside GTSAM) with the the
* three methods below. In GTSAM, unfortunately, we are still using the
* VectorValues methods called in iterative-inl.h
*/
/* implement x = L^{-1} y */
/// implement x = L^{-1} y
virtual void solve(const Vector& y, Vector &x) const = 0;
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
/* implement x = L^{-T} y */
/// implement x = L^{-T} y
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
// /* implement x = L^{-1} L^{-T} y */
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
/* build/factorize the preconditioner */
/// build/factorize the preconditioner
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
@ -113,14 +112,7 @@ public:
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const { x = y; }
// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
@ -145,8 +137,6 @@ public:
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
// virtual void fullSolve(const Vector& y, Vector &x) const ;
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,

254
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -52,17 +52,21 @@
#include <utility>
#include <vector>
using namespace std;
using std::cout;
using std::endl;
using std::vector;
using std::ostream;
namespace gtsam {
/* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
for(const GaussianFactor::shared_ptr &gf: gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
auto result = boost::make_shared<GaussianFactorGraph>();
for (const auto &factor : gfg) {
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
jf = boost::make_shared<JacobianFactor>(*factor);
}
result->push_back(jf);
}
@ -70,20 +74,23 @@ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFa
}
/*****************************************************************************/
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
/* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
if (sum==0.0) {
throw std::runtime_error("Weight vector has no non-zero weights");
}
/* make a normalized and accumulated version of the weight vector */
const size_t m = weight.size();
vector<double> w; w.reserve(m);
vector<double> cdf; cdf.reserve(m);
for ( size_t i = 0 ; i < m ; ++i ) {
w.push_back(weight[i]/sum);
cdf.push_back(weight[i]/sum);
}
vector<double> acc(m);
std::partial_sum(w.begin(),w.end(),acc.begin());
std::partial_sum(cdf.begin(),cdf.end(),acc.begin());
/* iid sample n times */
vector<size_t> result; result.reserve(n);
@ -91,26 +98,26 @@ static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t
for ( size_t i = 0 ; i < n ; ++i ) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
size_t idx = it - acc.begin();
const auto it = std::lower_bound(acc.begin(), acc.end(), value);
const size_t idx = it - acc.begin();
result.push_back(idx);
}
return result;
}
/*****************************************************************************/
vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n) {
const size_t m = weight.size();
if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size");
vector<size_t> result;
size_t count = 0;
std::vector<bool> touched(m, false);
vector<bool> touched(m, false);
while ( count < n ) {
std::vector<size_t> localIndices; localIndices.reserve(n-count);
std::vector<double> localWeights; localWeights.reserve(n-count);
vector<size_t> localIndices; localIndices.reserve(n-count);
vector<double> localWeights; localWeights.reserve(n-count);
/* collect data */
for ( size_t i = 0 ; i < m ; ++i ) {
@ -134,16 +141,16 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
}
/****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) {
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
for ( const size_t &idx: indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0));
edges_.emplace_back(idx, 1.0);
}
}
/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size());
vector<size_t> Subgraph::edgeIndices() const {
vector<size_t> eid; eid.reserve(size());
for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_);
}
@ -169,7 +176,7 @@ Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
ostream &operator<<(ostream &os, const SubgraphEdge &edge) {
if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else
@ -178,7 +185,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
ostream &operator<<(ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ;
@ -212,16 +219,16 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato
if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL;
throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
if ( w == NATURALCHAIN )return "NATURALCHAIN";
else if ( w == BFS ) return "BFS";
else if ( w == KRUSKAL )return "KRUSKAL";
else return "UNKNOWN";
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
if ( s == NATURALCHAIN ) return "NATURALCHAIN";
else if ( s == BFS ) return "BFS";
else if ( s == KRUSKAL ) return "KRUSKAL";
else return "UNKNOWN";
}
/****************************************************************/
@ -231,7 +238,7 @@ SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWei
else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM;
throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
return EQUAL;
}
@ -245,12 +252,14 @@ std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
SubgraphBuilderParameters::AugmentationWeight
SubgraphBuilderParameters::augmentationWeightTranslator(
const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
return SKELETON;
}
@ -263,7 +272,9 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN:
@ -273,21 +284,21 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, w);
return kruskal(gfg, ordering, weights);
break;
default:
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 1 ) {
for (const auto &factor : gfg) {
if ( factor->size() == 1 ) {
result.push_back(idx);
}
idx++;
@ -296,8 +307,8 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) {
@ -311,7 +322,7 @@ std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gf
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0];
@ -319,7 +330,7 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
std::vector<size_t> result;
vector<size_t> result;
result.reserve(n-1);
/* Initialize */
@ -347,30 +358,31 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(w) ;
const vector<size_t> idx = sort_idx(weights) ;
/* initialize buffer */
vector<size_t> result;
result.reserve(n-1);
// container for acsendingly sorted edges
DSFVector D(n) ;
DSFVector dsf(n);
size_t count = 0 ; double sum = 0.0 ;
for (const size_t id: idx) {
const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second,
u_root = D.find(u),
v = ordering.find(gf.keys()[1])->second,
v_root = D.find(v) ;
if ( u_root != v_root ) {
D.merge(u_root, v_root) ;
const auto keys = gf.keys();
if ( keys.size() != 2 ) continue;
const size_t u = ordering.find(keys[0])->second,
v = ordering.find(keys[1])->second;
if ( dsf.find(u) != dsf.find(v) ) {
dsf.merge(u, v) ;
result.push_back(id) ;
sum += w[id] ;
sum += weights[id] ;
if ( ++count == n-1 ) break ;
}
}
@ -378,33 +390,40 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
return uniqueSampler(weights, t);
vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
return UniqueSampler(weights, t);
}
/****************************************************************/
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
const SubgraphBuilderParameters &p = parameters_;
const Ordering inverse_ordering = Ordering::Natural(gfg);
const auto &p = parameters_;
const auto inverse_ordering = Ordering::Natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
const size_t n = inverse_ordering.size(), m = gfg.size();
vector<double> w = weights(gfg);
const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
// Make sure the subgraph preconditioner does not include more than half of
// the edges beyond the spanning tree, or we might as well solve the whole thing.
size_t numExtraEdges = n * p.complexity_;
const size_t numRemaining = m - (n - 1);
numExtraEdges = std::min(numExtraEdges, numRemaining/2);
/* sanity check */
// Calculate weights
vector<double> weights = this->weights(gfg);
// Build spanning tree.
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
if ( tree.size() != n-1 ) {
throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1");
}
/* down weight the tree edges to zero */
// Downweight the tree edges to zero.
for ( const size_t id: tree ) {
w[id] = 0.0;
weights[id] = 0.0;
}
/* decide how many edges to augment */
std::vector<size_t> offTree = sample(w, t);
vector<size_t> offTree = sample(weights, numExtraEdges);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
@ -450,7 +469,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
break;
default:
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
@ -484,21 +503,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const {
/* ************************************************************************* */
// 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 */
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);
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);
}
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(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);
return e;
}
@ -508,8 +526,10 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for (size_t i = 0; i < y.size(); ++i, ++ei)
*ei = y[i];
for(const auto& key_value: y) {
*ei = key_value.second;
++ei;
}
// Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
@ -522,8 +542,10 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for (size_t i = 0; i < y.size(); ++i, ++it)
y[i] = *it;
for(auto& key_value: y) {
key_value.second = *it;
++it;
}
transposeMultiplyAdd2(1.0, it, e.end(), y);
return y;
}
@ -534,9 +556,10 @@ void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for (size_t i = 0; i < y.size(); ++i, ++it) {
for(auto& key_value: y) {
const Vector& ei = *it;
axpy(alpha, ei, y[i]);
axpy(alpha, ei, key_value.second);
++it;
}
transposeMultiplyAdd2(alpha, it, e.end(), y);
}
@ -563,47 +586,52 @@ void SubgraphPreconditioner::print(const std::string& s) const {
}
/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
{
/* copy first */
std::copy(y.data(), y.data() + y.rows(), x.data());
void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
assert(x.size() == y.size());
/* in place back substitute */
for (auto cg: boost::adaptors::reverse(*Rc1_)) {
/* back substitute */
for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
/* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const KeyVector parentKeys(cg->beginParents(), cg->endParents());
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
const Vector xParent = getSubvector(x, keyInfo_, parentKeys);
const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
rhsFrontal - cg->get_S() * xParent);
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
setSubvector(solFrontal, keyInfo_, frontalKeys, x);
}
}
/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
{
void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
/* copy first */
assert(x.size() == y.size());
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
for (const auto &cg : *Rc1_) {
const Vector rhsFrontal = getSubvector(
x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const Vector solFrontal =
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
rhsFrontal);
// Check for indeterminant solution
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
if (solFrontal.hasNaN())
throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
setSubvector(solFrontal, keyInfo_,
KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
/* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
KeyInfo::const_iterator it2 = keyInfo_.find(*it);
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
const KeyInfoEntry &info = keyInfo_.find(*it)->second;
Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
}
}
@ -626,25 +654,24 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
}
/*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
const KeyVector &keys) {
/* a cache of starting index and dim */
typedef vector<pair<size_t, size_t> > Cache;
Cache cache;
vector<std::pair<size_t, size_t> > cache;
/* figure out dimension by traversing the keys */
size_t d = 0;
for ( const Key &key: keys ) {
size_t dim = 0;
for (const Key &key : keys) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair(entry.colstart(), entry.dim()));
d += entry.dim();
cache.emplace_back(entry.colstart(), entry.dim());
dim += entry.dim();
}
/* use the cache to fill the result */
Vector result = Vector::Zero(d, 1);
Vector result = Vector::Zero(dim);
size_t idx = 0;
for ( const Cache::value_type &p: cache ) {
result.segment(idx, p.second) = src.segment(p.first, p.second) ;
for (const auto &p : cache) {
result.segment(idx, p.second) = src.segment(p.first, p.second);
idx += p.second;
}
@ -653,7 +680,6 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &
/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
/* use the cache */
size_t idx = 0;
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
@ -663,10 +689,10 @@ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &ke
}
/*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr buildFactorSubgraph(
const GaussianFactorGraph &gfg, const Subgraph &subgraph,
const bool clone) {
auto result = boost::make_shared<GaussianFactorGraph>();
result->reserve(subgraph.size());
for ( const SubgraphEdge &e: subgraph ) {
const size_t idx = e.index();

35
gtsam/linear/SubgraphPreconditioner.h
View File

@ -129,11 +129,11 @@ namespace gtsam {
enum AugmentationWeight { /* how to weigh the graph edges */
SKELETON = 0, /* use the same weights in building the skeleton */
// STRETCH, /* stretch in the laplacian sense */
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
// STRETCH, /* stretch in the laplacian sense */
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
} augmentationWeight_ ;
double complexity_;
double complexity_; /* factor multiplied with n, yields number of extra edges. */
SubgraphBuilderParameters()
: skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
@ -145,7 +145,7 @@ namespace gtsam {
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
static Skeleton skeletonTranslator(const std::string &s);
static std::string skeletonTranslator(Skeleton w);
static std::string skeletonTranslator(Skeleton s);
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
static std::string skeletonWeightTranslator(SkeletonWeight w);
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
@ -170,7 +170,7 @@ namespace gtsam {
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
Weights weights(const GaussianFactorGraph &gfg) const;
SubgraphBuilderParameters parameters_;
@ -249,8 +249,8 @@ namespace gtsam {
/* A zero VectorValues with the structure of xbar */
VectorValues zero() const {
VectorValues V(VectorValues::Zero(*xbar_));
return V ;
assert(xbar_);
return VectorValues::Zero(*xbar_);
}
/**
@ -285,15 +285,18 @@ namespace gtsam {
/*****************************************************************************/
/* implement virtual functions of Preconditioner */
/* Computation Interfaces for Vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
/// implement x = R^{-1} y
void solve(const Vector& y, Vector &x) const override;
virtual void build(
/// implement x = R^{-T} y
void transposeSolve(const Vector& y, Vector& x) const override;
/// build/factorize the preconditioner
void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) ;
) override;
/*****************************************************************************/
};
@ -310,9 +313,9 @@ namespace gtsam {
/* sort the container and return permutation index with default comparator */
template <typename Container>
std::vector<size_t> sort_idx(const Container &src)
{
template <typename Container>
std::vector<size_t> sort_idx(const Container &src)
{
typedef typename Container::value_type T;
const size_t n = src.size() ;
std::vector<std::pair<size_t,T> > tmp;
@ -329,6 +332,6 @@ namespace gtsam {
idx.push_back(tmp[i].first) ;
}
return idx;
}
}
} // namespace gtsam

34
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -21,6 +21,8 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
@ -28,13 +30,11 @@ using namespace boost::assign;
// STL/C++
#include <iostream>
#include <sstream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
using namespace std;
using namespace gtsam;
static const Key _x_=0, _y_=1;
static const Key _x_ = 11, _y_ = 22, _z_ = 33;
static GaussianBayesNet smallBayesNet =
list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))(
@ -42,9 +42,9 @@ static GaussianBayesNet smallBayesNet =
static GaussianBayesNet noisyBayesNet =
list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))(
noiseModel::Isotropic::Sigma(1, 2.0)))(
GaussianConditional(_y_, Vector1::Constant(5), I_1x1,
noiseModel::Diagonal::Sigmas(Vector1::Constant(3))));
noiseModel::Isotropic::Sigma(1, 3.0)));
/* ************************************************************************* */
TEST( GaussianBayesNet, Matrix )
@ -140,11 +140,33 @@ TEST( GaussianBayesNet, optimize3 )
TEST(GaussianBayesNet, ordering)
{
Ordering expected;
expected += 0, 1;
expected += _x_, _y_;
const auto actual = noisyBayesNet.ordering();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( GaussianBayesNet, MatrixStress )
{
GaussianBayesNet bn;
using GC = GaussianConditional;
bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
const VectorValues expected = bn.optimize();
for (const auto keys :
{KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
const Ordering ordering(keys);
Matrix R;
Vector d;
boost::tie(R, d) = bn.matrix(ordering);
EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
}
}
/* ************************************************************************* */
TEST( GaussianBayesNet, backSubstituteTranspose )
{

1
gtsam/slam/dataset.cpp
View File

@ -66,6 +66,7 @@ string findExampleDataFile(const string& name) {
namesToSearch.push_back(name + ".graph");
namesToSearch.push_back(name + ".txt");
namesToSearch.push_back(name + ".out");
namesToSearch.push_back(name + ".xml");
// Find first name that exists
for(const fs::path& root: rootsToSearch) {

298
tests/testSubgraphPreconditioner.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -15,210 +15,302 @@
* @author Frank Dellaert
**/
#include <CppUnitLite/TestHarness.h>
#if 0
#include <tests/smallExample.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <boost/tuple/tuple.hpp>
#include <CppUnitLite/TestHarness.h>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/serialization/export.hpp>
#include <boost/tuple/tuple.hpp>
using namespace boost::assign;
#include <fstream>
using namespace std;
using namespace gtsam;
using namespace example;
// define keys
// Create key for simulated planar graph
Symbol key(int x, int y) {
return symbol_shorthand::X(1000*x+y);
}
Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); }
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarOrdering ) {
TEST(SubgraphPreconditioner, planarOrdering) {
// Check canonical ordering
Ordering expected, ordering = planarOrdering(3);
expected +=
key(3, 3), key(2, 3), key(1, 3),
key(3, 2), key(2, 2), key(1, 2),
key(3, 1), key(2, 1), key(1, 1);
CHECK(assert_equal(expected,ordering));
EXPECT(assert_equal(expected, ordering));
}
/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
for(const GaussianFactor::shared_ptr& factor: fg)
for (const GaussianFactor::shared_ptr& factor : fg)
total_error += factor->error(x);
return total_error;
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
{
TEST(SubgraphPreconditioner, planarGraph) {
// Check planar graph construction
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size());
LONGS_EQUAL(9,xtrue.size());
DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
LONGS_EQUAL(13, A.size());
LONGS_EQUAL(9, xtrue.size());
DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue
// Check that xtrue is optimal
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
VectorValues actual = optimize(*R1);
CHECK(assert_equal(xtrue,actual));
GaussianBayesNet::shared_ptr R1 = A.eliminateSequential();
VectorValues actual = R1->optimize();
EXPECT(assert_equal(xtrue, actual));
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, splitOffPlanarTree )
{
TEST(SubgraphPreconditioner, splitOffPlanarTree) {
// Build a planar graph
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
// Get the spanning tree and constraints, and check their sizes
GaussianFactorGraph T, C;
GaussianFactorGraph::shared_ptr T, C;
boost::tie(T, C) = splitOffPlanarTree(3, A);
LONGS_EQUAL(9,T.size());
LONGS_EQUAL(4,C.size());
LONGS_EQUAL(9, T->size());
LONGS_EQUAL(4, C->size());
// Check that the tree can be solved to give the ground xtrue
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
VectorValues xbar = optimize(*R1);
CHECK(assert_equal(xtrue,xbar));
GaussianBayesNet::shared_ptr R1 = T->eliminateSequential();
VectorValues xbar = R1->optimize();
EXPECT(assert_equal(xtrue, xbar));
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, system )
{
TEST(SubgraphPreconditioner, system) {
// 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) = 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);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Get the spanning tree and remaining graph
GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
// Eliminate the spanning tree to build a prior
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
const Ordering ord = planarOrdering(N);
auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1
VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
VectorValues::shared_ptr xbarShared(
new VectorValues(xbar)); // TODO: horrible
const SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config
VectorValues zeros = VectorValues::Zero(xbar);
// Get corresponding matrices for tests. Add dummy factors to Ab2 to make
// sure it works with the ordering.
Ordering ordering = Rc1->ordering(); // not ord in general!
Ab2->add(key(1, 1), Z_2x2, Z_2x1);
Ab2->add(key(1, 2), Z_2x2, Z_2x1);
Ab2->add(key(1, 3), Z_2x2, Z_2x1);
Matrix A, A1, A2;
Vector b, b1, b2;
std::tie(A, b) = Ab.jacobian(ordering);
std::tie(A1, b1) = Ab1->jacobian(ordering);
std::tie(A2, b2) = Ab2->jacobian(ordering);
Matrix R1 = Rc1->matrix(ordering).first;
Matrix Abar(13 * 2, 9 * 2);
Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
Abar.bottomRows(8) = A2.topRows(8) * R1.inverse();
// Helper function to vectorize in correct order, which is the order in which
// we eliminated the spanning tree.
auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); };
// Set up y0 as all zeros
VectorValues y0 = zeros;
const VectorValues y0 = system.zero();
// y1 = perturbed y0
VectorValues y1 = zeros;
y1[1] = Vector2(1.0, -1.0);
VectorValues y1 = system.zero();
y1[key(3, 3)] = Vector2(1.0, -1.0);
// Check corresponding x values
VectorValues expected_x1 = xtrue, x1 = system.x(y1);
expected_x1[1] = Vector2(2.01, 2.99);
expected_x1[0] = Vector2(3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0)));
CHECK(assert_equal(expected_x1,system.x(y1)));
// Check backSubstituteTranspose works with R1
VectorValues actual = Rc1->backSubstituteTranspose(y1);
Vector expected = R1.transpose().inverse() * vec(y1);
EXPECT(assert_equal(expected, vec(actual)));
// Check corresponding x values
// for y = 0, we get xbar:
EXPECT(assert_equal(xbar, system.x(y0)));
// for non-zero y, answer is x = xbar + inv(R1)*y
const Vector expected_x1 = vec(xbar) + R1.inverse() * vec(y1);
const VectorValues x1 = system.x(y1);
EXPECT(assert_equal(expected_x1, vec(x1)));
// Check errors
DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
DOUBLES_EQUAL(0,error(system,y0),1e-9);
DOUBLES_EQUAL(3,error(system,y1),1e-9);
DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9);
DOUBLES_EQUAL(0, system.error(y0), 1e-9);
DOUBLES_EQUAL(2, error(Ab, x1), 1e-9);
DOUBLES_EQUAL(2, system.error(y1), 1e-9);
// Test gradient in x
VectorValues expected_gx0 = zeros;
VectorValues expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
expected_gx1[2] = Vector2(-100., 100.);
expected_gx1[4] = Vector2(-100., 100.);
expected_gx1[1] = Vector2(200., -200.);
expected_gx1[3] = Vector2(-100., 100.);
expected_gx1[0] = Vector2(100., -100.);
CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
// Check that transposeMultiplyAdd <=> y += alpha * Abar' * e
// We check for e1 =[1;0] and e2=[0;1] corresponding to T and C
const double alpha = 0.5;
Errors e1, e2;
for (size_t i = 0; i < 13; i++) {
e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0);
e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0);
}
Vector ee1(13 * 2), ee2(13 * 2);
ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2);
ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2);
// Check transposeMultiplyAdd for e1
VectorValues y = system.zero();
system.transposeMultiplyAdd(alpha, e1, y);
Vector expected_y = alpha * Abar.transpose() * ee1;
EXPECT(assert_equal(expected_y, vec(y)));
// Check transposeMultiplyAdd for e2
y = system.zero();
system.transposeMultiplyAdd(alpha, e2, y);
expected_y = alpha * Abar.transpose() * ee2;
EXPECT(assert_equal(expected_y, vec(y)));
// Test gradient in y
VectorValues expected_gy0 = zeros;
VectorValues expected_gy1 = zeros;
expected_gy1[2] = Vector2(2., -2.);
expected_gy1[4] = Vector2(-2., 2.);
expected_gy1[1] = Vector2(3., -3.);
expected_gy1[3] = Vector2(-1., 1.);
expected_gy1[0] = Vector2(1., -1.);
CHECK(assert_equal(expected_gy0,gradient(system,y0)));
CHECK(assert_equal(expected_gy1,gradient(system,y1)));
// Check it numerically for good measure
// TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
// Vector numerical_g1 = numericalGradient<VectorValues> (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));
auto g = system.gradient(y0);
Vector expected_g = Vector::Zero(18);
EXPECT(assert_equal(expected_g, vec(g)));
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, conjugateGradients )
{
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor");
// Read from XML file
static GaussianFactorGraph read(const string& name) {
auto inputFile = findExampleDataFile(name);
ifstream is(inputFile);
if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
boost::archive::xml_iarchive in_archive(is);
GaussianFactorGraph Ab;
in_archive >> boost::serialization::make_nvp("graph", Ab);
return Ab;
}
TEST(SubgraphSolver, Solves) {
// Create preconditioner
SubgraphPreconditioner system;
// We test on three different graphs
const auto Ab1 = planarGraph(3).get<0>();
const auto Ab2 = read("toy3D");
const auto Ab3 = read("randomGrid3D");
// For all graphs, test solve and solveTranspose
for (const auto& Ab : {Ab1, Ab2, Ab3}) {
// Call build, a non-const method needed to make solve work :-(
KeyInfo keyInfo(Ab);
std::map<Key, Vector> lambda;
system.build(Ab, keyInfo, lambda);
// Create a perturbed (non-zero) RHS
const auto xbar = system.Rc1()->optimize(); // merely for use in zero below
auto values_y = VectorValues::Zero(xbar);
auto it = values_y.begin();
it->second.setConstant(100);
++it;
it->second.setConstant(-100);
// Solve the VectorValues way
auto values_x = system.Rc1()->backSubstitute(values_y);
// Solve the matrix way, this really just checks BN::backSubstitute
// This only works with Rc1 ordering, not with keyInfo !
// TODO(frank): why does this not work with an arbitrary ordering?
const auto ord = system.Rc1()->ordering();
const Matrix R1 = system.Rc1()->matrix(ord).first;
auto ord_y = values_y.vector(ord);
auto vector_x = R1.inverse() * ord_y;
EXPECT(assert_equal(vector_x, values_x.vector(ord)));
// Test that 'solve' does implement x = R^{-1} y
// We do this by asserting it gives same answer as backSubstitute
// Only works with keyInfo ordering:
const auto ordering = keyInfo.ordering();
auto vector_y = values_y.vector(ordering);
const size_t N = R1.cols();
Vector solve_x = Vector::Zero(N);
system.solve(vector_y, solve_x);
EXPECT(assert_equal(values_x.vector(ordering), solve_x));
// Test that transposeSolve does implement x = R^{-T} y
// We do this by asserting it gives same answer as backSubstituteTranspose
auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y);
Vector solveT_x = Vector::Zero(N);
system.transposeSolve(vector_y, solveT_x);
EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
}
}
/* ************************************************************************* */
TEST(SubgraphPreconditioner, conjugateGradients) {
// 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) = 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);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Get the spanning tree
GaussianFactorGraph::shared_ptr 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);
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
SubgraphPreconditioner::sharedBayesNet Rc1 =
Ab1->eliminateSequential(); // R1*x-c1
VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
VectorValues::shared_ptr xbarShared(
new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1
VectorValues y0 = VectorValues::Zero(xbar);
VectorValues y1 = y0;
y1[1] = Vector2(1.0, -1.0);
y1[key(2, 2)] = Vector2(1.0, -1.0);
VectorValues x1 = system.x(y1);
// Solve for the remaining constraints using PCG
ConjugateGradientParameters parameters;
VectorValues actual = conjugateGradients<SubgraphPreconditioner,
VectorValues, Errors>(system, y1, parameters);
CHECK(assert_equal(y0,actual));
EXPECT(assert_equal(y0,actual));
// Compare with non preconditioned version:
VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
CHECK(assert_equal(xtrue,actual2,1e-4));
EXPECT(assert_equal(xtrue, actual2, 1e-4));
}
#endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */