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

Merge branch 'master' into new_wrap_local

release/4.3a0
Alex Cunningham 2012-06-27 20:03:13 +00:00
parent ea0c85ef06
commit ddbea256af
23 changed files with 266 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/discrete/DiscreteConditional.h
View File

@ -63,9 +63,11 @@ namespace gtsam {
/// @{
/** GTSAM-style print */
void print(const std::string& s = "Discrete Conditional: ") const {
void print(const std::string& s = "Discrete Conditional: ",
const boost::function<std::string(Index)>& formatter
= &(boost::lexical_cast<std::string, Index>)) const {
std::cout << s << std::endl;
IndexConditional::print(s);
IndexConditional::print(s, formatter);
Potentials::print(s);
}

6
gtsam/discrete/DiscreteFactor.h
View File

@ -82,8 +82,10 @@ namespace gtsam {
/// @{
// print
virtual void print(const std::string& s = "DiscreteFactor\n") const {
IndexFactor::print(s);
virtual void print(const std::string& s = "DiscreteFactor\n",
const boost::function<std::string(Index)>& formatter
= &(boost::lexical_cast<std::string, Index>)) const {
IndexFactor::print(s,formatter);
}
/// @}

13
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -63,6 +63,18 @@ namespace gtsam {
return product;
}
/* ************************************************************************* */
void DiscreteFactorGraph::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
}
}
/* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {
@ -90,6 +102,7 @@ namespace gtsam {
return std::make_pair(cond, sum);
}
/* ************************************************************************* */
} // namespace

11
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -77,15 +77,8 @@ public:
double operator()(const DiscreteFactor::Values & values) const;
/// print
void print(const std::string& s = "DiscreteFactorGraph") const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str());
}
}
void print(const std::string& s = "DiscreteFactorGraph",
const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
};
// DiscreteFactorGraph

23
gtsam/inference/BayesNet-inl.h
View File

@ -26,6 +26,7 @@
#include <boost/assign/std/vector.hpp> // for +=
using boost::assign::operator+=;
#include <algorithm>
#include <iostream>
#include <fstream>
@ -33,10 +34,28 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s) const {
void BayesNet<CONDITIONAL>::print(const std::string& s,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_)
conditional->print();
conditional->print("Conditional", formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::printStats(const std::string& s) const {
const size_t n = conditionals_.size();
size_t max_size = 0;
size_t total = 0;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) {
max_size = std::max(max_size, conditional->size());
total += conditional->size();
}
std::cout << s
<< "maximum conditional size = " << max_size << std::endl
<< "average conditional size = " << total / n << std::endl
<< "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl;
}
/* ************************************************************************* */

8
gtsam/inference/BayesNet.h
View File

@ -23,6 +23,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
@ -48,6 +49,7 @@ public:
typedef typename boost::shared_ptr<BayesNet<CONDITIONAL> > shared_ptr;
/** We store shared pointers to Conditional densities */
typedef typename CONDITIONAL::KeyType KeyType;
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
typedef typename std::list<sharedConditional> Conditionals;
@ -88,7 +90,11 @@ public:
/// @{
/** print */
void print(const std::string& s = "") const;
void print(const std::string& s = "",
const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/** print statistics */
void printStats(const std::string& s = "") const;
/** check equality */
bool equals(const BayesNet& other, double tol = 1e-9) const;

12
gtsam/inference/BayesTree-inl.h
View File

@ -67,6 +67,7 @@ namespace gtsam {
of.close();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
static int num = 0;
@ -102,6 +103,17 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s
<<"avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl
<< "avg Separator Size: " << avgSeparatorSize << std::endl
<< "max Separator Size: " << maxSeparatorSize << std::endl;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {

1
gtsam/inference/BayesTree.h
View File

@ -81,6 +81,7 @@ namespace gtsam {
std::size_t maxConditionalSize;
double avgSeparatorSize;
std::size_t maxSeparatorSize;
void print(const std::string& s = "") const ;
};
/** store all the sizes */

3
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -117,6 +117,9 @@ namespace gtsam {
/** return the const reference of children */
const std::list<derived_ptr>& children() const { return children_; }
/** return a shared_ptr to the parent clique */
derived_ptr parent() const { return parent_.lock(); }
/// @}
/// @name Advanced Interface
/// @{

17
gtsam/inference/ClusterTree-inl.h
View File

@ -71,10 +71,11 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
void ClusterTree<FG>::Cluster::print(const std::string& indent) const {
void ClusterTree<FG>::Cluster::print(const std::string& indent,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << indent;
BOOST_FOREACH(const Index key, frontal)
std::cout << key << " ";
std::cout << formatter(key) << " ";
std::cout << ": ";
BOOST_FOREACH(const Index key, separator)
std::cout << key << " ";
@ -83,19 +84,21 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
void ClusterTree<FG>::Cluster::printTree(const std::string& indent) const {
print(indent);
void ClusterTree<FG>::Cluster::printTree(const std::string& indent,
const boost::function<std::string(KeyType)>& formatter) const {
print(indent, formatter);
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent + " ");
child->printTree(indent + " ", formatter);
}
/* ************************************************************************* *
* ClusterTree
* ************************************************************************* */
template<class FG>
void ClusterTree<FG>::print(const std::string& str) const {
void ClusterTree<FG>::print(const std::string& str,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << str << std::endl;
if (root_) root_->printTree("");
if (root_) root_->printTree("", formatter);
}
/* ************************************************************************* */

10
gtsam/inference/ClusterTree.h
View File

@ -25,6 +25,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h>
@ -38,6 +39,9 @@ namespace gtsam {
*/
template <class FG>
class ClusterTree {
public:
// Access to factor types
typedef typename FG::KeyType KeyType;
protected:
@ -74,10 +78,10 @@ namespace gtsam {
Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/// print
void print(const std::string& indent) const;
void print(const std::string& indent, const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/// print the enire tree
void printTree(const std::string& indent) const;
void printTree(const std::string& indent, const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/// check equality
bool equals(const Cluster& other) const;
@ -123,7 +127,7 @@ namespace gtsam {
/// @{
/// print the object
void print(const std::string& str="") const;
void print(const std::string& str="", const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/** check equality */
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;

9
gtsam/inference/EliminationTree-inl.h
View File

@ -166,12 +166,13 @@ EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
/* ************************************************************************* */
template<class FACTORGRAPH>
void EliminationTree<FACTORGRAPH>::print(const std::string& name) const {
std::cout << name << " (" << key_ << ")" << std::endl;
void EliminationTree<FACTORGRAPH>::print(const std::string& name,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << name << " (" << formatter(key_) << ")" << std::endl;
BOOST_FOREACH(const sharedFactor& factor, factors_) {
factor->print(name + " "); }
factor->print(name + " ", formatter); }
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
child->print(name + " "); }
child->print(name + " ", formatter); }
}
/* ************************************************************************* */

7
gtsam/inference/EliminationTree.h
View File

@ -54,6 +54,8 @@ public:
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
typedef FACTOR Factor;
typedef typename FACTOR::KeyType KeyType;
/** Typedef for an eliminate subroutine */
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
@ -67,7 +69,7 @@ private:
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor?
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
@ -141,7 +143,8 @@ public:
/// @{
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ") const;
void print(const std::string& name = "EliminationTree: ",
const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/** Test whether the tree is equal to another */
bool equals(const EliminationTree& other, double tol = 1e-9) const;

5
gtsam/inference/Factor-inl.h
View File

@ -56,9 +56,10 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::print(const std::string& s) const {
void Factor<KEY>::print(const std::string& s,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << key;
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
}

5
gtsam/inference/Factor.h
View File

@ -25,6 +25,8 @@
#include <vector>
#include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <boost/function/function1.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/FastMap.h>
namespace gtsam {
@ -190,7 +192,8 @@ public:
/// @{
/// print
void print(const std::string& s = "Factor") const;
void print(const std::string& s = "Factor",
const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/// check equality
bool equals(const This& other, double tol = 1e-9) const;

5
gtsam/inference/FactorGraph-inl.h
View File

@ -48,13 +48,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s) const {
void FactorGraph<FACTOR>::print(const std::string& s,
const boost::function<std::string(KeyType)>& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str());
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
}
}

3
gtsam/inference/FactorGraph.h
View File

@ -138,7 +138,8 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph") const;
void print(const std::string& s = "FactorGraph",
const boost::function<std::string(KeyType)>& formatter = &(boost::lexical_cast<std::string, KeyType>)) const;
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;

4
gtsam/linear/GaussianDensity.cpp
View File

@ -23,11 +23,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void GaussianDensity::print(const string &s) const
void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const
{
cout << s << ": density on ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
cout << (boost::format("[%1%]")%(*it)).str() << " ";
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
cout << endl;
gtsam::print(Matrix(get_R()),"R");
gtsam::print(Vector(get_d()),"d");

3
gtsam/linear/GaussianDensity.h
View File

@ -54,7 +54,8 @@ namespace gtsam {
}
/// print
void print(const std::string& = "GaussianDensity") const;
void print(const std::string& = "GaussianDensity",
const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
/// Mean \f$ \mu = R^{-1} d \f$
Vector mean() const;

4
gtsam/linear/VectorValues.cpp
View File

@ -77,10 +77,10 @@ void VectorValues::insert(Index j, const Vector& value) {
}
/* ************************************************************************* */
void VectorValues::print(const std::string& str) const {
void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n";
for (Index var = 0; var < size(); ++var)
std::cout << " " << var << ": \n" << operator[](var) << "\n";
std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n";
std::cout.flush();
}

4
gtsam/linear/VectorValues.h
View File

@ -21,6 +21,7 @@
#include <gtsam/base/types.h>
#include <gtsam/inference/Permutation.h>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <numeric>
@ -167,7 +168,8 @@ namespace gtsam {
const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const;
void print(const std::string& str = "VectorValues: ",
const IndexFormatter& formatter = &(boost::lexical_cast<std::string, Index>)) const;
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const;

8
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -118,13 +118,7 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con
/* ************************************************************************* */
void NonlinearISAM::printStats() const {
gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData();
gtsam::GaussianISAM::CliqueStats stats = data.getStats();
cout << "\navg Conditional Size: " << stats.avgConditionalSize;
cout << "\nmax Conditional Size: " << stats.maxConditionalSize;
cout << "\navg Separator Size: " << stats.avgSeparatorSize;
cout << "\nmax Separator Size: " << stats.maxSeparatorSize;
cout << endl;
isam_.getCliqueData().getStats().print();
}
/* ************************************************************************* */

297
tests/testSubgraphPreconditioner.cpp
View File

@ -40,14 +40,14 @@ Key i3002 = 3002, i2002 = 2002, i1002 = 1002;
Key i3001 = 3001, i2001 = 2001, i1001 = 1001;
// TODO fix Ordering::equals, because the ordering *is* correct !
/* ************************************************************************* *
TEST( SubgraphPreconditioner, planarOrdering )
{
// Check canonical ordering
Ordering expected, ordering = planarOrdering(3);
expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001;
CHECK(assert_equal(expected,ordering));
}
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, planarOrdering )
//{
// // Check canonical ordering
// Ordering expected, ordering = planarOrdering(3);
// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001;
// CHECK(assert_equal(expected,ordering));
//}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
@ -66,148 +66,145 @@ TEST( SubgraphPreconditioner, planarGraph )
CHECK(assert_equal(xtrue,actual));
}
/* ************************************************************************* *
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
JacobianFactorGraph T, C;
// TODO big mess: GFG and JFG mess !!!
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
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
VectorValues xbar = optimize(*R1);
CHECK(assert_equal(xtrue,xbar));
}
/* ************************************************************************* *
TEST( SubgraphPreconditioner, system )
{
// Build a planar graph
JacobianFactorGraph Ab;
VectorValues 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);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// 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
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
// Create zero config
VectorValues zeros = VectorValues::Zero(xbar);
// Set up y0 as all zeros
VectorValues y0 = zeros;
// y1 = perturbed y0
VectorValues y1 = zeros;
y1[i2003] = Vector_(2, 1.0, -1.0);
// Check corresponding x values
VectorValues expected_x1 = xtrue, x1 = system.x(y1);
expected_x1[i2003] = Vector_(2, 2.01, 2.99);
expected_x1[i3003] = 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,error(Ab,xtrue),1e-9); // TODO !
// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO !
DOUBLES_EQUAL(0,error(system,y0),1e-9);
DOUBLES_EQUAL(3,error(system,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[i1003] = Vector_(2, -100., 100.);
expected_gx1[i2002] = Vector_(2, -100., 100.);
expected_gx1[i2003] = Vector_(2, 200., -200.);
expected_gx1[i3002] = Vector_(2, -100., 100.);
expected_gx1[i3003] = Vector_(2, 100., -100.);
CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
// Test gradient in y
VectorValues expected_gy0 = zeros;
VectorValues expected_gy1 = zeros;
expected_gy1[i1003] = Vector_(2, 2., -2.);
expected_gy1[i2002] = Vector_(2, -2., 2.);
expected_gy1[i2003] = Vector_(2, 3., -3.);
expected_gy1[i3002] = Vector_(2, -1., 1.);
expected_gy1[i3003] = Vector_(2, 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));
}
/* ************************************************************************* *
TEST( SubgraphPreconditioner, conjugateGradients )
{
// Build a planar graph
GaussianFactorGraph Ab;
VectorValues 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);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// 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
// Create Subgraph-preconditioned system
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1
VectorValues y0 = VectorValues::Zero(xbar);
VectorValues y1 = y0;
y1[i2003] = Vector_(2, 1.0, -1.0);
VectorValues x1 = system.x(y1);
// Solve for the remaining constraints using PCG
ConjugateGradientParameters parameters;
// VectorValues actual = gtsam::conjugateGradients<SubgraphPreconditioner,
// VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
// CHECK(assert_equal(y0,actual));
// Compare with non preconditioned version:
VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
CHECK(assert_equal(xtrue,actual2,1e-4));
}
/* ************************************************************************* */
//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
// JacobianFactorGraph T, C;
// // TODO big mess: GFG and JFG mess !!!
// 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
// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
// VectorValues xbar = optimize(*R1);
// CHECK(assert_equal(xtrue,xbar));
//}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//TEST( SubgraphPreconditioner, system )
//{
// // Build a planar graph
// JacobianFactorGraph Ab;
// VectorValues 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);
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
//
// // 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
//
// // Create Subgraph-preconditioned system
// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
//
// // Create zero config
// VectorValues zeros = VectorValues::Zero(xbar);
//
// // Set up y0 as all zeros
// VectorValues y0 = zeros;
//
// // y1 = perturbed y0
// VectorValues y1 = zeros;
// y1[i2003] = Vector_(2, 1.0, -1.0);
//
// // Check corresponding x values
// VectorValues expected_x1 = xtrue, x1 = system.x(y1);
// expected_x1[i2003] = Vector_(2, 2.01, 2.99);
// expected_x1[i3003] = 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,error(Ab,xtrue),1e-9); // TODO !
//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO !
// DOUBLES_EQUAL(0,error(system,y0),1e-9);
// DOUBLES_EQUAL(3,error(system,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[i1003] = Vector_(2, -100., 100.);
// expected_gx1[i2002] = Vector_(2, -100., 100.);
// expected_gx1[i2003] = Vector_(2, 200., -200.);
// expected_gx1[i3002] = Vector_(2, -100., 100.);
// expected_gx1[i3003] = Vector_(2, 100., -100.);
// CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
//
// // Test gradient in y
// VectorValues expected_gy0 = zeros;
// VectorValues expected_gy1 = zeros;
// expected_gy1[i1003] = Vector_(2, 2., -2.);
// expected_gy1[i2002] = Vector_(2, -2., 2.);
// expected_gy1[i2003] = Vector_(2, 3., -3.);
// expected_gy1[i3002] = Vector_(2, -1., 1.);
// expected_gy1[i3003] = Vector_(2, 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));
//}
/* ************************************************************************* */
//TEST( SubgraphPreconditioner, conjugateGradients )
//{
// // Build a planar graph
// GaussianFactorGraph Ab;
// VectorValues 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);
// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
//
// // 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
//
// // Create Subgraph-preconditioned system
// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
//
// // Create zero config y0 and perturbed config y1
// VectorValues y0 = VectorValues::Zero(xbar);
//
// VectorValues y1 = y0;
// y1[i2003] = Vector_(2, 1.0, -1.0);
// VectorValues x1 = system.x(y1);
//
// // Solve for the remaining constraints using PCG
// ConjugateGradientParameters parameters;
//// VectorValues actual = gtsam::conjugateGradients<SubgraphPreconditioner,
//// VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
//// CHECK(assert_equal(y0,actual));
//
// // Compare with non preconditioned version:
// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
// CHECK(assert_equal(xtrue,actual2,1e-4));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */