From 2657f04bec750e1bcd5384cd09b85ec96c967fbc Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 10 Sep 2012 17:00:02 +0000 Subject: [PATCH 001/107] Added clone() to BayesTree to allow for full inheritance, wrapped GaussianBayesTree and made GaussianISAM inherit from GaussianBayesTree --- gtsam.h | 49 ++++++++++++++++++++++++++++++++----- gtsam/inference/BayesTree.h | 9 ++++++- 2 files changed, 51 insertions(+), 7 deletions(-) diff --git a/gtsam.h b/gtsam.h index 88efe7618..d19b0bf45 100644 --- a/gtsam.h +++ b/gtsam.h @@ -52,14 +52,16 @@ * Using classes defined in other modules * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error * Virtual inheritance - * - Specify fully-qualified base classes, i.e. "virtual class Derived : module::Base {" - * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : module::Base {" - * - Forward declarations must also be marked virtual, e.g. "virtual class module::Base;" and - * also "virtual class module::Derived;" + * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace + * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" + * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and + * also "virtual class ns::Derived;" * - Pure virtual (abstract) classes should list no constructors in this interface file * - Virtual classes must have a clone() function in C++ (though it does not have to be included * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead * of using the copy constructor (which is used for non-virtual objects). + * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree + * virtual boost::shared_ptr clone() const; * Templates * - Basic templates are supported either with an explicit list of types to instantiate, * e.g. template class Class1 { ... }; @@ -87,7 +89,7 @@ //Module.cpp needs to be changed to allow lowercase classnames class vector { - //Capcaity + //Capacity size_t size() const; size_t max_size() const; void resize(size_t sz, T c = T()); @@ -917,6 +919,38 @@ virtual class BayesTree { typedef gtsam::BayesTree ISAM2BayesTree; +template +virtual class BayesTreeClique { + BayesTreeClique(); + BayesTreeClique(CONDITIONAL* conditional); +// BayesTreeClique(const std::pair& result) : Base(result) {} + + bool equals(const This& other, double tol) const; + void print(string s) const; + void printTree() const; // Default indent of "" + void printTree(string indent) const; + + CONDITIONAL* conditional() const; + bool isRoot() const; + size_t treeSize() const; +// const std::list& children() const { return children_; } +// derived_ptr parent() const { return parent_.lock(); } + + void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); + + // FIXME: need wrapped versions graphs, BayesNet +// BayesNet shortcut(derived_ptr root, Eliminate function) const; +// FactorGraph marginal(derived_ptr root, Eliminate function) const; +// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; + + /** + * This deletes the cached shortcuts of all cliques (subtree) below this clique. + * This is performed when the bayes tree is modified. + */ + void deleteCachedShorcuts(); +}; + //************************************************************************* // linear //************************************************************************* @@ -1237,7 +1271,10 @@ class Errors { //Non-Class functions for Errors //double dot(const gtsam::Errors& A, const gtsam::Errors& b); -class GaussianISAM { +typedef gtsam::BayesTreeClique GaussianBayesTreeClique; +typedef gtsam::BayesTree GaussianBayesTree; + +virtual class GaussianISAM : gtsam::GaussianBayesTree { //Constructor GaussianISAM(); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 1e87ca141..842815bdc 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -248,7 +248,14 @@ namespace gtsam { sharedClique insert(const sharedConditional& clique, std::list& children, bool isRootClique = false); - + /** + * Create a clone of this object as a shared pointer + * Necessary for inheritance in matlab interface + */ + virtual shared_ptr clone() const { + return shared_ptr(new This(*this)); + } + protected: /** private helper method for saving the Tree to a text file in GraphViz format */ From 446d170f1cf849abd173fc4345339f18ae21cac6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Sep 2012 18:15:02 +0000 Subject: [PATCH 002/107] Fixed BayesTree test --- gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 8fdbc5870..e7292f0e5 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -220,16 +220,14 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244) // DiscreteConditional::shared_ptr root = chordal->back(); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); -#ifdef OLD // Let us create the Bayes tree here, just for fun, because we don't use it now typedef JunctionTree JT; GenericMultifrontalSolver solver(graph); - JT::BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); - bayesTree->print("Bayes Tree"); - - tictoc_print(); -} + BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); +// bayesTree->print("Bayes Tree"); + EXPECT_LONGS_EQUAL(2,bayesTree->size()); +#ifdef OLD // Create the elimination tree manually VariableIndex structure(graph); typedef EliminationTree ETree; From db22753767500ef0bf52226574d3e82e232f4a98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Sep 2012 20:07:40 +0000 Subject: [PATCH 003/107] Checking marginals more thoroughly --- .../discrete/tests/testDiscreteMarginals.cpp | 104 +++++++++++++++++- 1 file changed, 103 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 7f2b7b1b1..d0670bcf7 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -18,13 +18,15 @@ */ #include +#include +#include +using namespace boost::assign; #include using namespace std; using namespace gtsam; /* ************************************************************************* */ - TEST_UNSAFE( DiscreteMarginals, UGM_small ) { size_t nrStates = 2; DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), @@ -56,6 +58,7 @@ TEST_UNSAFE( DiscreteMarginals, UGM_small ) { EXPECT(assert_equal(Vector_(2, 0.48628, 0.51372), actualCvector, 1e-6)); } +/* ************************************************************************* */ TEST_UNSAFE( DiscreteMarginals, UGM_chain ) { const int nrNodes = 10; @@ -96,6 +99,105 @@ TEST_UNSAFE( DiscreteMarginals, UGM_chain ) { EXPECT_DOUBLES_EQUAL( 0.03426, (*actualC)(values), 1e-4); } +/* ************************************************************************* */ +TEST_UNSAFE( DiscreteMarginals, truss ) { + + const int nrNodes = 5; + const size_t nrStates = 2; + + // define variables + vector nodes; + for (int i = 0; i < nrNodes; i++) { + DiscreteKey dk(i, nrStates); + nodes.push_back(dk); + } + + // create graph and add three truss potentials + DiscreteFactorGraph graph; + graph.add(nodes[0] & nodes[2] & nodes[4],"2 2 2 2 1 1 1 1"); + graph.add(nodes[1] & nodes[3] & nodes[4],"1 1 1 1 2 2 2 2"); + graph.add(nodes[2] & nodes[3] & nodes[4],"1 1 1 1 1 1 1 1"); + typedef JunctionTree JT; + GenericMultifrontalSolver solver(graph); + BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); +// bayesTree->print("Bayes Tree"); + typedef BayesTreeClique Clique; + + Clique expected0(boost::make_shared((nodes[0] | nodes[2], nodes[4]) = "2/1 2/1 2/1 2/1")); + Clique::shared_ptr actual0 = (*bayesTree)[0]; +// EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails + + Clique expected1(boost::make_shared((nodes[1] | nodes[3], nodes[4]) = "1/2 1/2 1/2 1/2")); + Clique::shared_ptr actual1 = (*bayesTree)[1]; +// EXPECT(assert_equal(expected1, *actual1)); // TODO, correct but fails + + // Create Marginals instance + DiscreteMarginals marginals(graph); + + // test 0 + DecisionTreeFactor expectedM0(nodes[0],"0.666667 0.333333"); + DiscreteFactor::shared_ptr actualM0 = marginals(0); + EXPECT(assert_equal(expectedM0, *boost::dynamic_pointer_cast(actualM0),1e-5)); + + // test 1 + DecisionTreeFactor expectedM1(nodes[1],"0.333333 0.666667"); + DiscreteFactor::shared_ptr actualM1 = marginals(1); + EXPECT(assert_equal(expectedM1, *boost::dynamic_pointer_cast(actualM1),1e-5)); +} + +/* ************************************************************************* */ +// Second truss example with non-trivial factors +TEST_UNSAFE( DiscreteMarginals, truss2 ) { + + const int nrNodes = 5; + const size_t nrStates = 2; + + // define variables + vector nodes; + for (int i = 0; i < nrNodes; i++) { + DiscreteKey dk(i, nrStates); + nodes.push_back(dk); + } + + // create graph and add three truss potentials + DiscreteFactorGraph graph; + graph.add(nodes[0] & nodes[2] & nodes[4],"1 2 3 4 5 6 7 8"); + graph.add(nodes[1] & nodes[3] & nodes[4],"1 2 3 4 5 6 7 8"); + graph.add(nodes[2] & nodes[3] & nodes[4],"1 2 3 4 5 6 7 8"); + + // Calculate the marginals by brute force + vector allPosbValues = cartesianProduct( + nodes[0] & nodes[1] & nodes[2] & nodes[3] & nodes[4]); + Vector T = zero(5), F = zero(5); + for (size_t i = 0; i < allPosbValues.size(); ++i) { + DiscreteFactor::Values x = allPosbValues[i]; + double px = graph(x); + for (size_t j=0;j<5;j++) + if (x[j]) T[j]+=px; else F[j]+=px; + // cout << x[0] << " " << x[1] << " "<< x[2] << " " << x[3] << " " << x[4] << " :\t" << px << endl; + } + + // Check all marginals given by a sequential solver and Marginals + DiscreteSequentialSolver solver(graph); + DiscreteMarginals marginals(graph); + for (size_t j=0;j<5;j++) { + double sum = T[j]+F[j]; + T[j]/=sum; + F[j]/=sum; + + // solver + Vector actualV = solver.marginalProbabilities(nodes[j]); + EXPECT(assert_equal(Vector_(2,F[j],T[j]), actualV)); + + // Marginals + vector table; + table += F[j],T[j]; + DecisionTreeFactor expectedM(nodes[j],table); + DiscreteFactor::shared_ptr actualM = marginals(j); + EXPECT(assert_equal(expectedM, *boost::dynamic_pointer_cast(actualM))); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 684af9824974f67e0b517834a1038f5c1ac5ed85 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 10 Sep 2012 20:07:59 +0000 Subject: [PATCH 004/107] Added function to manually clear shortcut caches in a bayes tree --- gtsam.h | 5 +---- gtsam/inference/BayesTree.h | 3 +++ 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam.h b/gtsam.h index d19b0bf45..a3d8c478b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -914,6 +914,7 @@ virtual class BayesTree { size_t size(); CLIQUE* root() const; void clear(); + void deleteCachedShorcuts(); void insert(const CLIQUE* subtree); }; @@ -944,10 +945,6 @@ virtual class BayesTreeClique { // FactorGraph marginal(derived_ptr root, Eliminate function) const; // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; - /** - * This deletes the cached shortcuts of all cliques (subtree) below this clique. - * This is performed when the bayes tree is modified. - */ void deleteCachedShorcuts(); }; diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 842815bdc..6406fe8f6 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -212,6 +212,9 @@ namespace gtsam { /** Remove all nodes */ void clear(); + /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ + inline void deleteCachedShorcuts() { root_->deleteCachedShorcuts(); } + /** * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. From 0554532fcdf77d3034d8fc3b486c91aab06292c1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 11 Sep 2012 15:58:41 +0000 Subject: [PATCH 005/107] Disabled timing scripts for now (they are outdated and do not compile) --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index afca96736..6b7ce4599 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,7 +38,7 @@ endif() # Configurable Options option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) -option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) +#option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) # These do not currently work option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) @@ -187,7 +187,7 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") message(STATUS "Build flags ") -print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") +#print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") if (DOXYGEN_FOUND) From e9e40c733de3a14e60c93bca4dcbade2701d352d Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 19:58:20 +0000 Subject: [PATCH 006/107] Made BayesNet argument a reference (and moved printStats to the right place) --- gtsam/inference/BayesNet-inl.h | 4 ++-- gtsam/inference/BayesNet.h | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index c306cb39d..912e49ef4 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -112,14 +112,14 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNet::push_back(const BayesNet bn) { + void BayesNet::push_back(const BayesNet& bn) { BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) push_back(conditional); } /* ************************************************************************* */ template - void BayesNet::push_front(const BayesNet bn) { + void BayesNet::push_front(const BayesNet& bn) { BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) push_front(conditional); } diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 67d59c48d..4fa10eee2 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -93,9 +93,6 @@ public: void print(const std::string& s = "", const IndexFormatter& formatter = DefaultIndexFormatter) const; - /** print statistics */ - void printStats(const std::string& s = "") const; - /** check equality */ bool equals(const BayesNet& other, double tol = 1e-9) const; @@ -108,6 +105,9 @@ public: return conditionals_.size(); } + /** print statistics */ + void printStats(const std::string& s = "") const; + /** return keys in reverse topological sort order, i.e., elimination order */ FastList ordering() const; @@ -190,10 +190,10 @@ public: } /// push_back an entire Bayes net - void push_back(const BayesNet bn); + void push_back(const BayesNet& bn); /// push_front an entire Bayes net - void push_front(const BayesNet bn); + void push_front(const BayesNet& bn); /** += syntax for push_back, e.g. bayesNet += c1, c2, c3 * @param conditional The conditional to add to the back of the BayesNet From b8ccc78a162d9229299b60e6c583ac9e171851e4 Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 19:59:10 +0000 Subject: [PATCH 007/107] Wrapped BayesNet templates and made both SymbolicBayesNet and GaussianBayesNet derived classes. --- gtsam.h | 140 ++++++++++++++++++++++++++++++-------------------------- 1 file changed, 75 insertions(+), 65 deletions(-) diff --git a/gtsam.h b/gtsam.h index a3d8c478b..d4b3c0767 100644 --- a/gtsam.h +++ b/gtsam.h @@ -769,29 +769,90 @@ class IndexConditional { void permuteWithInverse(const gtsam::Permutation& inversePermutation); }; +#include +template +virtual class BayesNet { + // Testable + void print(string s) const; + bool equals(const This& other, double tol) const; + + // Standard interface + size_t size() const; + void printStats(string s) const; + CONDITIONAL* front() const; + CONDITIONAL* back() const; + void push_back(CONDITIONAL* conditional); + void push_front(CONDITIONAL* conditional); + void push_back(This& conditional); + void push_front(This& conditional); + void pop_front(); + void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); +}; + +#include +template +virtual class BayesTree { + + //Constructors + BayesTree(); + + // Testable + void print(string s); + bool equals(const This& other, double tol) const; + + //Standard Interface + //size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + CLIQUE* root() const; + void clear(); + void deleteCachedShorcuts(); + void insert(const CLIQUE* subtree); +}; + +template +virtual class BayesTreeClique { + BayesTreeClique(); + BayesTreeClique(CONDITIONAL* conditional); +// BayesTreeClique(const std::pair& result) : Base(result) {} + + bool equals(const This& other, double tol) const; + void print(string s) const; + void printTree() const; // Default indent of "" + void printTree(string indent) const; + + CONDITIONAL* conditional() const; + bool isRoot() const; + size_t treeSize() const; +// const std::list& children() const { return children_; } +// derived_ptr parent() const { return parent_.lock(); } + + void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); + + // FIXME: need wrapped versions graphs, BayesNet +// BayesNet shortcut(derived_ptr root, Eliminate function) const; +// FactorGraph marginal(derived_ptr root, Eliminate function) const; +// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; + + void deleteCachedShorcuts(); +}; + #include -class SymbolicBayesNet { +typedef gtsam::BayesNet SymbolicBayesNetBase; +virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { // Standard Constructors and Named Constructors SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& bn); SymbolicBayesNet(const gtsam::IndexConditional* conditional); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - // Standard interface - size_t size() const; - void push_back(const gtsam::IndexConditional* conditional); //TODO:Throws parse error //void push_back(const gtsam::SymbolicBayesNet bn); - void push_front(const gtsam::IndexConditional* conditional); //TODO:Throws parse error //void push_front(const gtsam::SymbolicBayesNet bn); //Advanced Interface - gtsam::IndexConditional* front() const; - gtsam::IndexConditional* back() const; void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); @@ -900,54 +961,6 @@ class VariableIndex { void permuteInPlace(const gtsam::Permutation& permutation); }; -#include -template -virtual class BayesTree { - - //Constructors - BayesTree(); - - //Standard Interface - bool equals(const This& other, double tol) const; - void print(string s); - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - CLIQUE* root() const; - void clear(); - void deleteCachedShorcuts(); - void insert(const CLIQUE* subtree); -}; - -typedef gtsam::BayesTree ISAM2BayesTree; - -template -virtual class BayesTreeClique { - BayesTreeClique(); - BayesTreeClique(CONDITIONAL* conditional); -// BayesTreeClique(const std::pair& result) : Base(result) {} - - bool equals(const This& other, double tol) const; - void print(string s) const; - void printTree() const; // Default indent of "" - void printTree(string indent) const; - - CONDITIONAL* conditional() const; - bool isRoot() const; - size_t treeSize() const; -// const std::list& children() const { return children_; } -// derived_ptr parent() const { return parent_.lock(); } - - void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); - - // FIXME: need wrapped versions graphs, BayesNet -// BayesNet shortcut(derived_ptr root, Eliminate function) const; -// FactorGraph marginal(derived_ptr root, Eliminate function) const; -// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; - - void deleteCachedShorcuts(); -}; - //************************************************************************* // linear //************************************************************************* @@ -1082,15 +1095,11 @@ class GaussianDensity { Matrix covariance() const; }; -class GaussianBayesNet { +typedef gtsam::BayesNet GaussianBayesNetBase; +virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase { //Constructors GaussianBayesNet(); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet& cbn, double tol) const; - void push_back(gtsam::GaussianConditional* conditional); - void push_front(gtsam::GaussianConditional* conditional); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); }; //Non-Class methods found in GaussianBayesNet.h @@ -1750,6 +1759,7 @@ class ISAM2Result { size_t getCliques() const; }; +typedef gtsam::BayesTree ISAM2BayesTree; virtual class ISAM2 : gtsam::ISAM2BayesTree { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); From 5cdcdaa448efbd01390a190eb535cc3f72d0778d Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 20:52:22 +0000 Subject: [PATCH 008/107] Made SymbolicBayesTree and GaussianBayesTree subclasses of templated BayesTree --- gtsam.h | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam.h b/gtsam.h index d4b3c0767..250d64f0b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -802,8 +802,9 @@ virtual class BayesTree { bool equals(const This& other, double tol) const; //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; + //size_t findParentClique(const gtsam::IndexVector& parents) const; size_t size(); + void saveGraph(string s) const; CLIQUE* root() const; void clear(); void deleteCachedShorcuts(); @@ -859,7 +860,9 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { }; #include -class SymbolicBayesTree { +typedef gtsam::BayesTreeClique SymbolicBayesTreeClique; +typedef gtsam::BayesTree SymbolicBayesTreeBase; +virtual class SymbolicBayesTree : gtsam::SymbolicBayesTreeBase { // Standard Constructors and Named Constructors SymbolicBayesTree(); SymbolicBayesTree(const gtsam::SymbolicBayesNet& bn); @@ -867,15 +870,8 @@ class SymbolicBayesTree { // FIXME: wrap needs to understand std::list //SymbolicBayesTree(const gtsam::SymbolicBayesNet& bayesNet, std::list subtrees); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - // Standard interface size_t findParentClique(const gtsam::IndexConditional& parents) const; - size_t size() const; - void saveGraph(string s) const; - void clear(); // TODO: There are many other BayesTree member functions which might be of use }; @@ -1120,6 +1116,16 @@ double determinant(const gtsam::GaussianBayesNet& bayesNet); gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0); void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);*/ +#include +typedef gtsam::BayesTreeClique GaussianBayesTreeClique; +typedef gtsam::BayesTree GaussianBayesTreeBase; +virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesNet& bn); + GaussianBayesTree(const gtsam::GaussianBayesNet& other); +}; + virtual class GaussianFactor { void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; @@ -1277,9 +1283,6 @@ class Errors { //Non-Class functions for Errors //double dot(const gtsam::Errors& A, const gtsam::Errors& b); -typedef gtsam::BayesTreeClique GaussianBayesTreeClique; -typedef gtsam::BayesTree GaussianBayesTree; - virtual class GaussianISAM : gtsam::GaussianBayesTree { //Constructor GaussianISAM(); From 090133f9443f9f3d6d14beeabc3008344b9a0cd9 Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 20:52:58 +0000 Subject: [PATCH 009/107] Fixed Contents to include new classes and used better ordering. --- matlab/+gtsam/Contents.m | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 0af19e1c8..7d469d704 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -6,26 +6,29 @@ % Permutation - class Permutation, see Doxygen page for details % %% General Inference Classes -% IndexConditional - class IndexConditional, see Doxygen page for details % IndexFactor - class IndexFactor, see Doxygen page for details -% SymbolicBayesNet - class SymbolicBayesNet, see Doxygen page for details -% SymbolicBayesTree - class SymbolicBayesTree, see Doxygen page for details % SymbolicFactorGraph - class SymbolicFactorGraph, see Doxygen page for details +% IndexConditional - class IndexConditional, see Doxygen page for details +% SymbolicBayesNet - class SymbolicBayesNet, see Doxygen page for details +% SymbolicBayesTreeClique - class SymbolicBayesTreeClique, see Doxygen page for details +% SymbolicBayesTree - class SymbolicBayesTree, see Doxygen page for details % SymbolicMultifrontalSolver - class SymbolicMultifrontalSolver, see Doxygen page for details % SymbolicSequentialSolver - class SymbolicSequentialSolver, see Doxygen page for details % VariableIndex - class VariableIndex, see Doxygen page for details % -%% Linear-Gaussian Factor Graphs +%% Linear-Gaussian Graphical Models % Errors - class Errors, see Doxygen page for details -% GaussianBayesNet - class GaussianBayesNet, see Doxygen page for details -% GaussianConditional - class GaussianConditional, see Doxygen page for details -% GaussianDensity - class GaussianDensity, see Doxygen page for details +% VectorValues - class VectorValues, see Doxygen page for details % GaussianFactor - class GaussianFactor, see Doxygen page for details -% GaussianFactorGraph - class GaussianFactorGraph, see Doxygen page for details -% GaussianISAM - class GaussianISAM, see Doxygen page for details % HessianFactor - class HessianFactor, see Doxygen page for details % JacobianFactor - class JacobianFactor, see Doxygen page for details -% VectorValues - class VectorValues, see Doxygen page for details +% GaussianFactorGraph - class GaussianFactorGraph, see Doxygen page for details +% GaussianDensity - class GaussianDensity, see Doxygen page for details +% GaussianConditional - class GaussianConditional, see Doxygen page for details +% GaussianBayesNet - class GaussianBayesNet, see Doxygen page for details +% GaussianBayesTreeClique - class GaussianBayesTreeClique, see Doxygen page for details +% GaussianBayesTree - class GaussianBayesTree, see Doxygen page for details +% GaussianISAM - class GaussianISAM, see Doxygen page for details % %% Linear Inference % GaussianSequentialSolver - class GaussianSequentialSolver, see Doxygen page for details @@ -37,11 +40,14 @@ % Sampler - class Sampler, see Doxygen page for details % %% Nonlinear Factor Graphs -% NonlinearFactor - class NonlinearFactor, see Doxygen page for details -% NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details % Ordering - class Ordering, see Doxygen page for details % Value - class Value, see Doxygen page for details % Values - class Values, see Doxygen page for details +% LieScalar - class LieScalar, see Doxygen page for details +% LieVector - class LieVector, see Doxygen page for details +% LieMatrix - class LieMatrix, see Doxygen page for details +% NonlinearFactor - class NonlinearFactor, see Doxygen page for details +% NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details % %% Nonlinear Optimization % ConjugateGradientParameters - class ConjugateGradientParameters, see Doxygen page for details @@ -49,6 +55,8 @@ % DoglegParams - class DoglegParams, see Doxygen page for details % GaussNewtonOptimizer - class GaussNewtonOptimizer, see Doxygen page for details % GaussNewtonParams - class GaussNewtonParams, see Doxygen page for details +% ISAM2Clique - class ISAM2Clique, see Doxygen page for details +% ISAM2BayesTree - class ISAM2BayesTree, see Doxygen page for details % ISAM2 - class ISAM2, see Doxygen page for details % ISAM2DoglegParams - class ISAM2DoglegParams, see Doxygen page for details % ISAM2GaussNewtonParams - class ISAM2GaussNewtonParams, see Doxygen page for details From ea2c13bca3c0c1a713c9b03892a5953efec44816 Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 22:13:33 +0000 Subject: [PATCH 010/107] Added method saveGraph for BayesNet. --- gtsam.h | 1 + gtsam/inference/BayesNet-inl.h | 31 ++++++++++++++++--- gtsam/inference/BayesNet.h | 4 +++ .../inference/tests/testSymbolicBayesNet.cpp | 14 +++++++++ 4 files changed, 45 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 250d64f0b..6777106b8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -779,6 +779,7 @@ virtual class BayesNet { // Standard interface size_t size() const; void printStats(string s) const; + void saveGraph(string s) const; CONDITIONAL* front() const; CONDITIONAL* back() const; void push_back(CONDITIONAL* conditional); diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 912e49ef4..e113b7628 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -41,6 +41,15 @@ namespace gtsam { conditional->print("Conditional", formatter); } + /* ************************************************************************* */ + template + bool BayesNet::equals(const BayesNet& cbn, double tol) const { + if (size() != cbn.size()) + return false; + return std::equal(conditionals_.begin(), conditionals_.end(), + cbn.conditionals_.begin(), equals_star(tol)); + } + /* ************************************************************************* */ template void BayesNet::printStats(const std::string& s) const { @@ -60,11 +69,23 @@ namespace gtsam { /* ************************************************************************* */ template - bool BayesNet::equals(const BayesNet& cbn, double tol) const { - if (size() != cbn.size()) - return false; - return std::equal(conditionals_.begin(), conditionals_.end(), - cbn.conditionals_.begin(), equals_star(tol)); + void BayesNet::saveGraph(const std::string &s, + const IndexFormatter& indexFormatter) const { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Index me = frontals.front(); +// of << me << std::endl; + typename CONDITIONAL::Parents parents = conditional->parents(); + BOOST_FOREACH(Index p, parents) + of << p << "->" << me << std::endl; + } + + + of << "}"; + of.close(); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 4fa10eee2..0bb5efedb 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -108,6 +108,10 @@ public: /** print statistics */ void printStats(const std::string& s = "") const; + /** save dot graph */ + void saveGraph(const std::string &s, const IndexFormatter& indexFormatter = + DefaultIndexFormatter) const; + /** return keys in reverse topological sort order, i.e., elimination order */ FastList ordering() const; diff --git a/gtsam/inference/tests/testSymbolicBayesNet.cpp b/gtsam/inference/tests/testSymbolicBayesNet.cpp index 06b1bded9..445bab697 100644 --- a/gtsam/inference/tests/testSymbolicBayesNet.cpp +++ b/gtsam/inference/tests/testSymbolicBayesNet.cpp @@ -188,6 +188,20 @@ TEST_UNSAFE(SymbolicBayesNet, popLeaf) { //#endif } +/* ************************************************************************* */ +TEST(SymbolicBayesNet, saveGraph) { + SymbolicBayesNet bn; + bn += IndexConditional::shared_ptr(new IndexConditional(_A_, _B_)); + std::vector keys; + keys.push_back(_B_); + keys.push_back(_C_); + keys.push_back(_D_); + bn += IndexConditional::shared_ptr(new IndexConditional(keys,2)); + bn += IndexConditional::shared_ptr(new IndexConditional(_D_)); + + bn.saveGraph("SymbolicBayesNet.dot"); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0357559827f9cf72243d5d1cd92934993387ba6d Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 22:14:37 +0000 Subject: [PATCH 011/107] Files and test files for the thinTree. To be debugged. --- matlab/gtsam_tests/testThinBayesTree.m | 23 +++++++ matlab/gtsam_tests/testThinTree.m | 49 ++++++++++++++ matlab/gtsam_tests/testThinTreeBayesNet.m | 23 +++++++ matlab/gtsam_tests/thinBayesTree.m | 5 ++ matlab/gtsam_tests/thinTree.m | 78 +++++++++++++++++++++++ matlab/gtsam_tests/thinTreeBayesNet.m | 33 ++++++++++ 6 files changed, 211 insertions(+) create mode 100644 matlab/gtsam_tests/testThinBayesTree.m create mode 100644 matlab/gtsam_tests/testThinTree.m create mode 100644 matlab/gtsam_tests/testThinTreeBayesNet.m create mode 100644 matlab/gtsam_tests/thinBayesTree.m create mode 100644 matlab/gtsam_tests/thinTree.m create mode 100644 matlab/gtsam_tests/thinTreeBayesNet.m diff --git a/matlab/gtsam_tests/testThinBayesTree.m b/matlab/gtsam_tests/testThinBayesTree.m new file mode 100644 index 000000000..accb40245 --- /dev/null +++ b/matlab/gtsam_tests/testThinBayesTree.m @@ -0,0 +1,23 @@ +% /* ---------------------------------------------------------------------------- +% +% * 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) +% +% * See LICENSE for the license information +% +% * -------------------------------------------------------------------------- */ +% +% /** +% * @file testThinBayesTree.cpp +% * @brief Test of binary tree +% * @date Sep 14, 2012 +% * @author Frank Dellaert +% * @author Jean-Guillaume Durand +% */ + +%% Run the tests +import gtsam.* +bayesTree = thinBayesTree(3,2); +EQUALITY('7 = bayesTree.size', 7, bayesTree.size); diff --git a/matlab/gtsam_tests/testThinTree.m b/matlab/gtsam_tests/testThinTree.m new file mode 100644 index 000000000..987af7333 --- /dev/null +++ b/matlab/gtsam_tests/testThinTree.m @@ -0,0 +1,49 @@ +% /* ---------------------------------------------------------------------------- +% +% * 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) +% +% * See LICENSE for the license information +% +% * -------------------------------------------------------------------------- */ +% +% /** +% * @file testThinTree.cpp +% * @brief Test of binary tree +% * @date Sep 13, 2012 +% * @author Frank Dellaert +% * @author Jean-Guillaume Durand +% */ + +%% Clear working space +clc, close all, clear all; + +%% Create different trees for our example +import gtsam.* +t0 = thinTree(2,1); +t1 = thinTree(3,2); +% Add contents in it +% TODO +%% Create the set of expected output TestValues +expectedNumberOfNodes0 = 3; +expectedNumberOfNodes1 = 7; +expectedParentsOf6in1 = [3 1]; +expectedParentsOf7in1 = [3 1]; + +%% Run the tests +% Tree depth +%TODO +% Number of parents for each node +%TODO +% Number of elements +EQUALITY('expectedNumberOfNodes0,t0.getNumberOfElements', expectedNumberOfNodes0,t0.getNumberOfElements); +EQUALITY('expectedNumberOfNodes1,t1.getNumberOfElements', expectedNumberOfNodes1,t1.getNumberOfElements); +% Parents linking +EQUALITY('expectedParentsOf6in1,t1.getParents(6)', expectedParentsOf6in1,t1.getParents(6)); +EQUALITY('expectedParentsOf7in1,t1.getParents(7)', expectedParentsOf7in1,t1.getParents(7)); +% Adding an element + +bn = thinTreeBayesNet(3,2); +EQUALITY('7 = bn.size', 7, bn.size); \ No newline at end of file diff --git a/matlab/gtsam_tests/testThinTreeBayesNet.m b/matlab/gtsam_tests/testThinTreeBayesNet.m new file mode 100644 index 000000000..dceb54e58 --- /dev/null +++ b/matlab/gtsam_tests/testThinTreeBayesNet.m @@ -0,0 +1,23 @@ +% /* ---------------------------------------------------------------------------- +% +% * 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) +% +% * See LICENSE for the license information +% +% * -------------------------------------------------------------------------- */ +% +% /** +% * @file testThinTree.cpp +% * @brief Test of binary tree +% * @date Sep 13, 2012 +% * @author Frank Dellaert +% * @author Jean-Guillaume Durand +% */ + +%% Run the tests +import gtsam.* +bayesNet = thinTreeBayesNet(3,2); +EQUALITY('7 = bayesNet.size', 7, bayesNet.size); diff --git a/matlab/gtsam_tests/thinBayesTree.m b/matlab/gtsam_tests/thinBayesTree.m new file mode 100644 index 000000000..6c2f496c6 --- /dev/null +++ b/matlab/gtsam_tests/thinBayesTree.m @@ -0,0 +1,5 @@ +function bayesTree = thinBayesTree(depth, width) + import gtsam.* + bayesNet = thinTreeBayesNet(depth, width); + bayesTree = GaussianBayesTree(bayesNet); +end \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTree.m b/matlab/gtsam_tests/thinTree.m new file mode 100644 index 000000000..32d2efff5 --- /dev/null +++ b/matlab/gtsam_tests/thinTree.m @@ -0,0 +1,78 @@ +classdef thinTree + + % Attributes + properties (SetAccess = private) + nodes = { [] }; % Array of the nodes + depth = 0; % Depth of the tree + w = 0 % Number of parents for each node + links = []; % The matrix representing the links between the nodes + end + + % Methods + methods + % Constructor + function [obj root_ID] = thinTree(d, w) + + % If 0 input arguments, assume d = 1 and w = 1 + if nargin < 1 + [obj root_ID] = thinTree(1, 1); + return + end + % If 1 input argument, assume w = 1 + if nargin < 1 + [obj root_ID] = thinTree(d, 1); + return + end + % Else + + if w > d-1 + error('MATLAB:thinTree:thinTree', ... + 'Cannot have %d parents on a binary tree of depth %d. You must have nParents < %d here.\n', w, d, d); + end + + root_ID = 1; + obj.nodes = cell(2^d - 1,1); % Creation of the d^2 empty cells + obj.depth = d; + obj.w = w; + obj.links = eye(2^d - 1); % Creation of the links matrix + + % Link the cells + + end + + % Function to add a content for a specific node + function [obj] = addContent(obj, content, nodeID) + obj.nodes{nodeID} = content; + return + end + + % Function to return the ID's of a node's parents + function ids = getParents(obj, nodeID) + % Initialisation + ids = zeros(1,obj.w); + node = nodeID; + % Loop on w, the number of parents associated to one node + for i=1:obj.w + ids(i) = floor(node/2); + node = floor(node/2); + end + % Return + return + end + + % Accessors + function output = getDepth(obj) + output = obj.depth; + return + end + + function output = getW(obj) + output = obj.w; + return + end + + function output = getNumberOfElements(obj) + output = 2^obj.depth - 1; + end + end % Methods +end % Class \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m new file mode 100644 index 000000000..c7dfed6c0 --- /dev/null +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -0,0 +1,33 @@ +function [bayesNet, tree] = thinTreeBayesNet(d,w) +import gtsam.* +bayesNet = GaussianBayesNet; +tree = thinTree(3,2); + +% Filling the tree + +% Creation of the root +gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); +% Getting it into the GaussianBayesNet +bayesNet.push_front(gc); + +for i=1:2^tree.getNumberOfElements() + % Getting the parents of that node + parents = tree.getParents(i); + % Create and link the corresponding GaussianConditionals + if tree.getW == 1 + % Creation of the GaussianConditional + gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1)); + % Getting it into the GaussianBayesNet + bayesNet.push_front(gc); + % Getting it in the thinTree + t = tree.addContent({gc,parents}, i); + elseif tree.getW == 2 + % Creation of the GaussianConditional + gc = gtsam.GaussianConditional(parents(2), 5*rand(1), 5*rand(1), parents(1), 5*rand(1), 5*rand(1)); + % Getting it into the GaussianBayesNet + bayesNet.push_front(gc); + % Getting it in the thinTree + t = tree.addContent({gc,parents}, i); + end +end +end \ No newline at end of file From 8d85d679cd6c92fac43dfeb719fb272c5bf32d97 Mon Sep 17 00:00:00 2001 From: jdurand7 Date: Fri, 14 Sep 2012 23:46:21 +0000 Subject: [PATCH 012/107] Debugged the thinTree. Now works with tree indexing starting at 1 at the root. TODO : make it work with index 0 on a leaf. --- matlab/gtsam_tests/testThinBayesTree.m | 2 +- matlab/gtsam_tests/testThinTreeBayesNet.m | 6 +++++- matlab/gtsam_tests/thinTree.m | 20 ++++++++++++++----- matlab/gtsam_tests/thinTreeBayesNet.m | 24 ++++++++++++++--------- 4 files changed, 36 insertions(+), 16 deletions(-) diff --git a/matlab/gtsam_tests/testThinBayesTree.m b/matlab/gtsam_tests/testThinBayesTree.m index accb40245..034fc38de 100644 --- a/matlab/gtsam_tests/testThinBayesTree.m +++ b/matlab/gtsam_tests/testThinBayesTree.m @@ -20,4 +20,4 @@ %% Run the tests import gtsam.* bayesTree = thinBayesTree(3,2); -EQUALITY('7 = bayesTree.size', 7, bayesTree.size); +EQUALITY('7 = bayesTree.size', 7, bayesTree.size); \ No newline at end of file diff --git a/matlab/gtsam_tests/testThinTreeBayesNet.m b/matlab/gtsam_tests/testThinTreeBayesNet.m index dceb54e58..55499e7ce 100644 --- a/matlab/gtsam_tests/testThinTreeBayesNet.m +++ b/matlab/gtsam_tests/testThinTreeBayesNet.m @@ -17,7 +17,11 @@ % * @author Jean-Guillaume Durand % */ +%% Clean the workspace +clc, clear all, close all; + %% Run the tests import gtsam.* -bayesNet = thinTreeBayesNet(3,2); +[bayesNet tree] = thinTreeBayesNet(4,2); EQUALITY('7 = bayesNet.size', 7, bayesNet.size); +bayesNet.saveGraph('thinTreeBayesNet.dot'); \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTree.m b/matlab/gtsam_tests/thinTree.m index 32d2efff5..72e411419 100644 --- a/matlab/gtsam_tests/thinTree.m +++ b/matlab/gtsam_tests/thinTree.m @@ -49,12 +49,17 @@ classdef thinTree % Function to return the ID's of a node's parents function ids = getParents(obj, nodeID) % Initialisation - ids = zeros(1,obj.w); node = nodeID; - % Loop on w, the number of parents associated to one node - for i=1:obj.w - ids(i) = floor(node/2); - node = floor(node/2); + depthOfNode = obj.getNodeDepth(nodeID); + if depthOfNode == 1 + ids = 1; + else + ids = zeros(1,min(obj.w, depthOfNode-1)); + % Loop on w, the number of parents associated to one node + for i=1:min(obj.w, depthOfNode-1) + ids(i) = floor(node/2); + node = floor(node/2); + end end % Return return @@ -74,5 +79,10 @@ classdef thinTree function output = getNumberOfElements(obj) output = 2^obj.depth - 1; end + + % Returns the depth of a node + function output = getNodeDepth(obj, nodeID) + output = ceil(log(nodeID+1)/log(2)); + end end % Methods end % Class \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m index c7dfed6c0..4d21db896 100644 --- a/matlab/gtsam_tests/thinTreeBayesNet.m +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -1,7 +1,7 @@ function [bayesNet, tree] = thinTreeBayesNet(d,w) import gtsam.* bayesNet = GaussianBayesNet; -tree = thinTree(3,2); +tree = thinTree(d,w); % Filling the tree @@ -10,24 +10,30 @@ gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); % Getting it into the GaussianBayesNet bayesNet.push_front(gc); -for i=1:2^tree.getNumberOfElements() +for i=1:tree.getNumberOfElements() % Getting the parents of that node parents = tree.getParents(i); % Create and link the corresponding GaussianConditionals - if tree.getW == 1 + if tree.getW == 1 || tree.getNodeDepth(i) <= 2 % Creation of the GaussianConditional - gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1)); + gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); % Getting it into the GaussianBayesNet bayesNet.push_front(gc); % Getting it in the thinTree - t = tree.addContent({gc,parents}, i); - elseif tree.getW == 2 - % Creation of the GaussianConditional - gc = gtsam.GaussianConditional(parents(2), 5*rand(1), 5*rand(1), parents(1), 5*rand(1), 5*rand(1)); + tree = tree.addContent({gc,parents}, i); + elseif tree.getW == 2 && tree.getNodeDepth(i) > 2 + % Creation of the GaussianConditional associated with the first + % parent + gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); + % Getting it into the GaussianBayesNet + bayesNet.push_front(gc); + % Creation of the GaussianConditionalj associated with the second + % parent + gc = gtsam.GaussianConditional(parents(2), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); % Getting it into the GaussianBayesNet bayesNet.push_front(gc); % Getting it in the thinTree - t = tree.addContent({gc,parents}, i); + tree = tree.addContent({gc,parents}, i); end end end \ No newline at end of file From 60da4cb2f9a448ad70298086f1681abc403110a0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 02:53:06 +0000 Subject: [PATCH 013/107] Reversed saving of nodes --- gtsam/inference/BayesNet-inl.h | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index e113b7628..69d09a8dc 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -74,15 +74,13 @@ namespace gtsam { std::ofstream of(s.c_str()); of << "digraph G{\n"; - BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Index me = frontals.front(); -// of << me << std::endl; - typename CONDITIONAL::Parents parents = conditional->parents(); - BOOST_FOREACH(Index p, parents) - of << p << "->" << me << std::endl; - } - + BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Index me = frontals.front(); + typename CONDITIONAL::Parents parents = conditional->parents(); + BOOST_FOREACH(Index p, parents) + of << p << "->" << me << std::endl; + } of << "}"; of.close(); From a9cae46b10631d3afaec6c2cdf5a383a8c1f6bb9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 02:53:54 +0000 Subject: [PATCH 014/107] Plot Bayes net within matlab ! (dot -> png -> imread -> imshow). --- matlab/+gtsam/plotBayesNet.m | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 matlab/+gtsam/plotBayesNet.m diff --git a/matlab/+gtsam/plotBayesNet.m b/matlab/+gtsam/plotBayesNet.m new file mode 100644 index 000000000..dea9b04ad --- /dev/null +++ b/matlab/+gtsam/plotBayesNet.m @@ -0,0 +1,8 @@ +function plotBayesNet(bayesNet) +% plotBayesNet saves as dot file, renders, and shows as image +% Needs dot installed + +bayesNet.saveGraph('/tmp/bayesNet.dot') +!dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot +dotImage=imread('/tmp/dotImage.png'); +imshow(dotImage) \ No newline at end of file From ea6655beba437bde05fe01d26021009e9e6f9058 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 02:54:42 +0000 Subject: [PATCH 015/107] Fixed thin tree indexing, and conditionals --- matlab/gtsam_tests/testThinTreeBayesNet.m | 7 +-- matlab/gtsam_tests/thinTreeBayesNet.m | 53 ++++++++++------------- 2 files changed, 24 insertions(+), 36 deletions(-) diff --git a/matlab/gtsam_tests/testThinTreeBayesNet.m b/matlab/gtsam_tests/testThinTreeBayesNet.m index 55499e7ce..6f1fa164b 100644 --- a/matlab/gtsam_tests/testThinTreeBayesNet.m +++ b/matlab/gtsam_tests/testThinTreeBayesNet.m @@ -10,18 +10,15 @@ % * -------------------------------------------------------------------------- */ % % /** -% * @file testThinTree.cpp +% * @file testThinTreeBayesNet.cpp % * @brief Test of binary tree % * @date Sep 13, 2012 % * @author Frank Dellaert % * @author Jean-Guillaume Durand % */ -%% Clean the workspace -clc, clear all, close all; - %% Run the tests import gtsam.* [bayesNet tree] = thinTreeBayesNet(4,2); EQUALITY('7 = bayesNet.size', 7, bayesNet.size); -bayesNet.saveGraph('thinTreeBayesNet.dot'); \ No newline at end of file +gtsam.plotBayesNet(bayesNet); \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m index 4d21db896..e4b51633d 100644 --- a/matlab/gtsam_tests/thinTreeBayesNet.m +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -1,39 +1,30 @@ -function [bayesNet, tree] = thinTreeBayesNet(d,w) +function [bayesNet, tree] = thinTreeBayesNet(depth,width) +% thinTreeBayesNet + import gtsam.* bayesNet = GaussianBayesNet; -tree = thinTree(d,w); +tree = thinTree(depth,width); -% Filling the tree - -% Creation of the root +% Add root to the Bayes net gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); -% Getting it into the GaussianBayesNet bayesNet.push_front(gc); -for i=1:tree.getNumberOfElements() - % Getting the parents of that node - parents = tree.getParents(i); - % Create and link the corresponding GaussianConditionals - if tree.getW == 1 || tree.getNodeDepth(i) <= 2 - % Creation of the GaussianConditional - gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); - % Getting it into the GaussianBayesNet - bayesNet.push_front(gc); - % Getting it in the thinTree - tree = tree.addContent({gc,parents}, i); - elseif tree.getW == 2 && tree.getNodeDepth(i) > 2 - % Creation of the GaussianConditional associated with the first - % parent - gc = gtsam.GaussianConditional(parents(1), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); - % Getting it into the GaussianBayesNet - bayesNet.push_front(gc); - % Creation of the GaussianConditionalj associated with the second - % parent - gc = gtsam.GaussianConditional(parents(2), 5*rand(1), 5*rand(1), i, 5*rand(1), 5*rand(1)); - % Getting it into the GaussianBayesNet - bayesNet.push_front(gc); - % Getting it in the thinTree - tree = tree.addContent({gc,parents}, i); - end +n=tree.getNumberOfElements(); +for i=2:n + % Getting the parents of that node + i + parents = tree.getParents(i) + di = tree.getNodeDepth(i) + % Create and link the corresponding GaussianConditionals + if tree.getW == 1 || di == 2 + % Creation of single-parent GaussianConditional + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1)); + elseif tree.getW == 2 || di == 3 + % GaussianConditionalj associated with the second parent + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1)); + end + % Add conditional to the Bayes net + bayesNet.push_front(gc); end + end \ No newline at end of file From 107d293ed7f8b2996cc20987c5508b1615c5cee6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 02:56:19 +0000 Subject: [PATCH 016/107] Removed debug printing --- matlab/gtsam_tests/thinTreeBayesNet.m | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m index e4b51633d..d993c3fd3 100644 --- a/matlab/gtsam_tests/thinTreeBayesNet.m +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -12,9 +12,8 @@ bayesNet.push_front(gc); n=tree.getNumberOfElements(); for i=2:n % Getting the parents of that node - i - parents = tree.getParents(i) - di = tree.getNodeDepth(i) + parents = tree.getParents(i); + di = tree.getNodeDepth(i); % Create and link the corresponding GaussianConditionals if tree.getW == 1 || di == 2 % Creation of single-parent GaussianConditional From b95210a5f0ffe352e2afc5766d584e6db6239a07 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 03:05:22 +0000 Subject: [PATCH 017/107] Plot Bayes tree within matlab ! (dot -> png -> imread -> imshow). --- matlab/+gtsam/plotBayesTree.m | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 matlab/+gtsam/plotBayesTree.m diff --git a/matlab/+gtsam/plotBayesTree.m b/matlab/+gtsam/plotBayesTree.m new file mode 100644 index 000000000..94628e8a5 --- /dev/null +++ b/matlab/+gtsam/plotBayesTree.m @@ -0,0 +1,8 @@ +function plotBayesTree(bayesTree) +% plotBayesTree saves as dot file, renders, and shows as image +% Needs dot installed + +bayesTree.saveGraph('/tmp/bayesTree.dot') +!dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot +dotImage=imread('/tmp/dotImage.png'); +imshow(dotImage) \ No newline at end of file From b819b7c446ccef1e1fd38a529a16508bd8302961 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 11:11:57 +0000 Subject: [PATCH 018/107] implemented Combine to we can create a BayesTree from a DiscreteBayesNet --- gtsam/discrete/DiscreteConditional.h | 25 +++++++++++++ .../tests/testDiscreteConditional.cpp | 35 +++++++++++++------ 2 files changed, 49 insertions(+), 11 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index e05bfd669..d54bf5518 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -58,6 +59,16 @@ namespace gtsam { DiscreteConditional(const DecisionTreeFactor& joint, const DecisionTreeFactor& marginal); + /** + * Combine several conditional into a single one. + * The conditionals must be given in increasing order, meaning that the parents + * of any conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. + * */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /// @} /// @name Testable /// @{ @@ -123,5 +134,19 @@ namespace gtsam { }; // DiscreteConditional + /* ************************************************************************* */ + template + DiscreteConditional::shared_ptr DiscreteConditional::Combine( + ITERATOR firstConditional, ITERATOR lastConditional) { + // TODO: check for being a clique + DecisionTreeFactor product; + for(ITERATOR it = firstConditional; it != lastConditional; ++it) { + DiscreteConditional::shared_ptr c = *it; + DecisionTreeFactor::shared_ptr factor = c->toFactor(); + product = (*factor) * product; + } + return boost::make_shared(1,product); + } + }// gtsam diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 8af23e4f8..04bc47f60 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -29,7 +29,7 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( DiscreteConditionalTest, constructors) +TEST( DiscreteConditional, constructors) { DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering ! @@ -48,7 +48,7 @@ TEST( DiscreteConditionalTest, constructors) } /* ************************************************************************* */ -TEST( DiscreteConditionalTest, constructors_alt_interface) +TEST( DiscreteConditional, constructors_alt_interface) { DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering ! @@ -71,7 +71,7 @@ TEST( DiscreteConditionalTest, constructors_alt_interface) } /* ************************************************************************* */ -TEST( DiscreteConditionalTest, constructors2) +TEST( DiscreteConditional, constructors2) { // Declare keys and ordering DiscreteKey C(0,2), B(1,2); @@ -83,15 +83,28 @@ TEST( DiscreteConditionalTest, constructors2) } /* ************************************************************************* */ -TEST( DiscreteConditionalTest, constructors3) +TEST( DiscreteConditional, constructors3) { - // Declare keys and ordering - DiscreteKey C(0,2), B(1,2), A(2,2); - DecisionTreeFactor expected(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8"); - Signature signature((C | B, A) = "4/1 1/1 1/1 1/4"); - DiscreteConditional actual(signature); - DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor(); - EXPECT(assert_equal(expected, *actualFactor)); + // Declare keys and ordering + DiscreteKey C(0,2), B(1,2), A(2,2); + DecisionTreeFactor expected(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8"); + Signature signature((C | B, A) = "4/1 1/1 1/1 1/4"); + DiscreteConditional actual(signature); + DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor(); + EXPECT(assert_equal(expected, *actualFactor)); +} + +/* ************************************************************************* */ +TEST( DiscreteConditional, Combine) +{ + DiscreteKey A(0,2), B(1,2); + vector c; + c.push_back(boost::make_shared(A | B = "1/2 2/1")); + c.push_back(boost::make_shared(B % "1/2")); + DiscreteConditional::shared_ptr actual = DiscreteConditional::Combine(c.begin(), c.end()); + DecisionTreeFactor::shared_ptr actualFactor = actual->toFactor(); + DecisionTreeFactor expected(A & B, "0.333333 0.666667 0.666667 0.333333"); + EXPECT(assert_equal(expected, *actualFactor,1e-5)); } /* ************************************************************************* */ From 8b6c1a0b9d104c58aac140899f7f56856bb0aeff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 11:48:21 +0000 Subject: [PATCH 019/107] evaluate (interesting that this is not even defined in BayesNet, should move there?) --- gtsam/discrete/DiscreteBayesNet.cpp | 9 +++++++++ gtsam/discrete/DiscreteBayesNet.h | 5 ++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 67f4e4333..1d21d0a4b 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -36,6 +36,15 @@ namespace gtsam { bayesNet.push_back(boost::make_shared(s)); } + /* ************************************************************************* */ + double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values) { + // evaluate all conditionals and multiply + double result = 1.0; + BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + result *= (*conditional)(values); + return result; + } + /* ************************************************************************* */ DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { // solve each node in turn in topological sort order (parents first) diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index a7c35e135..c8286f3a9 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -33,7 +33,10 @@ namespace gtsam { /** Add a DiscreteCondtional in front, when listing parents first*/ void add_front(DiscreteBayesNet&, const Signature& s); - /** Optimize function for back-substitution. */ + //** evaluate for given Values */ + double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values); + + /** Optimize function for back-substitution. */ DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); /** Do ancestral sampling */ From 4be00da2912328756ea750a8dece262627fec9ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 11:48:59 +0000 Subject: [PATCH 020/107] evaluate operator --- gtsam/discrete/DiscreteConditional.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index d54bf5518..806f0f21d 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -92,6 +92,11 @@ namespace gtsam { /// @name Standard Interface /// @{ + /// Evaluate, just look up in AlgebraicDecisonTree + virtual double operator()(const Values& values) const { + return Potentials::operator()(values); + } + /** Convert to a factor */ DecisionTreeFactor::shared_ptr toFactor() const { return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); From 1df761d9a0bf84faff5e258a1f5f980c1e5c4964 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 11:49:42 +0000 Subject: [PATCH 021/107] Small formatting --- gtsam/discrete/DiscreteMarginals.h | 1 - .../discrete/tests/testDiscreteMarginals.cpp | 56 ++++++++++--------- 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index 1e89286a8..ff07e5a6c 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -22,7 +22,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index d0670bcf7..62a69de8c 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -19,8 +19,10 @@ #include #include + #include using namespace boost::assign; + #include using namespace std; @@ -65,19 +67,19 @@ TEST_UNSAFE( DiscreteMarginals, UGM_chain ) { const size_t nrStates = 7; // define variables - vector nodes; + vector key; for (int i = 0; i < nrNodes; i++) { - DiscreteKey dk(i, nrStates); - nodes.push_back(dk); + DiscreteKey key_i(i, nrStates); + key.push_back(key_i); } // create graph DiscreteFactorGraph graph; // add node potentials - graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); + graph.add(key[0], ".3 .6 .1 0 0 0 0"); for (int i = 1; i < nrNodes; i++) - graph.add(nodes[i], "1 1 1 1 1 1 1"); + graph.add(key[i], "1 1 1 1 1 1 1"); const std::string edgePotential = ".08 .9 .01 0 0 0 .01 " ".03 .95 .01 0 0 0 .01 " @@ -89,13 +91,13 @@ TEST_UNSAFE( DiscreteMarginals, UGM_chain ) { // add edge potentials for (int i = 0; i < nrNodes - 1; i++) - graph.add(nodes[i] & nodes[i + 1], edgePotential); + graph.add(key[i] & key[i + 1], edgePotential); DiscreteMarginals marginals(graph); - DiscreteFactor::shared_ptr actualC = marginals(nodes[2].first); + DiscreteFactor::shared_ptr actualC = marginals(key[2].first); DiscreteFactor::Values values; - values[nodes[2].first] = 0; + values[key[2].first] = 0; EXPECT_DOUBLES_EQUAL( 0.03426, (*actualC)(values), 1e-4); } @@ -106,28 +108,28 @@ TEST_UNSAFE( DiscreteMarginals, truss ) { const size_t nrStates = 2; // define variables - vector nodes; + vector key; for (int i = 0; i < nrNodes; i++) { - DiscreteKey dk(i, nrStates); - nodes.push_back(dk); + DiscreteKey key_i(i, nrStates); + key.push_back(key_i); } // create graph and add three truss potentials DiscreteFactorGraph graph; - graph.add(nodes[0] & nodes[2] & nodes[4],"2 2 2 2 1 1 1 1"); - graph.add(nodes[1] & nodes[3] & nodes[4],"1 1 1 1 2 2 2 2"); - graph.add(nodes[2] & nodes[3] & nodes[4],"1 1 1 1 1 1 1 1"); + graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1"); + graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2"); + graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1"); typedef JunctionTree JT; GenericMultifrontalSolver solver(graph); BayesTree::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete); // bayesTree->print("Bayes Tree"); typedef BayesTreeClique Clique; - Clique expected0(boost::make_shared((nodes[0] | nodes[2], nodes[4]) = "2/1 2/1 2/1 2/1")); + Clique expected0(boost::make_shared((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1")); Clique::shared_ptr actual0 = (*bayesTree)[0]; // EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails - Clique expected1(boost::make_shared((nodes[1] | nodes[3], nodes[4]) = "1/2 1/2 1/2 1/2")); + Clique expected1(boost::make_shared((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2")); Clique::shared_ptr actual1 = (*bayesTree)[1]; // EXPECT(assert_equal(expected1, *actual1)); // TODO, correct but fails @@ -135,12 +137,12 @@ TEST_UNSAFE( DiscreteMarginals, truss ) { DiscreteMarginals marginals(graph); // test 0 - DecisionTreeFactor expectedM0(nodes[0],"0.666667 0.333333"); + DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333"); DiscreteFactor::shared_ptr actualM0 = marginals(0); EXPECT(assert_equal(expectedM0, *boost::dynamic_pointer_cast(actualM0),1e-5)); // test 1 - DecisionTreeFactor expectedM1(nodes[1],"0.333333 0.666667"); + DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667"); DiscreteFactor::shared_ptr actualM1 = marginals(1); EXPECT(assert_equal(expectedM1, *boost::dynamic_pointer_cast(actualM1),1e-5)); } @@ -153,21 +155,21 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { const size_t nrStates = 2; // define variables - vector nodes; + vector key; for (int i = 0; i < nrNodes; i++) { - DiscreteKey dk(i, nrStates); - nodes.push_back(dk); + DiscreteKey key_i(i, nrStates); + key.push_back(key_i); } // create graph and add three truss potentials DiscreteFactorGraph graph; - graph.add(nodes[0] & nodes[2] & nodes[4],"1 2 3 4 5 6 7 8"); - graph.add(nodes[1] & nodes[3] & nodes[4],"1 2 3 4 5 6 7 8"); - graph.add(nodes[2] & nodes[3] & nodes[4],"1 2 3 4 5 6 7 8"); + graph.add(key[0] & key[2] & key[4],"1 2 3 4 5 6 7 8"); + graph.add(key[1] & key[3] & key[4],"1 2 3 4 5 6 7 8"); + graph.add(key[2] & key[3] & key[4],"1 2 3 4 5 6 7 8"); // Calculate the marginals by brute force vector allPosbValues = cartesianProduct( - nodes[0] & nodes[1] & nodes[2] & nodes[3] & nodes[4]); + key[0] & key[1] & key[2] & key[3] & key[4]); Vector T = zero(5), F = zero(5); for (size_t i = 0; i < allPosbValues.size(); ++i) { DiscreteFactor::Values x = allPosbValues[i]; @@ -186,13 +188,13 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { F[j]/=sum; // solver - Vector actualV = solver.marginalProbabilities(nodes[j]); + Vector actualV = solver.marginalProbabilities(key[j]); EXPECT(assert_equal(Vector_(2,F[j],T[j]), actualV)); // Marginals vector table; table += F[j],T[j]; - DecisionTreeFactor expectedM(nodes[j],table); + DecisionTreeFactor expectedM(key[j],table); DiscreteFactor::shared_ptr actualM = marginals(j); EXPECT(assert_equal(expectedM, *boost::dynamic_pointer_cast(actualM))); } From e26ab012def801fdad1f273b80466f646ee57003 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Sep 2012 13:21:43 +0000 Subject: [PATCH 022/107] custom clique checks Combine and DiscreteBayesTree construction (testing 32000 configurations :-)) --- .cproject | 356 +++++++++--------- gtsam/discrete/DiscreteConditional.h | 8 +- .../discrete/tests/testDiscreteBayesTree.cpp | 135 +++++++ .../tests/testDiscreteConditional.cpp | 14 +- gtsam/inference/BayesTreeCliqueBase-inl.h | 1 - 5 files changed, 333 insertions(+), 181 deletions(-) create mode 100644 gtsam/discrete/tests/testDiscreteBayesTree.cpp diff --git a/.cproject b/.cproject index df6fdc5f5..69a891199 100644 --- a/.cproject +++ b/.cproject @@ -309,6 +309,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +343,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +350,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +397,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +404,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +411,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +426,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +519,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +535,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +575,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -685,26 +679,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -813,6 +807,30 @@ true true + + make + -j5 + testDiscreteBayesTree.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + make -j5 @@ -991,7 +1009,6 @@ make - testGraph.run true false @@ -999,7 +1016,6 @@ make - testJunctionTree.run true false @@ -1007,7 +1023,6 @@ make - testSymbolicBayesNetB.run true false @@ -1167,7 +1182,6 @@ make - testErrors.run true false @@ -1213,10 +1227,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1301,10 +1315,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1639,6 +1653,7 @@ make + testSimulated2DOriented.run true false @@ -1678,6 +1693,7 @@ make + testSimulated2D.run true false @@ -1685,6 +1701,7 @@ make + testSimulated3D.run true false @@ -1876,6 +1893,7 @@ make + tests/testGaussianISAM2 true false @@ -1897,9 +1915,105 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make - -j1 + -j3 install true false @@ -2098,7 +2212,6 @@ cpack - -G DEB true false @@ -2106,7 +2219,6 @@ cpack - -G RPM true false @@ -2114,7 +2226,6 @@ cpack - -G TGZ true false @@ -2122,7 +2233,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2256,98 +2366,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2391,38 +2437,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 806f0f21d..4512ea7b0 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -144,13 +144,17 @@ namespace gtsam { DiscreteConditional::shared_ptr DiscreteConditional::Combine( ITERATOR firstConditional, ITERATOR lastConditional) { // TODO: check for being a clique + + // multiply all the potentials of the given conditionals + size_t nrFrontals = 0; DecisionTreeFactor product; - for(ITERATOR it = firstConditional; it != lastConditional; ++it) { + for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { DiscreteConditional::shared_ptr c = *it; DecisionTreeFactor::shared_ptr factor = c->toFactor(); product = (*factor) * product; } - return boost::make_shared(1,product); + // and then create a new multi-frontal conditional + return boost::make_shared(nrFrontals,product); } }// gtsam diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp new file mode 100644 index 000000000..c2bbcb55e --- /dev/null +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testDiscreteBayesTree.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + */ + +#include +#include + +#include +using namespace boost::assign; + +#include + +using namespace std; +using namespace gtsam; + +static bool debug = false; + +/** + * Custom clique class to debug shortcuts + */ +class Clique: public BayesTreeCliqueBase { +public: + + typedef BayesTreeCliqueBase Base; + + // Constructors + Clique() { + } + Clique(const DiscreteConditional::shared_ptr& conditional) : + Base(conditional) { + } + Clique( + const std::pair& result) : + Base(result) { + } + + // evaluate value of sub-tree + double evaluate(const DiscreteConditional::Values & values) { + double result = (*(this->conditional_))(values); + // evaluate all children and multiply into result + BOOST_FOREACH(boost::shared_ptr c, children_) + result *= c->evaluate(values); + return result; + } +}; + +typedef BayesTree DiscreteBayesTree; + +/* ************************************************************************* */ +double evaluate(const DiscreteBayesTree& tree, + const DiscreteConditional::Values & values) { + return tree.root()->evaluate(values); +} + +/* ************************************************************************* */ + +TEST_UNSAFE( DiscreteMarginals, thinTree ) { + + const int nrNodes = 15; + const size_t nrStates = 2; + + // define variables + vector key; + for (int i = 0; i < nrNodes; i++) { + DiscreteKey key_i(i, nrStates); + key.push_back(key_i); + } + + // create a thin-tree Bayesnet, a la Jean-Guillaume + DiscreteBayesNet bayesNet; + add_front(bayesNet, key[14] % "1/3"); + + add_front(bayesNet, key[13] | key[14] = "1/3 3/1"); + add_front(bayesNet, key[12] | key[14] = "3/1 3/1"); + + add_front(bayesNet, (key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1"); + add_front(bayesNet, (key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1"); + add_front(bayesNet, (key[9] | key[12], key[14]) = "4/1 2/3 3/2 1/4"); + add_front(bayesNet, (key[8] | key[12], key[14]) = "2/3 1/4 3/2 4/1"); + + add_front(bayesNet, (key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1"); + add_front(bayesNet, (key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1"); + add_front(bayesNet, (key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4"); + add_front(bayesNet, (key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1"); + + add_front(bayesNet, (key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1"); + add_front(bayesNet, (key[2] | key[9], key[12]) = "1/4 3/2 2/3 4/1"); + add_front(bayesNet, (key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4"); + add_front(bayesNet, (key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1"); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/discreteBayesNet.dot"); + } + + // create a BayesTree out of a Bayes net + DiscreteBayesTree bayesTree(bayesNet); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/discreteBayesTree.dot"); + } + + // Check whether BN and BT give the same answer on all configurations + vector allPosbValues = cartesianProduct( + key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] + & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); + for (size_t i = 0; i < allPosbValues.size(); ++i) { + DiscreteFactor::Values x = allPosbValues[i]; + double expected = evaluate(bayesNet, x); + double actual = evaluate(bayesTree, x); + DOUBLES_EQUAL(expected, actual, 1e-9); + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 04bc47f60..568f98db7 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -95,16 +95,16 @@ TEST( DiscreteConditional, constructors3) } /* ************************************************************************* */ -TEST( DiscreteConditional, Combine) -{ - DiscreteKey A(0,2), B(1,2); +TEST( DiscreteConditional, Combine) { + DiscreteKey A(0, 2), B(1, 2); vector c; c.push_back(boost::make_shared(A | B = "1/2 2/1")); c.push_back(boost::make_shared(B % "1/2")); - DiscreteConditional::shared_ptr actual = DiscreteConditional::Combine(c.begin(), c.end()); - DecisionTreeFactor::shared_ptr actualFactor = actual->toFactor(); - DecisionTreeFactor expected(A & B, "0.333333 0.666667 0.666667 0.333333"); - EXPECT(assert_equal(expected, *actualFactor,1e-5)); + DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222"); + DiscreteConditional expected(2, factor); + DiscreteConditional::shared_ptr actual = DiscreteConditional::Combine( + c.begin(), c.end()); + EXPECT(assert_equal(expected, *actual,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 344f845ac..7ca40345b 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -6,7 +6,6 @@ * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information - * -------------------------------------------------------------------------- */ /** From 896d006ddf6fdcafcc459788e96c9af34b4326b6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 04:36:35 +0000 Subject: [PATCH 023/107] return all keys (slow) --- gtsam/inference/FactorGraph-inl.h | 28 ++++++++++++++++++++-------- gtsam/inference/FactorGraph.h | 6 +++++- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 04490dbc6..47e330d7b 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -77,14 +77,26 @@ namespace gtsam { return true; } - /* ************************************************************************* */ - template - size_t FactorGraph::nrFactors() const { - size_t size_ = 0; - for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) - if (*factor != NULL) size_++; - return size_; - } + /* ************************************************************************* */ + template + size_t FactorGraph::nrFactors() const { + size_t size_ = 0; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) size_++; + return size_; + } + + /* ************************************************************************* */ + template + std::set FactorGraph::keys() const { + std::set allKeys; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) { + BOOST_FOREACH(Index j, factor->keys()) + allKeys.insert(j); + } + return allKeys; + } /* ************************************************************************* */ template diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index ed3e7d952..ae6c03439 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -27,6 +27,7 @@ #include #include +#include namespace gtsam { @@ -44,7 +45,7 @@ template class BayesTree; public: typedef FACTOR FactorType; ///< factor type - typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index factors with + typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional @@ -208,6 +209,9 @@ template class BayesTree; /** return the number valid factors */ size_t nrFactors() const; + /** Potentially very slow function to return all keys involved */ + std::set keys() const; + private: /** Serialization function */ From 44c66cb0cb3a418c5fd2778c3b31313936b47ee8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 04:37:04 +0000 Subject: [PATCH 024/107] moved stuff to cpp file --- gtsam/discrete/DiscreteConditional.cpp | 13 +++++++++++++ gtsam/discrete/DiscreteConditional.h | 18 +++++------------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 029873d2c..b42e3ba24 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -57,6 +57,19 @@ namespace gtsam { signature.discreteKeysParentsFirst(), signature.cpt()) { } + /* ******************************************************************************** */ + void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { + std::cout << s << std::endl; + IndexConditional::print(s, formatter); + Potentials::print(s); + } + + /* ******************************************************************************** */ + bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const { + return IndexConditional::equals(other, tol) + && Potentials::equals(other, tol); + } + /* ******************************************************************************** */ Potentials::ADT DiscreteConditional::choose( const Values& parentsValues) const { diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 4512ea7b0..a11bdca45 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -73,20 +73,12 @@ namespace gtsam { /// @name Testable /// @{ - /** GTSAM-style print */ - void print(const std::string& s = "Discrete Conditional: ", - const IndexFormatter& formatter - =DefaultIndexFormatter) const { - std::cout << s << std::endl; - IndexConditional::print(s, formatter); - Potentials::print(s); - } + /// GTSAM-style print + void print(const std::string& s = "Discrete Conditional: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; - /** GTSAM-style equals */ - bool equals(const DiscreteConditional& other, double tol = 1e-9) const { - return IndexConditional::equals(other, tol) - && Potentials::equals(other, tol); - } + /// GTSAM-style equals + bool equals(const DiscreteConditional& other, double tol = 1e-9) const; /// @} /// @name Standard Interface From 9094fe27449c02e9131a2034a8bef0c69a6cebb2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 04:37:59 +0000 Subject: [PATCH 025/107] Fully functioning, non-buggy separator shortcuts. Still not as tight as they can be.... --- .../discrete/tests/testDiscreteBayesTree.cpp | 175 +++++++++++++++++- 1 file changed, 167 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index c2bbcb55e..a86cd3e32 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -32,9 +33,42 @@ static bool debug = false; * Custom clique class to debug shortcuts */ class Clique: public BayesTreeCliqueBase { + +protected: + + /** + * Determine variable indices to keep in recursive separator shortcut calculation + * The factor graph p_Cp_B has keys from the parent clique Cp and from B. + * But we only keep the variables not in S union B. + */ + vector indices(derived_ptr B, + const FactorGraph& p_Cp_B) const { + + // Get all keys + set allKeys = p_Cp_B.keys(); + + // We do this by first merging S and B + boost::iterator_range indicesS = + this->conditional()->parents(); + size_t sizeS = indicesS.end() - indicesS.begin(); + vector &indicesB = B->conditional()->keys(); + vector S_union_B(indicesB.size() + sizeS); + vector::iterator it = set_union(indicesS.begin(), indicesS.end(), + indicesB.begin(), indicesB.end(), S_union_B.begin()); + + // then intersecting S_union_B with allKeys + vector keepers(indicesB.size() + sizeS); + it = set_intersection(S_union_B.begin(), it, allKeys.begin(), allKeys.end(), + keepers.begin()); + keepers.erase(it, keepers.end()); + + return keepers; + } + public: typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; // Constructors Clique() { @@ -48,7 +82,13 @@ public: Base(result) { } - // evaluate value of sub-tree + /// print index signature only + void printSignature(const std::string& s = "Clique: ", + const IndexFormatter& indexFormatter = DefaultIndexFormatter) const { + ((IndexConditional::shared_ptr) conditional_)->print(s, indexFormatter); + } + + /// evaluate value of sub-tree double evaluate(const DiscreteConditional::Values & values) { double result = (*(this->conditional_))(values); // evaluate all children and multiply into result @@ -56,6 +96,85 @@ public: result *= c->evaluate(values); return result; } + + /** + * Separator shortcut function P(S||B) = P(S\B|B) + * where S is a clique separator, and B any node (e.g., a brancing in the tree) + * We can compute it recursively from the parent shortcut + * P(Sp||B) as \int P(Fp|Sp) P(Sp||B), where Fp are the frontal nodes in p + */ + FactorGraph::shared_ptr separatorShortcut(derived_ptr B) const { + + FactorGraph::shared_ptr p_S_B; //shortcut P(S||B) This is empty now + + // We only calculate the shortcut when this clique is not B + derived_ptr parent(parent_.lock()); + if (B.get() != this) { + + // Obtain P(Fp|Sp) as a factor + boost::shared_ptr p_Fp_Sp = parent->conditional()->toFactor(); + + // Obtain the parent shortcut P(Sp|B) as factors + // TODO: really annoying that we eliminate more than we have to ! + // TODO: we should only eliminate C_p\B, with S\B variables last + // TODO: and this index dance will be easier then, as well + FactorGraph p_Sp_B(parent->shortcut(B, &EliminateDiscrete)); + + // now combine P(Cp||B) = P(Fp|Sp) * P(Sp||B) + FactorGraph p_Cp_B; + p_Cp_B.push_back(p_Fp_Sp); + p_Cp_B.push_back(p_Sp_B); + + // Create a generic solver that will marginalize for us + GenericSequentialSolver solver(p_Cp_B); + + // The factor graph above will have keys from the parent clique Cp and from B. + // But we only keep the variables not in S union B. + vector keepers = indices(B, p_Cp_B); + + p_S_B = solver.jointFactorGraph(keepers, &EliminateDiscrete); + } + // return the shortcut P(S||B) + return p_S_B; + } + + /** + * The shortcut density is a conditional P(S||B) of the separator of this + * clique on the clique B. + */ + BayesNet shortcut(derived_ptr B, + Eliminate function) const { + + //Check if the ShortCut already exists + if (cachedShortcut_) { + return *cachedShortcut_; // return the cached version + } else { + BayesNet bn; + FactorGraph::shared_ptr fg = separatorShortcut(B); + if (fg) { + // calculate set S\B of indices to keep in Bayes net + vector indicesS(this->conditional()->beginParents(), + this->conditional()->endParents()); + // now get B indices out + vector &indicesB = B->conditional()->keys(); + vector S_setminus_B(indicesS.size()); + vector::iterator it = set_difference(indicesS.begin(), + indicesS.end(), indicesB.begin(), indicesB.end(), + S_setminus_B.begin()); + S_setminus_B.erase(it, S_setminus_B.end()); + set keep(S_setminus_B.begin(), S_setminus_B.end()); + BOOST_FOREACH (FactorType::shared_ptr factor,*fg) { + DecisionTreeFactor::shared_ptr df = boost::dynamic_pointer_cast< + DecisionTreeFactor>(factor); + if (keep.count(*factor->begin())) + bn.push_front(boost::make_shared(1, *df)); + } + } + cachedShortcut_ = bn; + return bn; + } + } + }; typedef BayesTree DiscreteBayesTree; @@ -73,14 +192,14 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { const int nrNodes = 15; const size_t nrStates = 2; - // define variables +// define variables vector key; for (int i = 0; i < nrNodes; i++) { DiscreteKey key_i(i, nrStates); key.push_back(key_i); } - // create a thin-tree Bayesnet, a la Jean-Guillaume +// create a thin-tree Bayesnet, a la Jean-Guillaume DiscreteBayesNet bayesNet; add_front(bayesNet, key[14] % "1/3"); @@ -89,8 +208,8 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { add_front(bayesNet, (key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1"); add_front(bayesNet, (key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1"); - add_front(bayesNet, (key[9] | key[12], key[14]) = "4/1 2/3 3/2 1/4"); - add_front(bayesNet, (key[8] | key[12], key[14]) = "2/3 1/4 3/2 4/1"); + add_front(bayesNet, (key[9] | key[12], key[14]) = "4/1 2/3 F 1/4"); + add_front(bayesNet, (key[8] | key[12], key[14]) = "T 1/4 3/2 4/1"); add_front(bayesNet, (key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1"); add_front(bayesNet, (key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1"); @@ -98,7 +217,7 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { add_front(bayesNet, (key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1"); add_front(bayesNet, (key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1"); - add_front(bayesNet, (key[2] | key[9], key[12]) = "1/4 3/2 2/3 4/1"); + add_front(bayesNet, (key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1"); add_front(bayesNet, (key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4"); add_front(bayesNet, (key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1"); @@ -107,14 +226,17 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { bayesNet.saveGraph("/tmp/discreteBayesNet.dot"); } - // create a BayesTree out of a Bayes net +// create a BayesTree out of a Bayes net DiscreteBayesTree bayesTree(bayesNet); if (debug) { GTSAM_PRINT(bayesTree); bayesTree.saveGraph("/tmp/discreteBayesTree.dot"); } - // Check whether BN and BT give the same answer on all configurations +// Check whether BN and BT give the same answer on all configurations +// Also calculate all some marginals + Vector marginals = zero(15); + double shortcut0, sum0; vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); @@ -123,7 +245,44 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { double expected = evaluate(bayesNet, x); double actual = evaluate(bayesTree, x); DOUBLES_EQUAL(expected, actual, 1e-9); + // collect marginals + for (size_t i = 0; i < 15; i++) + if (x[i]) + marginals[i] += actual; + // calculate a deep shortcut + if (x[12] && x[14] & x[8]) + shortcut0 += actual; + if (x[14]) + sum0 += actual; } + DiscreteFactor::Values all1 = allPosbValues.back(); + + // check shortcut P(S0||R) to root + Clique::shared_ptr R = bayesTree.root(); + Clique::shared_ptr c = bayesTree[0]; + DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(shortcut0/sum0, evaluate(shortcut,all1), 1e-9); + + // calculate all shortcuts to root + DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); + BOOST_FOREACH(Clique::shared_ptr c, cliques) { + DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); + if (debug) { + c->printSignature(); + shortcut.print("shortcut:"); + } + } + + // Check all marginals + DiscreteFactor::shared_ptr marginalFactor; + for (size_t i = 0; i < 15; i++) { + marginalFactor = bayesTree.marginalFactor(i, &EliminateDiscrete); + DiscreteFactor::Values x; + x[i] = 1; + double actual = (*marginalFactor)(x); + EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9); + } + } /* ************************************************************************* */ From 16c8cfb1cf3f82fd7b310f48401e8abfd36a0308 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 13:28:50 +0000 Subject: [PATCH 026/107] Better set calculations --- .cproject | 4 +- .../discrete/tests/testDiscreteBayesTree.cpp | 44 +++++++++---------- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/.cproject b/.cproject index 69a891199..6b473f6be 100644 --- a/.cproject +++ b/.cproject @@ -809,10 +809,10 @@ make - -j5 + -j1 testDiscreteBayesTree.run true - true + false true diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index a86cd3e32..6109948ca 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -36,6 +36,16 @@ class Clique: public BayesTreeCliqueBase { protected: + /// Calculate set S\B + vector separatorShortcutVariables(derived_ptr B) const { + sharedConditional p_F_S = this->conditional(); + vector &indicesB = B->conditional()->keys(); + vector S_setminus_B; + set_difference(p_F_S->beginParents(), p_F_S->endParents(), // + indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); + return S_setminus_B; + } + /** * Determine variable indices to keep in recursive separator shortcut calculation * The factor graph p_Cp_B has keys from the parent clique Cp and from B. @@ -44,23 +54,18 @@ protected: vector indices(derived_ptr B, const FactorGraph& p_Cp_B) const { - // Get all keys - set allKeys = p_Cp_B.keys(); - // We do this by first merging S and B - boost::iterator_range indicesS = - this->conditional()->parents(); - size_t sizeS = indicesS.end() - indicesS.begin(); + sharedConditional p_F_S = this->conditional(); vector &indicesB = B->conditional()->keys(); - vector S_union_B(indicesB.size() + sizeS); - vector::iterator it = set_union(indicesS.begin(), indicesS.end(), - indicesB.begin(), indicesB.end(), S_union_B.begin()); + vector S_union_B; + set_union(p_F_S->beginParents(), p_F_S->endParents(), // + indicesB.begin(), indicesB.end(), back_inserter(S_union_B)); - // then intersecting S_union_B with allKeys - vector keepers(indicesB.size() + sizeS); - it = set_intersection(S_union_B.begin(), it, allKeys.begin(), allKeys.end(), - keepers.begin()); - keepers.erase(it, keepers.end()); + // then intersecting S_union_B with all keys in p_Cp_B + set allKeys = p_Cp_B.keys(); + vector keepers; + set_intersection(S_union_B.begin(), S_union_B.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keepers)); return keepers; } @@ -153,16 +158,9 @@ public: FactorGraph::shared_ptr fg = separatorShortcut(B); if (fg) { // calculate set S\B of indices to keep in Bayes net - vector indicesS(this->conditional()->beginParents(), - this->conditional()->endParents()); - // now get B indices out - vector &indicesB = B->conditional()->keys(); - vector S_setminus_B(indicesS.size()); - vector::iterator it = set_difference(indicesS.begin(), - indicesS.end(), indicesB.begin(), indicesB.end(), - S_setminus_B.begin()); - S_setminus_B.erase(it, S_setminus_B.end()); + vector S_setminus_B = separatorShortcutVariables(B); set keep(S_setminus_B.begin(), S_setminus_B.end()); + BOOST_FOREACH (FactorType::shared_ptr factor,*fg) { DecisionTreeFactor::shared_ptr df = boost::dynamic_pointer_cast< DecisionTreeFactor>(factor); From dfbb02570b3c044fbd2ae098da94fe7218e0c977 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 13:40:31 +0000 Subject: [PATCH 027/107] Fixed confusing template naming --- gtsam/inference/VariableIndex.h | 34 ++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index d747d1122..312906218 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -64,13 +64,13 @@ public: * of a factor graph. This constructor is used when the number of variables * is known beforehand. */ - template VariableIndex(const FactorGraph& factorGraph, Index nVariables); + template VariableIndex(const FG& factorGraph, Index nVariables); /** * Create a VariableIndex that computes and stores the block column structure * of a factor graph. */ - template VariableIndex(const FactorGraph& factorGraph); + template VariableIndex(const FG& factorGraph); /// @} /// @name Standard Interface @@ -116,7 +116,7 @@ public: * Augment the variable index with new factors. This can be used when * solving problems incrementally. */ - template void augment(const FactorGraph& factors); + template void augment(const FG& factors); /** * Remove entries corresponding to the specified factors. @@ -129,7 +129,7 @@ public: * @param factors The factors being removed, which must symbolically correspond * exactly to the factors with the specified \c indices that were added. */ - template void remove(const CONTAINER& indices, const FactorGraph& factors); + template void remove(const CONTAINER& indices, const FG& factors); /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); @@ -154,14 +154,14 @@ protected: void checkVar(Index variable) const { assert(variable < index_.size()); } /// Internal function to populate the variable index from a factor graph - template void fill(const FactorGraph& factorGraph); + template void fill(const FG& factorGraph); /// @} }; /* ************************************************************************* */ -template -void VariableIndex::fill(const FactorGraph& factorGraph) { +template +void VariableIndex::fill(const FG& factorGraph) { // Build index mapping from variable id to factor index for(size_t fi=0; fi -VariableIndex::VariableIndex(const FactorGraph& factorGraph) : +template +VariableIndex::VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { // If the factor graph is empty, return an empty index because inside this @@ -187,7 +187,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : if(factorGraph.size() > 0) { // Find highest-numbered variable Index maxVar = 0; - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) { + BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { if(factor) { BOOST_FOREACH(const Index key, factor->keys()) { if(key > maxVar) @@ -204,21 +204,21 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : } /* ************************************************************************* */ -template -VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : +template +VariableIndex::VariableIndex(const FG& factorGraph, Index nVariables) : index_(nVariables), nFactors_(0), nEntries_(0) { fill(factorGraph); } /* ************************************************************************* */ -template -void VariableIndex::augment(const FactorGraph& factors) { +template +void VariableIndex::augment(const FG& factors) { // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. if(factors.size() > 0) { // Find highest-numbered variable Index maxVar = 0; - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factors) { + BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) { if(factor) { BOOST_FOREACH(const Index key, factor->keys()) { if(key > maxVar) @@ -245,8 +245,8 @@ void VariableIndex::augment(const FactorGraph& factors) { } /* ************************************************************************* */ -template -void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) { +template +void VariableIndex::remove(const CONTAINER& indices, const FG& factors) { // NOTE: We intentionally do not decrement nFactors_ because the factor // indices need to remain consistent. Removing factors from a factor graph // does not shift the indices of other factors. Also, we keep nFactors_ From 1bbbd0ee4a2c351e91650f3bf2a277f9f65b54b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 14:34:19 +0000 Subject: [PATCH 028/107] Cleaned up unit test *mess* --- gtsam/inference/tests/testEliminationTree.cpp | 16 +- .../tests/testSymbolicFactorGraph.cpp | 307 ------------------ .../tests/testSymbolicSequentialSolver.cpp | 82 +++++ 3 files changed, 87 insertions(+), 318 deletions(-) create mode 100644 gtsam/inference/tests/testSymbolicSequentialSolver.cpp diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp index c1df6e98e..00679083a 100644 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ b/gtsam/inference/tests/testEliminationTree.cpp @@ -79,18 +79,12 @@ TEST(EliminationTree, Create) TEST(EliminationTree, eliminate ) { // create expected Chordal bayes Net - IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2)); - IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4)); - IndexConditional::shared_ptr c2(new IndexConditional(2, 4)); - IndexConditional::shared_ptr c3(new IndexConditional(3, 4)); - IndexConditional::shared_ptr c4(new IndexConditional(4)); - SymbolicBayesNet expected; - expected.push_back(c0); - expected.push_back(c1); - expected.push_back(c2); - expected.push_back(c3); - expected.push_back(c4); + expected.push_front(boost::make_shared(4)); + expected.push_front(boost::make_shared(3,4)); + expected.push_front(boost::make_shared(2,4)); + expected.push_front(boost::make_shared(1,2,4)); + expected.push_front(boost::make_shared(0,1,2)); // Create factor graph SymbolicFactorGraph fg; diff --git a/gtsam/inference/tests/testSymbolicFactorGraph.cpp b/gtsam/inference/tests/testSymbolicFactorGraph.cpp index 15538851e..607a0aa7c 100644 --- a/gtsam/inference/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/inference/tests/testSymbolicFactorGraph.cpp @@ -48,20 +48,6 @@ static const Index vl1 = 2; // CHECK(assert_equal(expected, fg)); //} -/* ************************************************************************* */ -TEST( SymbolicFactorGraph, SymbolicSequentialSolver ) -{ - // create factor graph - SymbolicFactorGraph g; - g.push_factor(vx2, vx1, vl1); - g.push_factor(vx1, vl1); - g.push_factor(vx1); - // test solver is Testable - SymbolicSequentialSolver solver(g); -// GTSAM_PRINT(solver); - EXPECT(assert_equal(solver,solver)); -} - /* ************************************************************************* */ TEST( SymbolicFactorGraph, constructFromBayesNet ) { @@ -113,299 +99,6 @@ TEST( SymbolicFactorGraph, push_back ) CHECK(assert_equal(expected, fg1)); } -/* ************************************************************************* */ - -///** -// * An elimination tree is a tree of factors -// */ -//class ETree: public Testable { -// -//public: -// -// typedef boost::shared_ptr sharedFactor; -// typedef boost::shared_ptr shared_ptr; -// -//private: -// -// Index key_; /** index associated with root */ -// list factors_; /** factors associated with root */ -// list subTrees_; /** sub-trees */ -// -// typedef pair Result; -// -// /** -// * Recursive routine that eliminates the factors arranged in an elimination tree -// */ -// Result eliminate_() const { -// -// SymbolicBayesNet bn; -// -// set separator; -// -// // loop over all factors associated with root -// // and set-union their keys to a separator -// BOOST_FOREACH(const sharedFactor& factor, factors_) -// BOOST_FOREACH(Index key, *factor) { -// if (key != key_) separator.insert(key); } -// -// // for all children, eliminate into Bayes net -// BOOST_FOREACH(const shared_ptr& child, subTrees_) { -// Result result = child->eliminate_(); -// bn.push_back(result.first); -// BOOST_FOREACH(Index key, result.second) -// if (key != key_) separator.insert(key); -// } -// -// // Make the conditional from the key and separator, and insert it in Bayes net -// vector parents; -// std::copy(separator.begin(), separator.end(), back_inserter(parents)); -// IndexConditional::shared_ptr conditional(new IndexConditional(key_, parents)); -// bn.push_back(conditional); -// -// // now create the new factor from separator to return to caller -// IndexFactor newFactor(separator.begin(), separator.end()); -// return Result(bn, newFactor); -// } -// -//public: -// -// /** default constructor */ -// ETree(Index key = 0) : -// key_(key) { -// } -// -// /** add a factor */ -// void add(const sharedFactor& factor) { -// factors_.push_back(factor); -// } -// -// /** add a subtree */ -// void add(const shared_ptr& child) { -// subTrees_.push_back(child); -// } -// -// void print(const std::string& name) const { -// cout << name << " (" << key_ << ")" << endl; -// BOOST_FOREACH(const sharedFactor& factor, factors_) -// factor->print(name + " "); -// BOOST_FOREACH(const shared_ptr& child, subTrees_) -// child->print(name + " "); -// } -// -// bool equals(const ETree& expected, double tol) const { -// // todo -// return false; -// } -// -// /** -// * Eliminate the factors to a Bayes Net -// */ -// SymbolicBayesNet eliminate() const { -// -// // call recursive routine -// Result result = eliminate_(); -// return result.first; -// } -// -//}; -// -//// build hardcoded tree -//ETree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { -// -// ETree::shared_ptr leaf0(new ETree); -// leaf0->add(fg[0]); -// leaf0->add(fg[1]); -// -// ETree::shared_ptr node1(new ETree(1)); -// node1->add(fg[2]); -// node1->add(leaf0); -// -// ETree::shared_ptr node2(new ETree(2)); -// node2->add(fg[3]); -// node2->add(node1); -// -// ETree::shared_ptr leaf3(new ETree(3)); -// leaf3->add(fg[4]); -// -// ETree::shared_ptr etree(new ETree(4)); -// etree->add(leaf3); -// etree->add(node2); -// -// return etree; -//} -// -//typedef size_t RowIndex; -//typedef vector > StructA; -//typedef vector > optionalIndices; -// -///** -// * Gilbert01bit algorithm in Figure 2.2 -// */ -//optionalIndices buildETree(const StructA& structA) { -// -// // todo: get n -// size_t n = 5; -// optionalIndices parent(n); -// -// // todo: get m -// size_t m = 5; -// optionalIndices prev_col(m); -// -// // for column j \in 1 to n do -// for (Index j = 0; j < n; j++) { -// // for row i \in Struct[A*j] do -// BOOST_FOREACH(RowIndex i, structA[j]) { -// if (prev_col[i]) { -// Index k = *(prev_col[i]); -// // find root r of the current tree that contains k -// Index r = k; -// while (parent[r]) -// r = *parent[r]; -// if (r != j) parent[r].reset(j); -// } -// prev_col[i].reset(j); -// } -// } -// -// return parent; -//} -// -///** -// * TODO: Build StructA from factor graph -// */ -//StructA createStructA(const SymbolicFactorGraph& fg) { -// -// StructA structA; -// -// // hardcode for now -// list list0; -// list0 += 0, 1; -// structA.push_back(list0); -// list list1; -// list1 += 0, 2; -// structA.push_back(list1); -// list list2; -// list2 += 1, 3; -// structA.push_back(list2); -// list list3; -// list3 += 4; -// structA.push_back(list3); -// list list4; -// list4 += 2, 3, 4; -// structA.push_back(list4); -// -// return structA; -//} -// -///** -// * Build ETree from factor graph and parent indices -// */ -//ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg, const optionalIndices& parent) { -// -// // todo: get n -// size_t n = 5; -// -// // Create tree structure -// vector trees(n); -// for (Index k = 1; k <= n; k++) { -// Index j = n - k; -// trees[j].reset(new ETree(j)); -// if (parent[j]) trees[*parent[j]]->add(trees[j]); -// } -// -// // Hang factors in right places -// BOOST_FOREACH(const ETree::sharedFactor& factor, fg) -// { -// Index j = factor->front(); -// trees[j]->add(factor); -// } -// -// return trees[n - 1]; -//} -// -///* ************************************************************************* */ -//// Test of elimination tree creation -//// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -///* ************************************************************************* */ -///** -// * Build ETree from factor graph -// */ -//ETree::shared_ptr buildETree(const SymbolicFactorGraph& fg) { -// -// // create vector of factor indices -// StructA structA = createStructA(fg); -// -// // call Gilbert01bit algorithm -// optionalIndices parent = buildETree(structA); -// -// // Build ETree from factor graph and parent indices -// return buildETree(fg, parent); -//} -// -//TEST( ETree, buildETree ) -//{ -// // create example factor graph -// SymbolicFactorGraph fg; -// fg.push_factor(0, 1); -// fg.push_factor(0, 2); -// fg.push_factor(1, 4); -// fg.push_factor(2, 4); -// fg.push_factor(3, 4); -// -// ETree::shared_ptr expected = buildHardcodedTree(fg); -// -// // Build from factor graph -// ETree::shared_ptr actual = buildETree(fg); -// -// // todo: CHECK(assert_equal(*expected,*actual)); -//} -// -///* ************************************************************************* */ -//// Test to drive elimination tree development -//// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -///* ************************************************************************* */ -// -///** -// * Eliminate factor graph -// */ -//SymbolicBayesNet eliminate(const SymbolicFactorGraph& fg) { -// -// // build etree -// ETree::shared_ptr etree = buildETree(fg); -// -// return etree->eliminate(); -//} - -//TEST( SymbolicFactorGraph, eliminate ) -//{ -// // create expected Chordal bayes Net -// IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2)); -// IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4)); -// IndexConditional::shared_ptr c2(new IndexConditional(2, 4)); -// IndexConditional::shared_ptr c3(new IndexConditional(3, 4)); -// IndexConditional::shared_ptr c4(new IndexConditional(4)); -// -// SymbolicBayesNet expected; -// expected.push_back(c3); -// expected.push_back(c0); -// expected.push_back(c1); -// expected.push_back(c2); -// expected.push_back(c4); -// -// // Create factor graph -// SymbolicFactorGraph fg; -// fg.push_factor(0, 1); -// fg.push_factor(0, 2); -// fg.push_factor(1, 4); -// fg.push_factor(2, 4); -// fg.push_factor(3, 4); -// -// // eliminate -// SymbolicBayesNet actual = eliminate(fg); -// -// CHECK(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp new file mode 100644 index 000000000..2397180bb --- /dev/null +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSymbolicSequentialSolver.cpp + * @brief Unit tests for a symbolic IndexFactor Graph + * @author Frank Dellaert + * @date Sept 16, 2012 + */ + +#include // for operator += +using namespace boost::assign; + +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Index vx2 = 0; +static const Index vx1 = 1; +static const Index vl1 = 2; + +/* ************************************************************************* */ +TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) +{ + // create factor graph + SymbolicFactorGraph g; + g.push_factor(vx2, vx1, vl1); + g.push_factor(vx1, vl1); + g.push_factor(vx1); + // test solver is Testable + SymbolicSequentialSolver solver(g); +// GTSAM_PRINT(solver); + EXPECT(assert_equal(solver,solver)); +} + +/* ************************************************************************* */ +TEST( SymbolicSequentialSolver, eliminate ) +{ + // create expected Chordal bayes Net + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(4)); + expected.push_front(boost::make_shared(3,4)); + expected.push_front(boost::make_shared(2,4)); + expected.push_front(boost::make_shared(1,2,4)); + expected.push_front(boost::make_shared(0,1,2)); + + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + SymbolicSequentialSolver solver(fg); + SymbolicBayesNet::shared_ptr actual = solver.eliminate(); + + CHECK(assert_equal(expected,*actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From de66a5cd4acb2f2b5d04094f3f032ae0f206383d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 14:35:59 +0000 Subject: [PATCH 029/107] Added unit test targets --- .cproject | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/.cproject b/.cproject index 6b473f6be..0b3a13e37 100644 --- a/.cproject +++ b/.cproject @@ -847,6 +847,22 @@ true true + + make + -j5 + testSymbolicSequentialSolver.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + make -j2 From 3f194bebffafb62e107db5f54d932c64d39d264b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 16:04:28 +0000 Subject: [PATCH 030/107] Used technique described in http://gcc.gnu.org/onlinedocs/gcc/Diagnostic-Pragmas.html to turn off excessive warnings generated by boost BFS header --- gtsam/inference/graph-inl.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index ff65147cc..48cfffd7c 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -19,7 +19,11 @@ #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" #include +#pragma GCC diagnostic pop #include #include From db57f1872ad9855c1feaa8a378b2df7eceb56ac5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 16:06:28 +0000 Subject: [PATCH 031/107] jointBayesNet function avoids conversion to factorgraph (which was converted back to a BayesNet in shortcut calculation) --- .cproject | 4 +- gtsam/inference/GenericSequentialSolver-inl.h | 71 ++++++----- gtsam/inference/GenericSequentialSolver.h | 31 +++-- gtsam/inference/SymbolicSequentialSolver.h | 13 +- .../tests/testSymbolicSequentialSolver.cpp | 117 +++++++++++------- 5 files changed, 147 insertions(+), 89 deletions(-) diff --git a/.cproject b/.cproject index 0b3a13e37..766138423 100644 --- a/.cproject +++ b/.cproject @@ -849,10 +849,10 @@ make - -j5 + -j1 testSymbolicSequentialSolver.run true - true + false true diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 368aae831..19e2d76d4 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -82,45 +82,54 @@ namespace gtsam { return eliminationTree_->eliminate(function); } - /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr // - GenericSequentialSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const { + /* ************************************************************************* */ + template + typename BayesNet::shared_ptr // + GenericSequentialSolver::jointBayesNet( + const std::vector& js, Eliminate function) const { - // Compute a COLAMD permutation with the marginal variables constrained to the end. - Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); - Permutation::shared_ptr permutationInverse(permutation->inverse()); + // Compute a COLAMD permutation with the marginal variables constrained to the end. + Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); + Permutation::shared_ptr permutationInverse(permutation->inverse()); - // Permute the factors - NOTE that this permutes the original factors, not - // copies. Other parts of the code may hold shared_ptr's to these factors so - // we must undo the permutation before returning. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutationInverse); + // Permute the factors - NOTE that this permutes the original factors, not + // copies. Other parts of the code may hold shared_ptr's to these factors so + // we must undo the permutation before returning. + BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) + if (factor) factor->permuteWithInverse(*permutationInverse); - // Eliminate all variables - typename BayesNet::shared_ptr - bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); + // Eliminate all variables + typename BayesNet::shared_ptr + bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); - // Undo the permuation on the original factors and on the structure. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutation); + // Undo the permutation on the original factors and on the structure. + BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) + if (factor) factor->permuteWithInverse(*permutation); - // Take the joint marginal from the Bayes net. - sharedFactorGraph joint(new FactorGraph ); - joint->reserve(js.size()); - typename BayesNet::const_reverse_iterator - conditional = bayesNet->rbegin(); + // Get rid of conditionals on variables that we want to marginalize out + size_t nrMarginalizedOut = bayesNet->size()-js.size(); + for(int i=0;ipop_front(); - for (size_t i = 0; i < js.size(); ++i) - joint->push_back((*(conditional++))->toFactor()); + // Undo the permutation on the conditionals + BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet) + c->permuteWithInverse(*permutation); - // Undo the permutation on the eliminated joint marginal factors - BOOST_FOREACH(const typename boost::shared_ptr& factor, *joint) - factor->permuteWithInverse(*permutation); + return bayesNet; + } - return joint; - } + /* ************************************************************************* */ + template + typename FactorGraph::shared_ptr // + GenericSequentialSolver::jointFactorGraph( + const std::vector& js, Eliminate function) const { + + // Eliminate all variables + typename BayesNet::shared_ptr + bayesNet = jointBayesNet(js,function); + + return boost::make_shared >(*bayesNet); + } /* ************************************************************************* */ template diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5b4d4b41e..5299b1afe 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -51,10 +51,8 @@ namespace gtsam { protected: typedef boost::shared_ptr > sharedFactorGraph; - - typedef std::pair< - boost::shared_ptr, - boost::shared_ptr > EliminationResult; + typedef typename FACTOR::ConditionalType Conditional; + typedef std::pair, boost::shared_ptr > EliminationResult; typedef boost::function&, size_t)> Eliminate; /** Store the original factors for computing marginals @@ -117,20 +115,29 @@ namespace gtsam { * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename boost::shared_ptr > eliminate(Eliminate function) const; + typename boost::shared_ptr > + eliminate(Eliminate function) const; - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - typename FactorGraph::shared_ptr jointFactorGraph( - const std::vector& js, Eliminate function) const; + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. Returns the result as a Bayes net + */ + typename BayesNet::shared_ptr + jointBayesNet(const std::vector& js, Eliminate function) const; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. Returns the result as a factor graph. + */ + typename FactorGraph::shared_ptr + jointFactorGraph(const std::vector& js, Eliminate function) const; /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename boost::shared_ptr marginalFactor(Index j, Eliminate function) const; + typename boost::shared_ptr + marginalFactor(Index j, Eliminate function) const; /// @} diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h index c114261f0..f023acbaf 100644 --- a/gtsam/inference/SymbolicSequentialSolver.h +++ b/gtsam/inference/SymbolicSequentialSolver.h @@ -52,13 +52,22 @@ namespace gtsam { * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - SymbolicBayesNet::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; + SymbolicBayesNet::shared_ptr eliminate() const + { return Base::eliminate(&EliminateSymbolic); }; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. Returns the result as a Bayes net. + */ + SymbolicBayesNet::shared_ptr jointBayesNet(const std::vector& js) const + { return Base::jointBayesNet(js, &EliminateSymbolic); }; /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a factor graph. */ - SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); }; + SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const + { return Base::jointFactorGraph(js, &EliminateSymbolic); }; /** * Compute the marginal Gaussian density over a variable, by integrating out diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp index 2397180bb..a1c637dcf 100644 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -11,72 +11,105 @@ /** * @file testSymbolicSequentialSolver.cpp - * @brief Unit tests for a symbolic IndexFactor Graph + * @brief Unit tests for a symbolic sequential solver routines * @author Frank Dellaert * @date Sept 16, 2012 */ -#include // for operator += -using namespace boost::assign; +#include #include -#include -#include -#include -#include -#include +#include // for operator += +using namespace boost::assign; using namespace std; using namespace gtsam; -static const Index vx2 = 0; -static const Index vx1 = 1; -static const Index vl1 = 2; - /* ************************************************************************* */ -TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) -{ - // create factor graph - SymbolicFactorGraph g; - g.push_factor(vx2, vx1, vl1); - g.push_factor(vx1, vl1); - g.push_factor(vx1); - // test solver is Testable - SymbolicSequentialSolver solver(g); + +TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) { + // create factor graph + SymbolicFactorGraph g; + g.push_factor(2, 2, 0); + g.push_factor(2, 0); + g.push_factor(2); + // test solver is Testable + SymbolicSequentialSolver solver(g); // GTSAM_PRINT(solver); - EXPECT(assert_equal(solver,solver)); + EXPECT(assert_equal(solver,solver)); } /* ************************************************************************* */ -TEST( SymbolicSequentialSolver, eliminate ) -{ - // create expected Chordal bayes Net + +TEST( SymbolicSequentialSolver, inference ) { + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + SymbolicSequentialSolver solver(fg); + SymbolicBayesNet::shared_ptr actual = solver.eliminate(); SymbolicBayesNet expected; expected.push_front(boost::make_shared(4)); - expected.push_front(boost::make_shared(3,4)); - expected.push_front(boost::make_shared(2,4)); - expected.push_front(boost::make_shared(1,2,4)); - expected.push_front(boost::make_shared(0,1,2)); + expected.push_front(boost::make_shared(3, 4)); + expected.push_front(boost::make_shared(2, 4)); + expected.push_front(boost::make_shared(1, 2, 4)); + expected.push_front(boost::make_shared(0, 1, 2)); + EXPECT(assert_equal(expected,*actual)); - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); + { + // jointBayesNet + vector js; + js.push_back(0); + js.push_back(4); + js.push_back(3); + SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(3)); + expectedBN.push_front(boost::make_shared(4, 3)); + expectedBN.push_front(boost::make_shared(0, 4)); + EXPECT( assert_equal(expectedBN,*actualBN)); - // eliminate - SymbolicSequentialSolver solver(fg); - SymbolicBayesNet::shared_ptr actual = solver.eliminate(); + // jointFactorGraph + SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); + SymbolicFactorGraph expectedFG; + expectedFG.push_factor(0, 4); + expectedFG.push_factor(4, 3); + expectedFG.push_factor(3); + EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); + } - CHECK(assert_equal(expected,*actual)); + { + // jointBayesNet + vector js; + js.push_back(0); + js.push_back(3); + js.push_back(4); + SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(3)); + expectedBN.push_front(boost::make_shared(4, 3)); + expectedBN.push_front(boost::make_shared(0, 4)); + EXPECT( assert_equal(expectedBN,*actualBN)); + + // jointFactorGraph + SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); + SymbolicFactorGraph expectedFG; + expectedFG.push_factor(0, 4); + expectedFG.push_factor(4, 3); + expectedFG.push_factor(3); + EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); + } } /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 338ea6e92082ddb6cd4273fb75baef13ece70a28 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 17:52:14 +0000 Subject: [PATCH 032/107] conditionalBayesNet and an internal eliminate - developed for making shortcuts --- gtsam/inference/GenericSequentialSolver-inl.h | 215 +++++++++++------- gtsam/inference/GenericSequentialSolver.h | 205 ++++++++++------- gtsam/inference/SymbolicSequentialSolver.h | 8 + 3 files changed, 259 insertions(+), 169 deletions(-) diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 19e2d76d4..7d99d7c52 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -28,92 +28,142 @@ namespace gtsam { - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const FactorGraph& factorGraph) : - factors_(new FactorGraph(factorGraph)), - structure_(new VariableIndex(factorGraph)), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { - } - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex) : - factors_(factorGraph), structure_(variableIndex), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::print(const std::string& s) const { - this->factors_->print(s + " factors:"); - this->structure_->print(s + " structure:\n"); - this->eliminationTree_->print(s + " etree:"); - } - - /* ************************************************************************* */ - template - bool GenericSequentialSolver::equals( - const GenericSequentialSolver& expected, double tol) const { - if (!this->factors_->equals(*expected.factors_, tol)) return false; - if (!this->structure_->equals(*expected.structure_, tol)) return false; - if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::replaceFactors( - const sharedFactorGraph& factorGraph) { - // Reset this shared pointer first to deallocate if possible - for big - // problems there may not be enough memory to store two copies. - eliminationTree_.reset(); - factors_ = factorGraph; - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr > // - GenericSequentialSolver::eliminate(Eliminate function) const { - return eliminationTree_->eliminate(function); - } + /* ************************************************************************* */ + template + GenericSequentialSolver::GenericSequentialSolver( + const FactorGraph& factorGraph) : + factors_(new FactorGraph(factorGraph)), structure_( + new VariableIndex(factorGraph)), eliminationTree_( + EliminationTree::Create(*factors_, *structure_)) { + } /* ************************************************************************* */ template - typename BayesNet::shared_ptr // - GenericSequentialSolver::jointBayesNet( - const std::vector& js, Eliminate function) const { + GenericSequentialSolver::GenericSequentialSolver( + const sharedFactorGraph& factorGraph, + const boost::shared_ptr& variableIndex) : + factors_(factorGraph), structure_(variableIndex), eliminationTree_( + EliminationTree::Create(*factors_, *structure_)) { + } - // Compute a COLAMD permutation with the marginal variables constrained to the end. - Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); - Permutation::shared_ptr permutationInverse(permutation->inverse()); + /* ************************************************************************* */ + template + void GenericSequentialSolver::print(const std::string& s) const { + this->factors_->print(s + " factors:"); + this->structure_->print(s + " structure:\n"); + this->eliminationTree_->print(s + " etree:"); + } + + /* ************************************************************************* */ + template + bool GenericSequentialSolver::equals( + const GenericSequentialSolver& expected, double tol) const { + if (!this->factors_->equals(*expected.factors_, tol)) + return false; + if (!this->structure_->equals(*expected.structure_, tol)) + return false; + if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) + return false; + return true; + } + + /* ************************************************************************* */ + template + void GenericSequentialSolver::replaceFactors( + const sharedFactorGraph& factorGraph) { + // Reset this shared pointer first to deallocate if possible - for big + // problems there may not be enough memory to store two copies. + eliminationTree_.reset(); + factors_ = factorGraph; + eliminationTree_ = EliminationTree::Create(*factors_, *structure_); + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::eliminate(Eliminate function) const { + return eliminationTree_->eliminate(function); + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::eliminate(const Permutation& permutation, + Eliminate function, boost::optional nrToEliminate) const { + + // Create inverse permutation + Permutation::shared_ptr permutationInverse(permutation.inverse()); // Permute the factors - NOTE that this permutes the original factors, not // copies. Other parts of the code may hold shared_ptr's to these factors so // we must undo the permutation before returning. BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutationInverse); + if (factor) + factor->permuteWithInverse(*permutationInverse); - // Eliminate all variables - typename BayesNet::shared_ptr - bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); + // Eliminate using elimination tree provided + typename EliminationTree::shared_ptr etree; + if (nrToEliminate) { + VariableIndex structure(*factors_, *nrToEliminate); + etree = EliminationTree::Create(*factors_, structure); + } else + etree = EliminationTree::Create(*factors_); + sharedBayesNet bayesNet = etree->eliminate(function); // Undo the permutation on the original factors and on the structure. BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutation); - - // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalizedOut = bayesNet->size()-js.size(); - for(int i=0;ipop_front(); + if (factor) + factor->permuteWithInverse(permutation); // Undo the permutation on the conditionals BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet) - c->permuteWithInverse(*permutation); + c->permuteWithInverse(permutation); + + return bayesNet; + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::conditionalBayesNet( + const std::vector& js, size_t nrFrontals, + Eliminate function) const { + + // Compute a COLAMD permutation with the marginal variables constrained to the end. + // TODO in case of nrFrontals, the order of js has to be respected here ! + Permutation::shared_ptr permutation( + inference::PermutationCOLAMD(*structure_, js)); + + // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) + size_t nrVariables = factors_->keys().size(); // TODO expensive! + size_t nrMarginalized = nrVariables - js.size(); + size_t nrToEliminate = nrMarginalized + nrFrontals; + sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); + + // Get rid of conditionals on variables that we want to marginalize out + for (int i = 0; i < nrMarginalized; i++) + bayesNet->pop_front(); + + return bayesNet; + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::jointBayesNet(const std::vector& js, + Eliminate function) const { + + // Compute a COLAMD permutation with the marginal variables constrained to the end. + Permutation::shared_ptr permutation( + inference::PermutationCOLAMD(*structure_, js)); + + // Eliminate all variables + sharedBayesNet bayesNet = eliminate(*permutation, function); + + // Get rid of conditionals on variables that we want to marginalize out + size_t nrMarginalizedOut = bayesNet->size() - js.size(); + for (int i = 0; i < nrMarginalizedOut; i++) + bayesNet->pop_front(); return bayesNet; } @@ -125,22 +175,23 @@ namespace gtsam { const std::vector& js, Eliminate function) const { // Eliminate all variables - typename BayesNet::shared_ptr - bayesNet = jointBayesNet(js,function); + typename BayesNet::shared_ptr bayesNet = jointBayesNet(js, + function); return boost::make_shared >(*bayesNet); } - /* ************************************************************************* */ - template - typename boost::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { - // Create a container for the one variable index - std::vector js(1); - js[0] = j; + /* ************************************************************************* */ + template + typename boost::shared_ptr // + GenericSequentialSolver::marginalFactor(Index j, + Eliminate function) const { + // Create a container for the one variable index + std::vector js(1); + js[0] = j; - // Call joint and return the only factor in the factor graph it returns - return (*this->jointFactorGraph(js, function))[0]; - } + // Call joint and return the only factor in the factor graph it returns + return (*this->jointFactorGraph(js, function))[0]; + } } // namespace gtsam diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5299b1afe..5b4cea73c 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -18,112 +18,141 @@ #pragma once -#include -#include -#include #include #include -namespace gtsam { class VariableIndex; } -namespace gtsam { template class EliminationTree; } -namespace gtsam { template class FactorGraph; } -namespace gtsam { template class BayesNet; } +#include +#include + +#include +#include + +namespace gtsam { + class VariableIndex; + class Permutation; +} +namespace gtsam { + template class EliminationTree; +} +namespace gtsam { + template class FactorGraph; +} +namespace gtsam { + template class BayesNet; +} namespace gtsam { - /** - * This solver implements sequential variable elimination for factor graphs. - * Underlying this is a column elimination tree, see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which examines and uses the clique structure. - * However, sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * \nosubgrouping - */ - template - class GenericSequentialSolver { + /** + * This solver implements sequential variable elimination for factor graphs. + * Underlying this is a column elimination tree, see Gilbert 2001 BIT. + * + * The elimination ordering is "baked in" to the variable indices at this + * stage, i.e. elimination proceeds in order from '0'. + * + * This is not the most efficient algorithm we provide, most efficient is the + * MultifrontalSolver, which examines and uses the clique structure. + * However, sequential variable elimination is easier to understand so this is a good + * starting point to learn about these algorithms and our implementation. + * Additionally, the first step of MFQR is symbolic sequential elimination. + * \nosubgrouping + */ + template + class GenericSequentialSolver { - protected: + protected: - typedef boost::shared_ptr > sharedFactorGraph; - typedef typename FACTOR::ConditionalType Conditional; + typedef boost::shared_ptr > sharedFactorGraph; + typedef typename FACTOR::ConditionalType Conditional; + typedef typename boost::shared_ptr > sharedBayesNet; typedef std::pair, boost::shared_ptr > EliminationResult; - typedef boost::function&, size_t)> Eliminate; + typedef boost::function< + EliminationResult(const FactorGraph&, size_t)> Eliminate; - /** Store the original factors for computing marginals - * TODO Frank says: really? Marginals should be computed from result. - */ - sharedFactorGraph factors_; + /** Store the original factors for computing marginals + * TODO Frank says: really? Marginals should be computed from result. + */ + sharedFactorGraph factors_; - /** Store column structure of the factor graph. Why? */ - boost::shared_ptr structure_; + /** Store column structure of the factor graph. Why? */ + boost::shared_ptr structure_; - /** Elimination tree that performs elimination */ - boost::shared_ptr > eliminationTree_; + /** Elimination tree that performs elimination */ + boost::shared_ptr > eliminationTree_; - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) -// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) + /** concept checks */ + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) - public: + /** + * Eliminate in a different order, given a permutation + * If given a number of variables to eliminate, will only eliminate that many + */ + sharedBayesNet + eliminate(const Permutation& permutation, Eliminate function, + boost::optional nrToEliminate = boost::none) const; - /// @name Standard Constructors - /// @{ + public: - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GenericSequentialSolver(const FactorGraph& factorGraph); + /// @name Standard Constructors + /// @{ - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex); + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the work of elimination. + */ + GenericSequentialSolver(const FactorGraph& factorGraph); - /// @} - /// @name Testable - /// @{ + /** + * Construct the solver with a shared pointer to a factor graph and to a + * VariableIndex. The solver will store these pointers, so this constructor + * is the fastest. + */ + GenericSequentialSolver(const sharedFactorGraph& factorGraph, + const boost::shared_ptr& variableIndex); - /** Print to cout */ - void print(const std::string& name = "GenericSequentialSolver: ") const; + /// @} + /// @name Testable + /// @{ - /** Test whether is equal to another */ - bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; + /** Print to cout */ + void print(const std::string& name = "GenericSequentialSolver: ") const; - /// @} - /// @name Standard Interface - /// @{ + /** Test whether is equal to another */ + bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedFactorGraph& factorGraph); + /// @} + /// @name Standard Interface + /// @{ - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - typename boost::shared_ptr > - eliminate(Eliminate function) const; + /** + * Replace the factor graph with a new one having the same structure. The + * This function can be used if the numerical part of the factors changes, + * such as during relinearization or adjusting of noise models. + */ + void replaceFactors(const sharedFactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + sharedBayesNet eliminate(Eliminate function) const; + + /** + * Compute a conditional density P(F|S) while marginalizing out variables J + * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) + * Returns the result as a Bayes net. + */ + sharedBayesNet + conditionalBayesNet(const std::vector& js, size_t nrFrontals, + Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a Bayes net */ - typename BayesNet::shared_ptr - jointBayesNet(const std::vector& js, Eliminate function) const; + sharedBayesNet + jointBayesNet(const std::vector& js, Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out @@ -132,17 +161,19 @@ namespace gtsam { typename FactorGraph::shared_ptr jointFactorGraph(const std::vector& js, Eliminate function) const; - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename boost::shared_ptr - marginalFactor(Index j, Eliminate function) const; + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + typename boost::shared_ptr + marginalFactor(Index j, Eliminate function) const; - /// @} + /// @} - }; // GenericSequentialSolver + } + ; +// GenericSequentialSolver -} // namespace gtsam +}// namespace gtsam #include diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h index f023acbaf..5bff88430 100644 --- a/gtsam/inference/SymbolicSequentialSolver.h +++ b/gtsam/inference/SymbolicSequentialSolver.h @@ -55,6 +55,14 @@ namespace gtsam { SymbolicBayesNet::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; + /** + * Compute a conditional density P(F|S) while marginalizing out variables J + * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) + * Returns the result as a Bayes net. + */ + SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector& js, size_t nrFrontals) const + { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); }; + /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a Bayes net. From 538714fb69527aaf5d91725b404f1b2cd27c047e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 17:52:49 +0000 Subject: [PATCH 033/107] avoid lambda.hpp warnings --- gtsam/linear/GaussianConditional.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index b35bffd3c..ba9628862 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -17,7 +17,10 @@ #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include #include From 89cd6001654862d94bd3721bacdcd4c697d064f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 17:54:43 +0000 Subject: [PATCH 034/107] avoid warnings --- gtsam/inference/tests/testISAM.cpp | 4 ++-- tests/testGaussianISAM.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAM.cpp index 18cf66723..79524e73e 100644 --- a/gtsam/inference/tests/testISAM.cpp +++ b/gtsam/inference/tests/testISAM.cpp @@ -34,8 +34,8 @@ typedef ISAM SymbolicISAM; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = - 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; +//static double sigmax1 = 0.786153, sigmax2 = 0.687131 ,sigmax3 = 0.671512, +//sigmax4 = 0.669534, sigmax5 = sigmax3,, sigmax7 = sigmax1, sigmax6 = sigmax2; /* ************************************************************************* */ diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 74417b57a..aaf32483e 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -41,7 +41,7 @@ using symbol_shorthand::L; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = +static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 = 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; static const double tol = 1e-4; From 439b72401127a975b6ff550d88701ea1894fd5af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 17:54:59 +0000 Subject: [PATCH 035/107] test new solver functions --- .../tests/testSymbolicSequentialSolver.cpp | 87 +++++++++++-------- 1 file changed, 51 insertions(+), 36 deletions(-) diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp index a1c637dcf..943358b79 100644 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -63,47 +63,62 @@ TEST( SymbolicSequentialSolver, inference ) { EXPECT(assert_equal(expected,*actual)); { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(4); - js.push_back(3); - SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(3)); - expectedBN.push_front(boost::make_shared(4, 3)); - expectedBN.push_front(boost::make_shared(0, 4)); - EXPECT( assert_equal(expectedBN,*actualBN)); + // jointBayesNet + vector js; + js.push_back(0); + js.push_back(4); + js.push_back(3); + SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(3)); + expectedBN.push_front(boost::make_shared(4, 3)); + expectedBN.push_front(boost::make_shared(0, 4)); + EXPECT( assert_equal(expectedBN,*actualBN)); - // jointFactorGraph - SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraph expectedFG; - expectedFG.push_factor(0, 4); - expectedFG.push_factor(4, 3); - expectedFG.push_factor(3); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); + // jointFactorGraph + SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); + SymbolicFactorGraph expectedFG; + expectedFG.push_factor(0, 4); + expectedFG.push_factor(4, 3); + expectedFG.push_factor(3); + EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); } { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(3); - js.push_back(4); - SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(3)); - expectedBN.push_front(boost::make_shared(4, 3)); - expectedBN.push_front(boost::make_shared(0, 4)); - EXPECT( assert_equal(expectedBN,*actualBN)); + // jointBayesNet + vector js; + js.push_back(0); + js.push_back(2); + js.push_back(3); + SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(2)); + expectedBN.push_front(boost::make_shared(3, 2)); + expectedBN.push_front(boost::make_shared(0, 3, 2)); + EXPECT( assert_equal(expectedBN,*actualBN)); - // jointFactorGraph - SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraph expectedFG; - expectedFG.push_factor(0, 4); - expectedFG.push_factor(4, 3); - expectedFG.push_factor(3); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); + // jointFactorGraph + SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); + SymbolicFactorGraph expectedFG; + expectedFG.push_factor(0, 3, 2); + expectedFG.push_factor(3, 2); + expectedFG.push_factor(2); + EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); + } + + { + // conditionalBayesNet + vector js; + js.push_back(0); + js.push_back(2); + js.push_back(3); + size_t nrFrontals = 2; + SymbolicBayesNet::shared_ptr actualBN = // + solver.conditionalBayesNet(js, nrFrontals); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(3, 2)); + expectedBN.push_front(boost::make_shared(0, 3, 2)); + EXPECT( assert_equal(expectedBN,*actualBN)); } } From aeb43bc8fcf3ac107a6896ae4545dcb070340bb2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 18:07:50 +0000 Subject: [PATCH 036/107] Used technique described in http://gcc.gnu.org/onlinedocs/gcc/Diagnostic-Pragmas.html to turn off excessive warnings generated by boost lambda headers --- gtsam/base/numericalDerivative.h | 3 +++ gtsam/inference/IndexConditional.cpp | 3 +++ gtsam/linear/GaussianConditional.cpp | 2 +- gtsam/linear/HessianFactor.cpp | 3 +++ gtsam/linear/JacobianFactor.cpp | 5 +++-- gtsam/nonlinear/Symbol.cpp | 5 ++++- gtsam/nonlinear/Values.cpp | 3 +++ gtsam/nonlinear/Values.h | 3 +++ tests/testDoglegOptimizer.cpp | 3 +++ wrap/Module.cpp | 3 +++ 10 files changed, 29 insertions(+), 4 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index d18db38be..86abf6067 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -20,7 +20,10 @@ #pragma once #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include #include diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 9830782f3..0c0c6c938 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -17,7 +17,10 @@ #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop namespace gtsam { diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index ba9628862..8557e3a6c 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -20,8 +20,8 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" #include -#pragma GCC diagnostic pop #include +#pragma GCC diagnostic pop #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index e152738fb..183f5fa54 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -21,7 +21,10 @@ #include #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include #include diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 759be4948..02da6b079 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -30,9 +30,10 @@ #include #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include -//#include -//#include +#pragma GCC diagnostic pop #include #include diff --git a/gtsam/nonlinear/Symbol.cpp b/gtsam/nonlinear/Symbol.cpp index 19cf78eb5..41fafd3eb 100644 --- a/gtsam/nonlinear/Symbol.cpp +++ b/gtsam/nonlinear/Symbol.cpp @@ -21,9 +21,12 @@ #include #include #include -#include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#include #include +#pragma GCC diagnostic pop #include #include diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index eab37ff17..4e22a4662 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -27,7 +27,10 @@ #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include using namespace std; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 25bd3dc2e..82e5d8c2d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -35,7 +35,10 @@ #include #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include #include diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 6b03b572b..e87835b67 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -26,7 +26,10 @@ #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include +#pragma GCC diagnostic pop #include // for 'list_of()' #include diff --git a/wrap/Module.cpp b/wrap/Module.cpp index aad533a9a..60147e23d 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -27,8 +27,11 @@ #include #include #include +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" #include #include +#pragma GCC diagnostic pop #include #include #include From dcce64ea3a9c9aab346bf3d218b9aaa3bc155e9b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 16 Sep 2012 18:08:47 +0000 Subject: [PATCH 037/107] Check in old shortcut way --- .../discrete/tests/testDiscreteBayesTree.cpp | 59 +++++++++++++------ 1 file changed, 40 insertions(+), 19 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 6109948ca..90a294893 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -110,32 +110,43 @@ public: */ FactorGraph::shared_ptr separatorShortcut(derived_ptr B) const { - FactorGraph::shared_ptr p_S_B; //shortcut P(S||B) This is empty now + typedef FactorGraph FG; + + FG::shared_ptr p_S_B; //shortcut P(S||B) This is empty now // We only calculate the shortcut when this clique is not B - derived_ptr parent(parent_.lock()); - if (B.get() != this) { + // and when the S\B is not empty + vector S_setminus_B = separatorShortcutVariables(B); + if (B.get() != this && !S_setminus_B.empty()) { // Obtain P(Fp|Sp) as a factor + derived_ptr parent(parent_.lock()); boost::shared_ptr p_Fp_Sp = parent->conditional()->toFactor(); // Obtain the parent shortcut P(Sp|B) as factors // TODO: really annoying that we eliminate more than we have to ! // TODO: we should only eliminate C_p\B, with S\B variables last // TODO: and this index dance will be easier then, as well - FactorGraph p_Sp_B(parent->shortcut(B, &EliminateDiscrete)); + FG p_Sp_B(parent->shortcut(B, &EliminateDiscrete)); // now combine P(Cp||B) = P(Fp|Sp) * P(Sp||B) - FactorGraph p_Cp_B; - p_Cp_B.push_back(p_Fp_Sp); - p_Cp_B.push_back(p_Sp_B); + boost::shared_ptr p_Cp_B(new FG); + p_Cp_B->push_back(p_Fp_Sp); + p_Cp_B->push_back(p_Sp_B); + + // Figure out how many variables there are in in the shortcut +// size_t nVariables = *max_element(S_setminus_B.begin(),S_setminus_B.end()); +// cout << "nVariables: " << nVariables << endl; +// VariableIndex::shared_ptr structure(new VariableIndex(*p_Cp_B)); +// GTSAM_PRINT(*p_Cp_B); +// GTSAM_PRINT(*structure); // Create a generic solver that will marginalize for us - GenericSequentialSolver solver(p_Cp_B); + GenericSequentialSolver solver(*p_Cp_B); // The factor graph above will have keys from the parent clique Cp and from B. // But we only keep the variables not in S union B. - vector keepers = indices(B, p_Cp_B); + vector keepers = indices(B, *p_Cp_B); p_S_B = solver.jointFactorGraph(keepers, &EliminateDiscrete); } @@ -234,7 +245,7 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { // Check whether BN and BT give the same answer on all configurations // Also calculate all some marginals Vector marginals = zero(15); - double shortcut0, sum0; + double shortcut8, shortcut0; vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); @@ -247,21 +258,31 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { for (size_t i = 0; i < 15; i++) if (x[i]) marginals[i] += actual; - // calculate a deep shortcut - if (x[12] && x[14] & x[8]) + // calculate shortcut 8 and 0 + if (x[12] && x[14]) + shortcut8 += actual; + if (x[8] && x[12] & x[14]) shortcut0 += actual; - if (x[14]) - sum0 += actual; } DiscreteFactor::Values all1 = allPosbValues.back(); - // check shortcut P(S0||R) to root +// check shortcut P(S9||R) to root Clique::shared_ptr R = bayesTree.root(); - Clique::shared_ptr c = bayesTree[0]; + Clique::shared_ptr c = bayesTree[9]; DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(shortcut0/sum0, evaluate(shortcut,all1), 1e-9); + EXPECT_LONGS_EQUAL(0, shortcut.size()); - // calculate all shortcuts to root +// check shortcut P(S8||R) to root + c = bayesTree[8]; + shortcut = c->shortcut(R, &EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(shortcut8/marginals[14], evaluate(shortcut,all1), 1e-9); + +// check shortcut P(S0||R) to root + c = bayesTree[0]; + shortcut = c->shortcut(R, &EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(shortcut0/marginals[14], evaluate(shortcut,all1), 1e-9); + +// calculate all shortcuts to root DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); BOOST_FOREACH(Clique::shared_ptr c, cliques) { DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); @@ -271,7 +292,7 @@ TEST_UNSAFE( DiscreteMarginals, thinTree ) { } } - // Check all marginals +// Check all marginals DiscreteFactor::shared_ptr marginalFactor; for (size_t i = 0; i < 15; i++) { marginalFactor = bayesTree.marginalFactor(i, &EliminateDiscrete); From d9b639ab978558ae335157788493c69cf0ede1a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:22:06 +0000 Subject: [PATCH 038/107] added newlines --- gtsam/base/Testable.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 009d286db..b6ae1b3ef 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -90,8 +90,8 @@ namespace gtsam { if (actual.equals(expected, tol)) return true; printf("Not equal:\n"); - expected.print("expected"); - actual.print("actual"); + expected.print("expected:\n"); + actual.print("actual:\n"); return false; } From 54afba0bbcebaff311a707cc103722617932b5ff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:23:42 +0000 Subject: [PATCH 039/107] Accept list in constructor --- gtsam/inference/Conditional.h | 359 +++++++++++++++++++--------------- 1 file changed, 206 insertions(+), 153 deletions(-) diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 8363c1800..fa75df83f 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -16,196 +16,249 @@ */ // \callgraph - #pragma once -#include -#include // for noncopyable -#include -#include -#include -#include #include +#include +#include + namespace gtsam { -/** - * Base class for conditional densities, templated on KEY type. This class - * provides storage for the keys involved in a conditional, and iterators and - * access to the frontal and separator keys. - * - * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer - * to the associated factor type and shared_ptr type of the derived class. See - * IndexConditional and GaussianConditional for examples. - * \nosubgrouping - */ -template -class Conditional: public gtsam::Factor { - -private: - - /** Create keys by adding key in front */ - template - static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); - std::copy(firstParent, lastParent, keys.begin() + 1); - keys[0] = key; - return keys; - } - -protected: - - /** The first nFrontal variables are frontal and the rest are parents. */ - size_t nrFrontals_; - - // Calls the base class assertInvariants, which checks for unique keys - void assertInvariants() const { Factor::assertInvariants(); } - -public: - - typedef KEY KeyType; - typedef Conditional This; - typedef Factor Base; - /** - * Typedef to the factor type that produces this conditional and that this - * conditional can be converted to using a factor constructor. Derived - * classes must redefine this. + * Base class for conditional densities, templated on KEY type. This class + * provides storage for the keys involved in a conditional, and iterators and + * access to the frontal and separator keys. + * + * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer + * to the associated factor type and shared_ptr type of the derived class. See + * IndexConditional and GaussianConditional for examples. + * \nosubgrouping */ - typedef gtsam::Factor FactorType; + template + class Conditional: public gtsam::Factor { - /** A shared_ptr to this class. Derived classes must redefine this. */ - typedef boost::shared_ptr shared_ptr; + private: - /** Iterator over keys */ - typedef typename FactorType::iterator iterator; + /** Create keys by adding key in front */ + template + static std::vector MakeKeys(KEY key, ITERATOR firstParent, + ITERATOR lastParent) { + std::vector keys((lastParent - firstParent) + 1); + std::copy(firstParent, lastParent, keys.begin() + 1); + keys[0] = key; + return keys; + } - /** Const iterator over keys */ - typedef typename FactorType::const_iterator const_iterator; + protected: - /** View of the frontal keys (call frontals()) */ - typedef boost::iterator_range Frontals; + /** The first nFrontal variables are frontal and the rest are parents. */ + size_t nrFrontals_; - /** View of the separator keys (call parents()) */ - typedef boost::iterator_range Parents; + // Calls the base class assertInvariants, which checks for unique keys + void assertInvariants() const { + Factor::assertInvariants(); + } - /// @name Standard Constructors - /// @{ + public: - /** Empty Constructor to make serialization possible */ - Conditional() : nrFrontals_(0) { assertInvariants(); } + typedef KEY KeyType; + typedef Conditional This; + typedef Factor Base; - /** No parents */ - Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); } + /** + * Typedef to the factor type that produces this conditional and that this + * conditional can be converted to using a factor constructor. Derived + * classes must redefine this. + */ + typedef gtsam::Factor FactorType; - /** Single parent */ - Conditional(KeyType key, KeyType parent) - : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } + /** A shared_ptr to this class. Derived classes must redefine this. */ + typedef boost::shared_ptr shared_ptr; - /** Two parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2) - : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } + /** Iterator over keys */ + typedef typename FactorType::iterator iterator; - /** Three parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) - : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } + /** Const iterator over keys */ + typedef typename FactorType::const_iterator const_iterator; - /// @} - /// @name Advanced Constructors - /// @{ + /** View of the frontal keys (call frontals()) */ + typedef boost::iterator_range Frontals; - /** Constructor from a frontal variable and a vector of parents */ - Conditional(KeyType key, const std::vector& parents) : - FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) { - assertInvariants(); - } + /** View of the separator keys (call parents()) */ + typedef boost::iterator_range Parents; - /** Constructor from keys and nr of frontal variables */ - Conditional(const std::vector& keys, size_t nrFrontals) : - FactorType(keys), nrFrontals_(nrFrontals) { - assertInvariants(); - } + /// @name Standard Constructors + /// @{ - /// @} - /// @name Testable - /// @{ + /** Empty Constructor to make serialization possible */ + Conditional() : + nrFrontals_(0) { + assertInvariants(); + } - /** print with optional formatter */ - void print(const std::string& s = "Conditional", const IndexFormatter& formatter = DefaultIndexFormatter) const; + /** No parents */ + Conditional(KeyType key) : + FactorType(key), nrFrontals_(1) { + assertInvariants(); + } - /** check equality */ - template - bool equals(const DERIVED& c, double tol = 1e-9) const { - return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); } + /** Single parent */ + Conditional(KeyType key, KeyType parent) : + FactorType(key, parent), nrFrontals_(1) { + assertInvariants(); + } - /// @} - /// @name Standard Interface - /// @{ + /** Two parents */ + Conditional(KeyType key, KeyType parent1, KeyType parent2) : + FactorType(key, parent1, parent2), nrFrontals_(1) { + assertInvariants(); + } - /** return the number of frontals */ - size_t nrFrontals() const { return nrFrontals_; } + /** Three parents */ + Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : + FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { + assertInvariants(); + } - /** return the number of parents */ - size_t nrParents() const { return FactorType::size() - nrFrontals_; } + /// @} + /// @name Advanced Constructors + /// @{ - /** Special accessor when there is only one frontal variable. */ - KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); } - KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); } + /** Constructor from a frontal variable and a vector of parents */ + Conditional(KeyType key, const std::vector& parents) : + FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_( + 1) { + assertInvariants(); + } - /** return a view of the frontal keys */ - Frontals frontals() const { - return boost::make_iterator_range(beginFrontals(), endFrontals()); } + /** Constructor from keys and nr of frontal variables */ + Conditional(const std::vector& keys, size_t nrFrontals) : + FactorType(keys), nrFrontals_(nrFrontals) { + assertInvariants(); + } - /** return a view of the parent keys */ - Parents parents() const { - return boost::make_iterator_range(beginParents(), endParents()); } + /** Constructor from keys and nr of frontal variables */ + Conditional(const std::list& keys, size_t nrFrontals) : + FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) { + assertInvariants(); + } - /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { return FactorType::begin(); } /// + bool equals(const DERIVED& c, double tol = 1e-9) const { + return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); + } - ///TODO: comment - boost::iterator_range frontals() { - return boost::make_iterator_range(beginFrontals(), endFrontals()); } + /// @} + /// @name Standard Interface + /// @{ - ///TODO: comment - boost::iterator_range parents() { - return boost::make_iterator_range(beginParents(), endParents()); } + /** return the number of frontals */ + size_t nrFrontals() const { + return nrFrontals_; + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + /** return the number of parents */ + size_t nrParents() const { + return FactorType::size() - nrFrontals_; + } + + /** Special accessor when there is only one frontal variable. */ + KeyType firstFrontalKey() const { + assert(nrFrontals_>0); + return FactorType::front(); + } + KeyType lastFrontalKey() const { + assert(nrFrontals_>0); + return *(endFrontals() - 1); + } + + /** return a view of the frontal keys */ + Frontals frontals() const { + return boost::make_iterator_range(beginFrontals(), endFrontals()); + } + + /** return a view of the parent keys */ + Parents parents() const { + return boost::make_iterator_range(beginParents(), endParents()); + } + + /** Iterators over frontal and parent variables. */ + const_iterator beginFrontals() const { + return FactorType::begin(); + } /// frontals() { + return boost::make_iterator_range(beginFrontals(), endFrontals()); + } + + ///TODO: comment + boost::iterator_range parents() { + return boost::make_iterator_range(beginParents(), endParents()); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + } + + /// @} + + }; + + /* ************************************************************************* */ + template + void Conditional::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << " P("; + BOOST_FOREACH(KeyType key, frontals()) + std::cout << " " << formatter(key); + if (nrParents() > 0) + std::cout << " |"; + BOOST_FOREACH(KeyType parent, parents()) + std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; } - /// @} - -}; - - -/* ************************************************************************* */ -template -void Conditional::print(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << " P("; - BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key); - if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; -} - } // gtsam From 33b772fc21ce7c4e7a56c9ed812a035e2f4d86d8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:23:56 +0000 Subject: [PATCH 040/107] Accept list in constructor --- gtsam/inference/IndexConditional.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index c1cbbdaac..941502b76 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -75,10 +75,16 @@ namespace gtsam { } /** Constructor from keys and nr of frontal variables */ - IndexConditional(const std::vector& keys, size_t nrFrontals) : - Base(keys, nrFrontals) { - assertInvariants(); - } + IndexConditional(const std::vector& keys, size_t nrFrontals) : + Base(keys, nrFrontals) { + assertInvariants(); + } + + /** Constructor from keys and nr of frontal variables */ + IndexConditional(const std::list& keys, size_t nrFrontals) : + Base(keys, nrFrontals) { + assertInvariants(); + } /// @} /// @name Standard Interface From db8264aaaebd7a87880866762761b11845a13a98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:24:49 +0000 Subject: [PATCH 041/107] forceOrder flag respects order of constrained variables passed to ccolamd --- gtsam/inference/inference-inl.h | 25 +++++++++++++++++-------- gtsam/inference/inference.h | 3 ++- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h index c17f947d1..81415ffd9 100644 --- a/gtsam/inference/inference-inl.h +++ b/gtsam/inference/inference-inl.h @@ -32,31 +32,39 @@ namespace inference { /* ************************************************************************* */ template Permutation::shared_ptr PermutationCOLAMD( - const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { + const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) { - std::vector cmember(variableIndex.size(), 0); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. - if(constrainLast.size() < variableIndex.size()) { + if(constrainLast.size() < n) { BOOST_FOREACH(Index var, constrainLast) { - assert(var < variableIndex.size()); + assert(var < n); cmember[var] = 1; } } - return PermutationCOLAMD_(variableIndex, cmember); + Permutation::shared_ptr permutation = PermutationCOLAMD_(variableIndex, cmember); + if (forceOrder) { + Index j=n; + BOOST_REVERSE_FOREACH(Index c, constrainLast) + permutation->operator[](--j) = c; + } + return permutation; } /* ************************************************************************* */ template Permutation::shared_ptr PermutationCOLAMDGrouped( const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) { - std::vector cmember(variableIndex.size(), 0); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); typedef typename CONSTRAINED_MAP::value_type constraint_pair; BOOST_FOREACH(const constraint_pair& p, constraints) { - assert(p.first < variableIndex.size()); + assert(p.first < n); // FIXME: check that no groups are skipped cmember[p.first] = p.second; } @@ -66,7 +74,8 @@ Permutation::shared_ptr PermutationCOLAMDGrouped( /* ************************************************************************* */ inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) { - std::vector cmember(variableIndex.size(), 0); + size_t n = variableIndex.size(); + std::vector cmember(n, 0); return PermutationCOLAMD_(variableIndex, cmember); } diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index 7ba268789..eea1c4d2e 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -43,10 +43,11 @@ namespace gtsam { * @param variableIndex is the variable index lookup from a graph * @param constrainlast is a vector of keys that should be constrained * @tparam constrainLast is a std::vector (or similar structure) + * @param forceOrder if true, will not allow re-ordering of constrained variables */ template Permutation::shared_ptr PermutationCOLAMD( - const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); + const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false); /** * Compute a permutation of variable ordering using constrained colamd to From bd8f9d0006d21ac680c52519687e2778fb8a6583 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:26:07 +0000 Subject: [PATCH 042/107] Went back to eliminate all in conditional because of singularities :-(, added use of forceOrder flag --- gtsam/inference/GenericSequentialSolver-inl.h | 41 +++++++++++++++---- gtsam/inference/GenericSequentialSolver.h | 10 +++-- 2 files changed, 40 insertions(+), 11 deletions(-) diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 7d99d7c52..1e48106e5 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,7 +89,11 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function, boost::optional nrToEliminate) const { + Eliminate function +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + , boost::optional nrToEliminate +#endif + ) const { // Create inverse permutation Permutation::shared_ptr permutationInverse(permutation.inverse()); @@ -103,11 +107,13 @@ namespace gtsam { // Eliminate using elimination tree provided typename EliminationTree::shared_ptr etree; +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL if (nrToEliminate) { VariableIndex structure(*factors_, *nrToEliminate); etree = EliminationTree::Create(*factors_, structure); } else - etree = EliminationTree::Create(*factors_); +#endif + etree = EliminationTree::Create(*factors_); sharedBayesNet bayesNet = etree->eliminate(function); // Undo the permutation on the original factors and on the structure. @@ -132,17 +138,36 @@ namespace gtsam { // Compute a COLAMD permutation with the marginal variables constrained to the end. // TODO in case of nrFrontals, the order of js has to be respected here ! Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js)); + inference::PermutationCOLAMD(*structure_, js, true)); +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + // TODO Frank says: this was my attempt at eliminating exactly + // as many variables as we need. Unfortunately, in some cases + // (see testSymbolicSequentialSolver::problematicConditional) + // my trick below (passing nrToEliminate to eliminate) sometimes leads + // to a disconnected graph. // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = factors_->keys().size(); // TODO expensive! + size_t nrVariables = factors_->keys().size();// TODO expensive! size_t nrMarginalized = nrVariables - js.size(); size_t nrToEliminate = nrMarginalized + nrFrontals; sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); - // Get rid of conditionals on variables that we want to marginalize out for (int i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); + bayesNet->pop_front(); +#else + // Eliminate all variables + sharedBayesNet fullBayesNet = eliminate(*permutation, function); + + // Get rid of conditionals we do not need (front and back) + size_t nrMarginalized = fullBayesNet->size() - js.size(); + sharedBayesNet bayesNet(new BayesNet()); + size_t i = 1; + BOOST_FOREACH(sharedConditional c, *fullBayesNet) { + if (i > nrMarginalized && i - nrMarginalized <= nrFrontals) + bayesNet->push_back(c); + i += 1; + } +#endif return bayesNet; } @@ -161,8 +186,8 @@ namespace gtsam { sharedBayesNet bayesNet = eliminate(*permutation, function); // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalizedOut = bayesNet->size() - js.size(); - for (int i = 0; i < nrMarginalizedOut; i++) + size_t nrMarginalized = bayesNet->size() - js.size(); + for (int i = 0; i < nrMarginalized; i++) bayesNet->pop_front(); return bayesNet; diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5b4cea73c..6f0bf3b5b 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -64,6 +64,7 @@ namespace gtsam { typedef boost::shared_ptr > sharedFactorGraph; typedef typename FACTOR::ConditionalType Conditional; + typedef typename boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr > sharedBayesNet; typedef std::pair, boost::shared_ptr > EliminationResult; typedef boost::function< @@ -86,11 +87,14 @@ namespace gtsam { /** * Eliminate in a different order, given a permutation - * If given a number of variables to eliminate, will only eliminate that many */ sharedBayesNet - eliminate(const Permutation& permutation, Eliminate function, - boost::optional nrToEliminate = boost::none) const; + eliminate(const Permutation& permutation, Eliminate function +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + , boost::optional nrToEliminate = boost::none + // If given a number of variables to eliminate, will only eliminate that many +#endif + ) const; public: From 34a9000134ba373906012348f8225f74f338ac9a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:28:27 +0000 Subject: [PATCH 043/107] forceOrder flag respects order of constrained variables passed to conditionalBayesNet --- .../tests/testSymbolicSequentialSolver.cpp | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp index 943358b79..ef6f415e1 100644 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -31,7 +31,7 @@ using namespace gtsam; TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) { // create factor graph SymbolicFactorGraph g; - g.push_factor(2, 2, 0); + g.push_factor(2, 1, 0); g.push_factor(2, 0); g.push_factor(2); // test solver is Testable @@ -116,12 +116,33 @@ TEST( SymbolicSequentialSolver, inference ) { SymbolicBayesNet::shared_ptr actualBN = // solver.conditionalBayesNet(js, nrFrontals); SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(3, 2)); - expectedBN.push_front(boost::make_shared(0, 3, 2)); + expectedBN.push_front(boost::make_shared(2, 3)); + expectedBN.push_front(boost::make_shared(0, 2, 3)); EXPECT( assert_equal(expectedBN,*actualBN)); } } +/* ************************************************************************* */ +// This test shows a problem with my (Frank) attempt at a faster conditionalBayesNet +TEST( SymbolicSequentialSolver, problematicConditional ) { + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(9, 12, 14); + + // eliminate + SymbolicSequentialSolver solver(fg); + // conditionalBayesNet + vector js; + js.push_back(12); + js.push_back(14); + size_t nrFrontals = 1; + SymbolicBayesNet::shared_ptr actualBN = // + solver.conditionalBayesNet(js, nrFrontals); + SymbolicBayesNet expectedBN; + expectedBN.push_front(boost::make_shared(12,14)); + EXPECT( assert_equal(expectedBN,*actualBN)); +} + /* ************************************************************************* */ int main() { TestResult tr; From cdf45105c26968cbec3854497422bc43f852ab43 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:29:03 +0000 Subject: [PATCH 044/107] Fixed shortcuts after adding several more problematic testcases --- .cproject | 8 + .../discrete/tests/testDiscreteBayesTree.cpp | 118 +------- gtsam/inference/BayesTreeCliqueBase-inl.h | 257 ++++++++-------- gtsam/inference/BayesTreeCliqueBase.h | 152 +++++++--- .../inference/tests/testSymbolicBayesTree.cpp | 278 ++++++++++++++++++ 5 files changed, 519 insertions(+), 294 deletions(-) create mode 100644 gtsam/inference/tests/testSymbolicBayesTree.cpp diff --git a/.cproject b/.cproject index 766138423..52375afab 100644 --- a/.cproject +++ b/.cproject @@ -863,6 +863,14 @@ true true + + make + -j1 + testSymbolicBayesTree.run + true + false + true + make -j2 diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 90a294893..8ff50ad3c 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -36,40 +36,6 @@ class Clique: public BayesTreeCliqueBase { protected: - /// Calculate set S\B - vector separatorShortcutVariables(derived_ptr B) const { - sharedConditional p_F_S = this->conditional(); - vector &indicesB = B->conditional()->keys(); - vector S_setminus_B; - set_difference(p_F_S->beginParents(), p_F_S->endParents(), // - indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); - return S_setminus_B; - } - - /** - * Determine variable indices to keep in recursive separator shortcut calculation - * The factor graph p_Cp_B has keys from the parent clique Cp and from B. - * But we only keep the variables not in S union B. - */ - vector indices(derived_ptr B, - const FactorGraph& p_Cp_B) const { - - // We do this by first merging S and B - sharedConditional p_F_S = this->conditional(); - vector &indicesB = B->conditional()->keys(); - vector S_union_B; - set_union(p_F_S->beginParents(), p_F_S->endParents(), // - indicesB.begin(), indicesB.end(), back_inserter(S_union_B)); - - // then intersecting S_union_B with all keys in p_Cp_B - set allKeys = p_Cp_B.keys(); - vector keepers; - set_intersection(S_union_B.begin(), S_union_B.end(), // - allKeys.begin(), allKeys.end(), back_inserter(keepers)); - - return keepers; - } - public: typedef BayesTreeCliqueBase Base; @@ -102,88 +68,6 @@ public: return result; } - /** - * Separator shortcut function P(S||B) = P(S\B|B) - * where S is a clique separator, and B any node (e.g., a brancing in the tree) - * We can compute it recursively from the parent shortcut - * P(Sp||B) as \int P(Fp|Sp) P(Sp||B), where Fp are the frontal nodes in p - */ - FactorGraph::shared_ptr separatorShortcut(derived_ptr B) const { - - typedef FactorGraph FG; - - FG::shared_ptr p_S_B; //shortcut P(S||B) This is empty now - - // We only calculate the shortcut when this clique is not B - // and when the S\B is not empty - vector S_setminus_B = separatorShortcutVariables(B); - if (B.get() != this && !S_setminus_B.empty()) { - - // Obtain P(Fp|Sp) as a factor - derived_ptr parent(parent_.lock()); - boost::shared_ptr p_Fp_Sp = parent->conditional()->toFactor(); - - // Obtain the parent shortcut P(Sp|B) as factors - // TODO: really annoying that we eliminate more than we have to ! - // TODO: we should only eliminate C_p\B, with S\B variables last - // TODO: and this index dance will be easier then, as well - FG p_Sp_B(parent->shortcut(B, &EliminateDiscrete)); - - // now combine P(Cp||B) = P(Fp|Sp) * P(Sp||B) - boost::shared_ptr p_Cp_B(new FG); - p_Cp_B->push_back(p_Fp_Sp); - p_Cp_B->push_back(p_Sp_B); - - // Figure out how many variables there are in in the shortcut -// size_t nVariables = *max_element(S_setminus_B.begin(),S_setminus_B.end()); -// cout << "nVariables: " << nVariables << endl; -// VariableIndex::shared_ptr structure(new VariableIndex(*p_Cp_B)); -// GTSAM_PRINT(*p_Cp_B); -// GTSAM_PRINT(*structure); - - // Create a generic solver that will marginalize for us - GenericSequentialSolver solver(*p_Cp_B); - - // The factor graph above will have keys from the parent clique Cp and from B. - // But we only keep the variables not in S union B. - vector keepers = indices(B, *p_Cp_B); - - p_S_B = solver.jointFactorGraph(keepers, &EliminateDiscrete); - } - // return the shortcut P(S||B) - return p_S_B; - } - - /** - * The shortcut density is a conditional P(S||B) of the separator of this - * clique on the clique B. - */ - BayesNet shortcut(derived_ptr B, - Eliminate function) const { - - //Check if the ShortCut already exists - if (cachedShortcut_) { - return *cachedShortcut_; // return the cached version - } else { - BayesNet bn; - FactorGraph::shared_ptr fg = separatorShortcut(B); - if (fg) { - // calculate set S\B of indices to keep in Bayes net - vector S_setminus_B = separatorShortcutVariables(B); - set keep(S_setminus_B.begin(), S_setminus_B.end()); - - BOOST_FOREACH (FactorType::shared_ptr factor,*fg) { - DecisionTreeFactor::shared_ptr df = boost::dynamic_pointer_cast< - DecisionTreeFactor>(factor); - if (keep.count(*factor->begin())) - bn.push_front(boost::make_shared(1, *df)); - } - } - cachedShortcut_ = bn; - return bn; - } - } - }; typedef BayesTree DiscreteBayesTree; @@ -196,7 +80,7 @@ double evaluate(const DiscreteBayesTree& tree, /* ************************************************************************* */ -TEST_UNSAFE( DiscreteMarginals, thinTree ) { +TEST_UNSAFE( DiscreteBayesTree, thinTree ) { const int nrNodes = 15; const size_t nrStates = 2; diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 7ca40345b..5ac87b6c3 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -22,7 +22,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTreeCliqueBase::assertInvariants() const { + void BayesTreeCliqueBase::assertInvariants() const { #ifndef NDEBUG // We rely on the keys being sorted // FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); @@ -35,27 +35,71 @@ namespace gtsam { /* ************************************************************************* */ template - BayesTreeCliqueBase::BayesTreeCliqueBase(const sharedConditional& conditional) : - conditional_(conditional) { + std::vector BayesTreeCliqueBase::separator_setminus_B( + derived_ptr B) const { + sharedConditional p_F_S = this->conditional(); + std::vector &indicesB = B->conditional()->keys(); + std::vector S_setminus_B; + std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), // + indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); + return S_setminus_B; + } + + /* ************************************************************************* */ + template + std::vector BayesTreeCliqueBase::shortcut_indices( + derived_ptr B, const FactorGraph& p_Cp_B) const { + std::set allKeys = p_Cp_B.keys(); + std::vector &indicesB = B->conditional()->keys(); + std::vector keep; +#ifdef OLD_INDICES + // We do this by first merging S and B + sharedConditional p_F_S = this->conditional(); + std::vector S_union_B; + std::set_union(p_F_S->beginParents(), p_F_S->endParents(),// + indicesB.begin(), indicesB.end(), back_inserter(S_union_B)); + + // then intersecting S_union_B with all keys in p_Cp_B + std::set_intersection(S_union_B.begin(), S_union_B.end(),// + allKeys.begin(), allKeys.end(), back_inserter(keep)); +#else + std::vector S_setminus_B = separator_setminus_B(B); // TODO, get as argument? + std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + std::set_intersection(indicesB.begin(), indicesB.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); +#endif + // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl; + return keep; + } + + /* ************************************************************************* */ + template + BayesTreeCliqueBase::BayesTreeCliqueBase( + const sharedConditional& conditional) : + conditional_(conditional) { assertInvariants(); } /* ************************************************************************* */ template - BayesTreeCliqueBase::BayesTreeCliqueBase(const std::pair >& result) : - conditional_(result.first) { + BayesTreeCliqueBase::BayesTreeCliqueBase( + const std::pair >& result) : + conditional_(result.first) { assertInvariants(); } /* ************************************************************************* */ template - void BayesTreeCliqueBase::print(const std::string& s, const IndexFormatter& indexFormatter) const { + void BayesTreeCliqueBase::print(const std::string& s, + const IndexFormatter& indexFormatter) const { conditional_->print(s, indexFormatter); } /* ************************************************************************* */ template - size_t BayesTreeCliqueBase::treeSize() const { + size_t BayesTreeCliqueBase::treeSize() const { size_t size = 1; BOOST_FOREACH(const derived_ptr& child, children_) size += child->treeSize(); @@ -64,15 +108,17 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTreeCliqueBase::printTree(const std::string& indent, const IndexFormatter& indexFormatter) const { + void BayesTreeCliqueBase::printTree( + const std::string& indent, const IndexFormatter& indexFormatter) const { asDerived(this)->print(indent, indexFormatter); BOOST_FOREACH(const derived_ptr& child, children_) - child->printTree(indent+" ", indexFormatter); + child->printTree(indent + " ", indexFormatter); } /* ************************************************************************* */ template - void BayesTreeCliqueBase::permuteWithInverse(const Permutation& inversePermutation) { + void BayesTreeCliqueBase::permuteWithInverse( + const Permutation& inversePermutation) { conditional_->permuteWithInverse(inversePermutation); BOOST_FOREACH(const derived_ptr& child, children_) { child->permuteWithInverse(inversePermutation); @@ -82,19 +128,21 @@ namespace gtsam { /* ************************************************************************* */ template - bool BayesTreeCliqueBase::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); + bool BayesTreeCliqueBase::permuteSeparatorWithInverse( + const Permutation& inversePermutation) { + bool changed = conditional_->permuteSeparatorWithInverse( + inversePermutation); #ifndef NDEBUG if(!changed) { - BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } + BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);} BOOST_FOREACH(const derived_ptr& child, children_) { assert(child->permuteSeparatorWithInverse(inversePermutation) == false); } } #endif - if(changed) { + if (changed) { BOOST_FOREACH(const derived_ptr& child, children_) { - (void)child->permuteSeparatorWithInverse(inversePermutation); + (void) child->permuteSeparatorWithInverse(inversePermutation); } } assertInvariants(); @@ -108,105 +156,47 @@ namespace gtsam { /* ************************************************************************* */ template BayesNet BayesTreeCliqueBase::shortcut( - derived_ptr R, Eliminate function) const{ + derived_ptr B, Eliminate function) const { - static const bool debug = false; + // Check if the ShortCut already exists + if (!cachedShortcut_) { - BayesNet p_S_R; //shortcut P(S|R) This is empty now + // We only calculate the shortcut when this clique is not B + // and when the S\B is not empty + std::vector S_setminus_B = separator_setminus_B(B); + if (B.get() != this && !S_setminus_B.empty()) { - //Check if the ShortCut already exists - if(!cachedShortcut_){ + // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph + derived_ptr parent(parent_.lock()); + FactorGraph p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) + p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - // A first base case is when this clique or its parent is the root, - // in which case we return an empty Bayes net. + // Add the root conditional + // TODO: this is needed because otherwise we will be solving singular + // systems and exceptions are thrown. However, we should be able to omit + // this if we can get ATTEMPT_AT_NOT_ELIMINATING_ALL in + // GenericSequentialSolver.* working... + p_Cp_B.push_back(B->conditional()->toFactor()); // P(B) - derived_ptr parent(parent_.lock()); - if (R.get() != this && parent != R) { + // Create solver that will marginalize for us + GenericSequentialSolver solver(p_Cp_B); - // The root conditional - FactorGraph p_R(BayesNet(R->conditional())); + // Determine the variables we want to keep + std::vector keep = shortcut_indices(B, p_Cp_B); - // The parent clique has a ConditionalType for each frontal node in Fp - // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); + // Finally, we only want to have S\B variables in the Bayes net, so + size_t nrFrontals = S_setminus_B.size(); + cachedShortcut_ = // + *solver.conditionalBayesNet(keep, nrFrontals, function); + assertInvariants(); + } else { + BayesNet empty; + cachedShortcut_ = empty; + } + } - // If not the base case, obtain the parent shortcut P(Sp|R) as factors - FactorGraph p_Sp_R(parent->shortcut(R, function)); - - // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FactorGraph p_Cp_R; - p_Cp_R.push_back(p_R); - p_Cp_R.push_back(p_Fp_Sp); - p_Cp_R.push_back(p_Sp_R); - - // Eliminate into a Bayes net with ordering designed to integrate out - // any variables not in *our* separator. Variables to integrate out must be - // eliminated first hence the desired ordering is [Cp\S S]. - // However, an added wrinkle is that Cp might overlap with the root. - // Keys corresponding to the root should not be added to the ordering at all. - - if(debug) { - p_R.print("p_R: "); - p_Fp_Sp.print("p_Fp_Sp: "); - p_Sp_R.print("p_Sp_R: "); - } - - // We want to factor into a conditional of the clique variables given the - // root and the marginal on the root, integrating out all other variables. - // The integrands include any parents of this clique and the variables of - // the parent clique. - FastSet variablesAtBack; - FastSet separator; - size_t uniqueRootVariables = 0; - BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { - variablesAtBack.insert(separatorIndex); - separator.insert(separatorIndex); - if(debug) std::cout << "At back (this): " << separatorIndex << std::endl; - } - BOOST_FOREACH(const Index key, R->conditional()->keys()) { - if(variablesAtBack.insert(key).second) - ++ uniqueRootVariables; - if(debug) std::cout << "At back (root): " << key << std::endl; - } - - Permutation toBack = Permutation::PushToBack( - std::vector(variablesAtBack.begin(), variablesAtBack.end()), - R->conditional()->lastFrontalKey() + 1); - Permutation::shared_ptr toBackInverse(toBack.inverse()); - BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { - factor->permuteWithInverse(*toBackInverse); } - typename BayesNet::shared_ptr eliminated(EliminationTree< - FactorType>::Create(p_Cp_R)->eliminate(function)); - - // Take only the conditionals for p(S|R). We check for each variable being - // in the separator set because if some separator variables overlap with - // root variables, we cannot rely on the number of root variables, and also - // want to include those variables in the conditional. - BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) { - assert(conditional->nrFrontals() == 1); - if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { - if(debug) - conditional->print("Taking C|R conditional: "); - p_S_R.push_front(conditional); - } - if(p_S_R.size() == separator.size()) - break; - } - - // Undo the permutation - if(debug) toBack.print("toBack: "); - p_S_R.permuteWithInverse(toBack); - } - - cachedShortcut_ = p_S_R; - } - else - p_S_R = *cachedShortcut_; // return the cached version - - assertInvariants(); - - // return the shortcut P(S|R) - return p_S_R; + // return the shortcut P(S||B) + return *cachedShortcut_; // return the cached version } /* ************************************************************************* */ @@ -216,12 +206,13 @@ namespace gtsam { // Because the root clique could be very big. /* ************************************************************************* */ template - FactorGraph::FactorType> BayesTreeCliqueBase::marginal( - derived_ptr R, Eliminate function) const{ + FactorGraph::FactorType> BayesTreeCliqueBase< + DERIVED, CONDITIONAL>::marginal(derived_ptr R, Eliminate function) const { // If we are the root, just return this root // NOTE: immediately cast to a factor graph BayesNet bn(R->conditional()); - if (R.get()==this) return bn; + if (R.get() == this) + return bn; // Combine P(F|S), P(S|R), and P(R) BayesNet p_FSR = this->shortcut(R, function); @@ -237,16 +228,21 @@ namespace gtsam { // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) /* ************************************************************************* */ template - FactorGraph::FactorType> BayesTreeCliqueBase::joint( - derived_ptr C2, derived_ptr R, Eliminate function) const { + FactorGraph::FactorType> BayesTreeCliqueBase< + DERIVED, CONDITIONAL>::joint(derived_ptr C2, derived_ptr R, + Eliminate function) const { // For now, assume neither is the root // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) FactorGraph joint; - if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1) - if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) - if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) - if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) + if (!isRoot()) + joint.push_back(this->conditional()->toFactor()); // P(F1|S1) + if (!isRoot()) + joint.push_back(shortcut(R, function)); // P(S1|R) + if (!C2->isRoot()) + joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) + if (!C2->isRoot()) + joint.push_back(C2->shortcut(R, function)); // P(S2|R) joint.push_back(R->conditional()->toFactor()); // P(R) // Find the keys of both C1 and C2 @@ -257,29 +253,30 @@ namespace gtsam { keys12.insert(keys2.begin(), keys2.end()); // Calculate the marginal - std::vector keys12vector; keys12vector.reserve(keys12.size()); + std::vector keys12vector; + keys12vector.reserve(keys12.size()); keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); assertInvariants(); GenericSequentialSolver solver(joint); return *solver.jointFactorGraph(keys12vector, function); } - /* ************************************************************************* */ - template - void BayesTreeCliqueBase::deleteCachedShorcuts() { + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::deleteCachedShorcuts() { - // When a shortcut is requested, all of the shortcuts between it and the - // root are also generated. So, if this clique's cached shortcut is set, - // recursively call over all child cliques. Otherwise, it is unnecessary. - if(cachedShortcut_) { - BOOST_FOREACH(derived_ptr& child, children_) { - child->deleteCachedShorcuts(); - } + // When a shortcut is requested, all of the shortcuts between it and the + // root are also generated. So, if this clique's cached shortcut is set, + // recursively call over all child cliques. Otherwise, it is unnecessary. + if (cachedShortcut_) { + BOOST_FOREACH(derived_ptr& child, children_) { + child->deleteCachedShorcuts(); + } - //Delete CachedShortcut for this clique - this->resetCachedShortcut(); - } + //Delete CachedShortcut for this clique + this->resetCachedShortcut(); + } - } + } } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index a2fb8feef..ceeef3f45 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -25,7 +25,9 @@ #include #include -namespace gtsam { template class BayesTree; } +namespace gtsam { + template class BayesTree; +} namespace gtsam { @@ -48,7 +50,7 @@ namespace gtsam { struct BayesTreeCliqueBase { public: - typedef BayesTreeCliqueBase This; + typedef BayesTreeCliqueBase This; typedef DERIVED DerivedType; typedef CONDITIONAL ConditionalType; typedef boost::shared_ptr sharedConditional; @@ -61,19 +63,22 @@ namespace gtsam { protected: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ /** Default constructor */ - BayesTreeCliqueBase() {} + BayesTreeCliqueBase() { + } /** Construct from a conditional, leaving parent and child pointers uninitialized */ BayesTreeCliqueBase(const sharedConditional& conditional); /** Construct from an elimination result, which is a pair */ - BayesTreeCliqueBase(const std::pair >& result); + BayesTreeCliqueBase( + const std::pair >& result); - /// @} + /// @} /// This stores the Cached Shortcut value mutable boost::optional > cachedShortcut_; @@ -83,67 +88,91 @@ namespace gtsam { derived_weak_ptr parent_; std::list children_; - /// @name Testable - /// @{ + /// @name Testable + /// @{ /** check equality */ - bool equals(const This& other, double tol=1e-9) const { - return (!conditional_ && !other.conditional()) || - conditional_->equals(*other.conditional(), tol); + bool equals(const This& other, double tol = 1e-9) const { + return (!conditional_ && !other.conditional()) + || conditional_->equals(*other.conditional(), tol); } /** print this node */ - void print(const std::string& s = "", const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; + void print(const std::string& s = "", const IndexFormatter& indexFormatter = + DefaultIndexFormatter) const; /** print this node and entire subtree below it */ - void printTree(const std::string& indent="", const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; + void printTree(const std::string& indent = "", + const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ /** Access the conditional */ - const sharedConditional& conditional() const { return conditional_; } + const sharedConditional& conditional() const { + return conditional_; + } /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { return parent_.expired(); } + inline bool isRoot() const { + return parent_.expired(); + } /** The size of subtree rooted at this clique, i.e., nr of Cliques */ size_t treeSize() const; /** The arrow operator accesses the conditional */ - const ConditionalType* operator->() const { return conditional_.get(); } + const ConditionalType* operator->() const { + return conditional_.get(); + } /** return the const reference of children */ - const std::list& children() const { return children_; } + const std::list& children() const { + return children_; + } /** return a shared_ptr to the parent clique */ - derived_ptr parent() const { return parent_.lock(); } + derived_ptr parent() const { + return parent_.lock(); + } - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ /** The arrow operator accesses the conditional */ - ConditionalType* operator->() { return conditional_.get(); } + ConditionalType* operator->() { + return conditional_.get(); + } /** return the reference of children non-const version*/ - std::list& children() { return children_; } + std::list& children() { + return children_; + } /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ - static derived_ptr Create(const sharedConditional& conditional) { return boost::make_shared(conditional); } + static derived_ptr Create(const sharedConditional& conditional) { + return boost::make_shared(conditional); + } /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class * the conditional part is kept and the factor part is ignored, but in derived clique * types, such as ISAM2Clique, the factor part is kept as a cached factor. * @param result An elimination result, which is a pair */ - static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } + static derived_ptr Create( + const std::pair >& result) { + return boost::make_shared(result); + } - /** Returns a new clique containing a copy of the conditional but without - * the parent and child clique pointers. - */ - derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); } + /** Returns a new clique containing a copy of the conditional but without + * the parent and child clique pointers. + */ + derived_ptr clone() const { + return Create(sharedConditional(new ConditionalType(*conditional_))); + } /** Permute the variables in the whole subtree rooted at this clique */ void permuteWithInverse(const Permutation& inversePermutation); @@ -156,13 +185,16 @@ namespace gtsam { bool permuteSeparatorWithInverse(const Permutation& inversePermutation); /** return the conditional P(S|Root) on the separator given the root */ - BayesNet shortcut(derived_ptr root, Eliminate function) const; + BayesNet shortcut(derived_ptr root, + Eliminate function) const; /** return the marginal P(C) of the clique */ - FactorGraph marginal(derived_ptr root, Eliminate function) const; + FactorGraph marginal(derived_ptr root, + Eliminate function) const; /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; + FactorGraph joint(derived_ptr C2, derived_ptr root, + Eliminate function) const; /** * This deletes the cached shortcuts of all cliques (subtree) below this clique. @@ -171,29 +203,53 @@ namespace gtsam { void deleteCachedShorcuts(); /** return cached shortcut of the clique */ - const boost::optional > cachedShortcut() const { return cachedShortcut_; } + const boost::optional > cachedShortcut() const { + return cachedShortcut_; + } - friend class BayesTree; + friend class BayesTree ; protected: - ///TODO: comment + /// assert invariants that have to hold in a clique void assertInvariants() const; + /// Calculate set \f$ S \setminus B \f$ for shortcut calculations + std::vector separator_setminus_B(derived_ptr B) const; + + /// Calculate set \f$ S_p \cap B \f$ for shortcut calculations + std::vector parent_separator_intersection_B(derived_ptr B) const; + + /** + * Determine variable indices to keep in recursive separator shortcut calculation + * The factor graph p_Cp_B has keys from the parent clique Cp and from B. + * But we only keep the variables not in S union B. + */ + std::vector shortcut_indices(derived_ptr B, + const FactorGraph& p_Cp_B) const; + /// Reset the computed shortcut of this clique. Used by friend BayesTree - void resetCachedShortcut() { cachedShortcut_ = boost::none; } + void resetCachedShortcut() { + cachedShortcut_ = boost::none; + } private: - /** Cliques cannot be copied except by the clone() method, which does not + /** + * Cliques cannot be copied except by the clone() method, which does not * copy the parent and child pointers. */ - BayesTreeCliqueBase(const This& other) { assert(false); } + BayesTreeCliqueBase(const This& other) { + assert(false); + } /** Cliques cannot be copied except by the clone() method, which does not * copy the parent and child pointers. */ - This& operator=(const This& other) { assert(false); return *this; } + This& operator=(const This& other) { + assert(false); + return *this; + } /** Serialization function */ friend class boost::serialization::access; @@ -204,17 +260,19 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(children_); } - /// @} + /// @} - }; // \struct Clique + }; + // \struct Clique template - const DERIVED* asDerived(const BayesTreeCliqueBase* base) { + const DERIVED* asDerived( + const BayesTreeCliqueBase* base) { return static_cast(base); } template - DERIVED* asDerived(BayesTreeCliqueBase* base) { + DERIVED* asDerived(BayesTreeCliqueBase* base) { return static_cast(base); } diff --git a/gtsam/inference/tests/testSymbolicBayesTree.cpp b/gtsam/inference/tests/testSymbolicBayesTree.cpp new file mode 100644 index 000000000..5e00e23b1 --- /dev/null +++ b/gtsam/inference/tests/testSymbolicBayesTree.cpp @@ -0,0 +1,278 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testSymbolicBayesTree.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include + +#include +#include +using namespace boost::assign; + +#include + +using namespace std; +using namespace gtsam; + +static bool debug = false; + +typedef BayesNet SymbolicBayesNet; +typedef BayesTree SymbolicBayesTree; + +/* ************************************************************************* */ + +TEST_UNSAFE( SymbolicBayesTree, thinTree ) { + + // create a thin-tree Bayesnet, a la Jean-Guillaume + SymbolicBayesNet bayesNet; + bayesNet.push_front(boost::make_shared(14)); + + bayesNet.push_front(boost::make_shared(13, 14)); + bayesNet.push_front(boost::make_shared(12, 14)); + + bayesNet.push_front(boost::make_shared(11, 13, 14)); + bayesNet.push_front(boost::make_shared(10, 13, 14)); + bayesNet.push_front(boost::make_shared(9, 12, 14)); + bayesNet.push_front(boost::make_shared(8, 12, 14)); + + bayesNet.push_front(boost::make_shared(7, 11, 13)); + bayesNet.push_front(boost::make_shared(6, 11, 13)); + bayesNet.push_front(boost::make_shared(5, 10, 13)); + bayesNet.push_front(boost::make_shared(4, 10, 13)); + + bayesNet.push_front(boost::make_shared(3, 9, 12)); + bayesNet.push_front(boost::make_shared(2, 9, 12)); + bayesNet.push_front(boost::make_shared(1, 8, 12)); + bayesNet.push_front(boost::make_shared(0, 8, 12)); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree out of a Bayes net + SymbolicBayesTree bayesTree(bayesNet); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); + } + + SymbolicBayesTree::Clique::shared_ptr R = bayesTree.root(); + + { + // check shortcut P(S9||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S8||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(12, 14)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S4||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(10, 13, 14)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S0||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(12, 14)); + expected.push_front(boost::make_shared(8, 12, 14)); + EXPECT(assert_equal(expected, shortcut)); + } +} + +/* ************************************************************************* * + Bayes tree for smoother with "natural" ordering: + C1 5 6 + C2 4 : 5 + C3 3 : 4 + C4 2 : 3 + C5 1 : 2 + C6 0 : 1 + **************************************************************************** */ + +TEST_UNSAFE( SymbolicBayesTree, linear_smoother_shortcuts ) { + // Create smoother with 7 nodes + SymbolicFactorGraph smoother; + smoother.push_factor(0); + smoother.push_factor(0, 1); + smoother.push_factor(1, 2); + smoother.push_factor(2, 3); + smoother.push_factor(3, 4); + smoother.push_factor(4, 5); + smoother.push_factor(5, 6); + + BayesNet bayesNet = + *SymbolicSequentialSolver(smoother).eliminate(); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree out of a Bayes net + SymbolicBayesTree bayesTree(bayesNet); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); + } + + SymbolicBayesTree::Clique::shared_ptr R = bayesTree.root(); + + { + // check shortcut P(S2||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S3||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(4, 5)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S4||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(3, 5)); + EXPECT(assert_equal(expected, shortcut)); + } +} + +/* ************************************************************************* */ +// from testSymbolicJunctionTree, which failed at one point +TEST(SymbolicBayesTree, complicatedMarginal) { + + // Create the conditionals to go in the BayesTree + list L; + L = list_of(1)(2)(5); + IndexConditional::shared_ptr R_1_2(new IndexConditional(L, 2)); + L = list_of(3)(4)(6); + IndexConditional::shared_ptr R_3_4(new IndexConditional(L, 2)); + L = list_of(5)(6)(7)(8); + IndexConditional::shared_ptr R_5_6(new IndexConditional(L, 2)); + L = list_of(7)(8)(11); + IndexConditional::shared_ptr R_7_8(new IndexConditional(L, 2)); + L = list_of(9)(10)(11)(12); + IndexConditional::shared_ptr R_9_10(new IndexConditional(L, 2)); + L = list_of(11)(12); + IndexConditional::shared_ptr R_11_12(new IndexConditional(L, 2)); + + // Symbolic Bayes Tree + typedef SymbolicBayesTree::Clique Clique; + typedef SymbolicBayesTree::sharedClique sharedClique; + + // Create Bayes Tree + SymbolicBayesTree bt; + bt.insert(sharedClique(new Clique(R_11_12))); + bt.insert(sharedClique(new Clique(R_9_10))); + bt.insert(sharedClique(new Clique(R_7_8))); + bt.insert(sharedClique(new Clique(R_5_6))); + bt.insert(sharedClique(new Clique(R_3_4))); + bt.insert(sharedClique(new Clique(R_1_2))); + if (debug) { + GTSAM_PRINT(bt); + bt.saveGraph("/tmp/symbolicBayesTree.dot"); + } + + SymbolicBayesTree::Clique::shared_ptr R = bt.root(); + SymbolicBayesNet empty; + + // Shortcut on 9 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[9]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + EXPECT(assert_equal(empty, shortcut)); + } + + // Shortcut on 7 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[7]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + EXPECT(assert_equal(empty, shortcut)); + } + + // Shortcut on 5 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[5]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(8, 11)); + expected.push_front(boost::make_shared(7, 8, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 3 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[3]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(6, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 1 + { + SymbolicBayesTree::Clique::shared_ptr c = bt[1]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(5, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Marginal on 5 + { + IndexFactor::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic); + EXPECT(assert_equal(IndexFactor(5), *actual, 1e-1)); + } + + // Shortcut on 6 + { + IndexFactor::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic); + EXPECT(assert_equal(IndexFactor(6), *actual, 1e-1)); + } + +} +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 75b96d9ed23bc18582f596ce0b67949abeb41533 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 00:50:15 +0000 Subject: [PATCH 045/107] Removed old indices code --- gtsam/inference/BayesTreeCliqueBase-inl.h | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 5ac87b6c3..c08a592f7 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -51,24 +51,12 @@ namespace gtsam { derived_ptr B, const FactorGraph& p_Cp_B) const { std::set allKeys = p_Cp_B.keys(); std::vector &indicesB = B->conditional()->keys(); - std::vector keep; -#ifdef OLD_INDICES - // We do this by first merging S and B - sharedConditional p_F_S = this->conditional(); - std::vector S_union_B; - std::set_union(p_F_S->beginParents(), p_F_S->endParents(),// - indicesB.begin(), indicesB.end(), back_inserter(S_union_B)); - - // then intersecting S_union_B with all keys in p_Cp_B - std::set_intersection(S_union_B.begin(), S_union_B.end(),// - allKeys.begin(), allKeys.end(), back_inserter(keep)); -#else std::vector S_setminus_B = separator_setminus_B(B); // TODO, get as argument? + std::vector keep; std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // allKeys.begin(), allKeys.end(), back_inserter(keep)); std::set_intersection(indicesB.begin(), indicesB.end(), // allKeys.begin(), allKeys.end(), back_inserter(keep)); -#endif // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl; return keep; } From 970efd9e299349750241d148ce9223de1d714a3c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 17 Sep 2012 01:26:08 +0000 Subject: [PATCH 046/107] Fixed missing include of boost.range, fixed signed/unsigned comparison warning --- gtsam/inference/Conditional.h | 2 ++ gtsam/inference/GenericSequentialSolver-inl.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index fa75df83f..0e3e98361 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -20,6 +20,8 @@ #include +#include + #include #include diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 1e48106e5..b5511dfd7 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -187,7 +187,7 @@ namespace gtsam { // Get rid of conditionals on variables that we want to marginalize out size_t nrMarginalized = bayesNet->size() - js.size(); - for (int i = 0; i < nrMarginalized; i++) + for (size_t i = 0; i < nrMarginalized; i++) bayesNet->pop_front(); return bayesNet; From 1f0cc0aaa4ed4ae6ab4e97f2c30853cc7bea9d89 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 03:31:24 +0000 Subject: [PATCH 047/107] 2-variable joint marginals computed with shortcuts are buggy: they only work if the cliques are disjoint or one of them is the root. They now throw an exception if that is the case. --- .../discrete/tests/testDiscreteBayesTree.cpp | 93 ++++++++++++++----- gtsam/inference/BayesTree.h | 10 +- gtsam/inference/BayesTreeCliqueBase-inl.h | 38 ++++---- gtsam/inference/BayesTreeCliqueBase.h | 5 +- .../inference/tests/testSymbolicBayesTree.cpp | 48 ++++++++++ 5 files changed, 152 insertions(+), 42 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 8ff50ad3c..68a3f45af 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -85,14 +85,14 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { const int nrNodes = 15; const size_t nrStates = 2; -// define variables + // define variables vector key; for (int i = 0; i < nrNodes; i++) { DiscreteKey key_i(i, nrStates); key.push_back(key_i); } -// create a thin-tree Bayesnet, a la Jean-Guillaume + // create a thin-tree Bayesnet, a la Jean-Guillaume DiscreteBayesNet bayesNet; add_front(bayesNet, key[14] % "1/3"); @@ -119,17 +119,18 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { bayesNet.saveGraph("/tmp/discreteBayesNet.dot"); } -// create a BayesTree out of a Bayes net + // create a BayesTree out of a Bayes net DiscreteBayesTree bayesTree(bayesNet); if (debug) { GTSAM_PRINT(bayesTree); bayesTree.saveGraph("/tmp/discreteBayesTree.dot"); } -// Check whether BN and BT give the same answer on all configurations -// Also calculate all some marginals + // Check whether BN and BT give the same answer on all configurations + // Also calculate all some marginals Vector marginals = zero(15); - double shortcut8, shortcut0; + double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint82 = 0, + joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, joint_4_11 = 0; vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); @@ -144,48 +145,94 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { marginals[i] += actual; // calculate shortcut 8 and 0 if (x[12] && x[14]) - shortcut8 += actual; + joint_12_14 += actual; + if (x[9] && x[12] & x[14]) + joint_9_12_14 += actual; if (x[8] && x[12] & x[14]) - shortcut0 += actual; + joint_8_12_14 += actual; + if (x[8] && x[2]) + joint82 += actual; + if (x[1] && x[2]) + joint12 += actual; + if (x[2] && x[4]) + joint24 += actual; + if (x[4] && x[5]) + joint45 += actual; + if (x[4] && x[6]) + joint46 += actual; + if (x[4] && x[11]) + joint_4_11 += actual; } DiscreteFactor::Values all1 = allPosbValues.back(); -// check shortcut P(S9||R) to root + // check shortcut P(S9||R) to root Clique::shared_ptr R = bayesTree.root(); Clique::shared_ptr c = bayesTree[9]; - DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); + DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); EXPECT_LONGS_EQUAL(0, shortcut.size()); -// check shortcut P(S8||R) to root + // check shortcut P(S8||R) to root c = bayesTree[8]; - shortcut = c->shortcut(R, &EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(shortcut8/marginals[14], evaluate(shortcut,all1), 1e-9); + shortcut = c->shortcut(R, EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint_12_14/marginals[14], evaluate(shortcut,all1), + 1e-9); -// check shortcut P(S0||R) to root + // check shortcut P(S2||R) to root + c = bayesTree[2]; + shortcut = c->shortcut(R, EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint_9_12_14/marginals[14], evaluate(shortcut,all1), + 1e-9); + + // check shortcut P(S0||R) to root c = bayesTree[0]; - shortcut = c->shortcut(R, &EliminateDiscrete); - EXPECT_DOUBLES_EQUAL(shortcut0/marginals[14], evaluate(shortcut,all1), 1e-9); + shortcut = c->shortcut(R, EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint_8_12_14/marginals[14], evaluate(shortcut,all1), + 1e-9); -// calculate all shortcuts to root + // calculate all shortcuts to root DiscreteBayesTree::Nodes cliques = bayesTree.nodes(); BOOST_FOREACH(Clique::shared_ptr c, cliques) { - DiscreteBayesNet shortcut = c->shortcut(R, &EliminateDiscrete); + DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); if (debug) { c->printSignature(); shortcut.print("shortcut:"); } } -// Check all marginals + // Check all marginals DiscreteFactor::shared_ptr marginalFactor; for (size_t i = 0; i < 15; i++) { - marginalFactor = bayesTree.marginalFactor(i, &EliminateDiscrete); - DiscreteFactor::Values x; - x[i] = 1; - double actual = (*marginalFactor)(x); + marginalFactor = bayesTree.marginalFactor(i, EliminateDiscrete); + double actual = (*marginalFactor)(all1); EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9); } + DiscreteBayesNet::shared_ptr actualJoint; + + // Check joint P(8,2) TODO: not disjoint ! +// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint82, evaluate(*actualJoint,all1), 1e-9); + + // Check joint P(1,2) TODO: not disjoint ! +// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint12, evaluate(*actualJoint,all1), 1e-9); + + // Check joint P(2,4) + actualJoint = bayesTree.jointBayesNet(2, 4, EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint24, evaluate(*actualJoint,all1), 1e-9); + + // Check joint P(4,5) TODO: not disjoint ! +// actualJoint = bayesTree.jointBayesNet(4, 5, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); + + // Check joint P(4,6) TODO: not disjoint ! +// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateDiscrete); +// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9); + + // Check joint P(4,11) + actualJoint = bayesTree.jointBayesNet(4, 11, EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint_4_11, evaluate(*actualJoint,all1), 1e-9); + } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 6406fe8f6..075b0b411 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -186,10 +186,16 @@ namespace gtsam { */ typename BayesNet::shared_ptr marginalBayesNet(Index j, Eliminate function) const; - /** return joint on two variables */ + /** + * return joint on two variables + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ typename FactorGraph::shared_ptr joint(Index j1, Index j2, Eliminate function) const; - /** return joint on two variables as a BayesNet */ + /** + * return joint on two variables as a BayesNet + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ typename BayesNet::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const; /** diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index c08a592f7..7d77a5150 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -221,32 +221,38 @@ namespace gtsam { Eliminate function) const { // For now, assume neither is the root + sharedConditional p_F1_S1 = this->conditional(); + sharedConditional p_F2_S2 = C2->conditional(); + // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) FactorGraph joint; - if (!isRoot()) - joint.push_back(this->conditional()->toFactor()); // P(F1|S1) - if (!isRoot()) + if (!isRoot()) { + joint.push_back(p_F1_S1->toFactor()); // P(F1|S1) joint.push_back(shortcut(R, function)); // P(S1|R) - if (!C2->isRoot()) - joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) - if (!C2->isRoot()) + } + if (!C2->isRoot()) { + joint.push_back(p_F2_S2->toFactor()); // P(F2|S2) joint.push_back(C2->shortcut(R, function)); // P(S2|R) + } joint.push_back(R->conditional()->toFactor()); // P(R) - // Find the keys of both C1 and C2 - std::vector keys1(conditional_->keys()); - std::vector keys2(C2->conditional_->keys()); - FastSet keys12; - keys12.insert(keys1.begin(), keys1.end()); - keys12.insert(keys2.begin(), keys2.end()); + // Merge the keys of C1 and C2 + std::vector keys12; + std::vector &indices1 = p_F1_S1->keys(), &indices2 = p_F2_S2->keys(); + std::set_union(indices1.begin(), indices1.end(), // + indices2.begin(), indices2.end(), std::back_inserter(keys12)); + + // Check validity + bool cliques_intersect = (keys12.size() < indices1.size() + indices2.size()); + if (!isRoot() && !C2->isRoot() && cliques_intersect) + throw std::runtime_error( + "BayesTreeCliqueBase::joint can only calculate joint if cliques are disjoint\n" + "or one of them is the root clique"); // Calculate the marginal - std::vector keys12vector; - keys12vector.reserve(keys12.size()); - keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); assertInvariants(); GenericSequentialSolver solver(joint); - return *solver.jointFactorGraph(keys12vector, function); + return *solver.jointFactorGraph(keys12, function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index ceeef3f45..87711b3f2 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -192,7 +192,10 @@ namespace gtsam { FactorGraph marginal(derived_ptr root, Eliminate function) const; - /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ + /** + * return the joint P(C1,C2), where C1==this. TODO: not a method? + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; diff --git a/gtsam/inference/tests/testSymbolicBayesTree.cpp b/gtsam/inference/tests/testSymbolicBayesTree.cpp index 5e00e23b1..b69bdac5e 100644 --- a/gtsam/inference/tests/testSymbolicBayesTree.cpp +++ b/gtsam/inference/tests/testSymbolicBayesTree.cpp @@ -99,6 +99,16 @@ TEST_UNSAFE( SymbolicBayesTree, thinTree ) { EXPECT(assert_equal(expected, shortcut)); } + { + // check shortcut P(S2||R) to root + SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; + SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(12, 14)); + expected.push_front(boost::make_shared(9, 12, 14)); + EXPECT(assert_equal(expected, shortcut)); + } + { // check shortcut P(S0||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; @@ -108,6 +118,44 @@ TEST_UNSAFE( SymbolicBayesTree, thinTree ) { expected.push_front(boost::make_shared(8, 12, 14)); EXPECT(assert_equal(expected, shortcut)); } + + SymbolicBayesNet::shared_ptr actualJoint; + + // Check joint P(8,2) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(8)); + expected.push_front(boost::make_shared(2, 8)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(1,2) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(2)); + expected.push_front(boost::make_shared(1, 2)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(2,6) + if (true) { + actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(6)); + expected.push_front(boost::make_shared(2, 6)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(4,6) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic); + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(6)); + expected.push_front(boost::make_shared(4, 6)); + EXPECT(assert_equal(expected, *actualJoint)); + } } /* ************************************************************************* * From 7fcd06bb4fba7e594e21d186c01cdf59fe0c42bb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 17 Sep 2012 14:03:54 +0000 Subject: [PATCH 048/107] BayesTree::marginalFactor now calls Clique::marginal2, which in turn calles the new function Clique::separatorMarginal. This calculates marginals with a much simpler recursion, using the parent separator marginal. This could be faster than the shortcut way, especially if separator sizes are small and the root clique is large. The cached marginals have to be discarded when the bayes tree is updated, but this is no different from shortcuts to the root. --- .../discrete/tests/testDiscreteBayesTree.cpp | 30 +++++++++-- gtsam/inference/BayesTree-inl.h | 27 +++++----- gtsam/inference/BayesTree.h | 4 +- gtsam/inference/BayesTreeCliqueBase-inl.h | 52 +++++++++++++++++++ gtsam/inference/BayesTreeCliqueBase.h | 12 +++++ gtsam/inference/GenericSequentialSolver-inl.h | 5 +- 6 files changed, 110 insertions(+), 20 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 68a3f45af..e3a771940 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -129,8 +129,9 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { // Check whether BN and BT give the same answer on all configurations // Also calculate all some marginals Vector marginals = zero(15); - double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint82 = 0, - joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, joint_4_11 = 0; + double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0, + joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, + joint_4_11 = 0; vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]); @@ -150,6 +151,8 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { joint_9_12_14 += actual; if (x[8] && x[12] & x[14]) joint_8_12_14 += actual; + if (x[8] && x[12]) + joint_8_12 += actual; if (x[8] && x[2]) joint82 += actual; if (x[1] && x[2]) @@ -165,9 +168,28 @@ TEST_UNSAFE( DiscreteBayesTree, thinTree ) { } DiscreteFactor::Values all1 = allPosbValues.back(); - // check shortcut P(S9||R) to root Clique::shared_ptr R = bayesTree.root(); - Clique::shared_ptr c = bayesTree[9]; + + // check separator marginal P(S0) + Clique::shared_ptr c = bayesTree[0]; + DiscreteFactorGraph separatorMarginal0 = c->separatorMarginal(R, + EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9); + + // check separator marginal P(S9), should be P(14) + c = bayesTree[9]; + DiscreteFactorGraph separatorMarginal9 = c->separatorMarginal(R, + EliminateDiscrete); + EXPECT_DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9); + + // check separator marginal of root, should be empty + c = bayesTree[11]; + DiscreteFactorGraph separatorMarginal11 = c->separatorMarginal(R, + EliminateDiscrete); + EXPECT_LONGS_EQUAL(0, separatorMarginal11.size()); + + // check shortcut P(S9||R) to root + c = bayesTree[9]; DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete); EXPECT_LONGS_EQUAL(0, shortcut.size()); diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 3d5276be8..e09fdc83a 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -475,22 +475,23 @@ namespace gtsam { } } - /* ************************************************************************* */ - // First finds clique marginal then marginalizes that - /* ************************************************************************* */ - template - typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( - Index j, Eliminate function) const { + /* ************************************************************************* */ + // First finds clique marginal then marginalizes that + /* ************************************************************************* */ + template + typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( + Index j, Eliminate function) const { - // get clique containing Index j - sharedClique clique = (*this)[j]; + // get clique containing Index j + sharedClique clique = (*this)[j]; - // calculate or retrieve its marginal - FactorGraph cliqueMarginal = clique->marginal(root_,function); + // calculate or retrieve its marginal P(C) = P(F,S) + FactorGraph cliqueMarginal = clique->marginal2(root_,function); - return GenericSequentialSolver(cliqueMarginal).marginalFactor( - j, function); - } + // now, marginalize out everything that is not variable j + GenericSequentialSolver solver(cliqueMarginal); + return solver.marginalFactor(j, function); + } /* ************************************************************************* */ template diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 075b0b411..54e738559 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -219,7 +219,9 @@ namespace gtsam { void clear(); /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ - inline void deleteCachedShorcuts() { root_->deleteCachedShorcuts(); } + inline void deleteCachedShorcuts() { + root_->deleteCachedShorcuts(); + } /** * Remove path from clique to root and return that path as factors diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 7d77a5150..bc98c28b5 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -212,6 +212,58 @@ namespace gtsam { return *solver.jointFactorGraph(conditional_->keys(), function); } + /* ************************************************************************* */ + // separator marginal, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + FactorGraph::FactorType> BayesTreeCliqueBase< + DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const { + // Check if the Separator marginal was already calculated + if (!cachedSeparatorMarginal_) { + + // If this is the root, there is no separator + if (R.get() == this) { + // we are root, return empty + FactorGraph empty; + cachedSeparatorMarginal_ = empty; + } else { + // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) + // initialize P(Cp) with the parent separator marginal + derived_ptr parent(parent_.lock()); + FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) + // now add the parent cobnditional + p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + + // Create solver that will marginalize for us + GenericSequentialSolver solver(p_Cp); + + // The variables we want to keep are exactly the ones in S + sharedConditional p_F_S = this->conditional(); + std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); + + cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); + } + } + + // return the shortcut P(S||B) + return *cachedSeparatorMarginal_; // return the cached version + } + + /* ************************************************************************* */ + // marginal2, uses separator marginal of parent recursively + // P(C) = P(F|S) P(S) + /* ************************************************************************* */ + template + FactorGraph::FactorType> BayesTreeCliqueBase< + DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const { + // initialize with separator marginal P(S) + FactorGraph p_C(this->separatorMarginal(R, function)); + // add the conditional P(F|S) + p_C.push_back(this->conditional()->toFactor()); + return p_C; + } + /* ************************************************************************* */ // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 87711b3f2..edde8dedc 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -83,6 +83,9 @@ namespace gtsam { /// This stores the Cached Shortcut value mutable boost::optional > cachedShortcut_; + /// This stores the Cached separator margnal P(S) + mutable boost::optional > cachedSeparatorMarginal_; + public: sharedConditional conditional_; derived_weak_ptr parent_; @@ -192,6 +195,14 @@ namespace gtsam { FactorGraph marginal(derived_ptr root, Eliminate function) const; + /** return the conditional P(S|Root) on the separator given the root */ + FactorGraph separatorMarginal(derived_ptr root, + Eliminate function) const; + + /** return the marginal P(C) of the clique, using separator shortcuts */ + FactorGraph marginal2(derived_ptr root, + Eliminate function) const; + /** * return the joint P(C1,C2), where C1==this. TODO: not a method? * Limitation: can only calculate joint if cliques are disjoint or one of them is root @@ -233,6 +244,7 @@ namespace gtsam { /// Reset the computed shortcut of this clique. Used by friend BayesTree void resetCachedShortcut() { + cachedSeparatorMarginal_ = boost::none; cachedShortcut_ = boost::none; } diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index b5511dfd7..7800f4adb 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -200,8 +200,8 @@ namespace gtsam { const std::vector& js, Eliminate function) const { // Eliminate all variables - typename BayesNet::shared_ptr bayesNet = jointBayesNet(js, - function); + typename BayesNet::shared_ptr bayesNet = // + jointBayesNet(js, function); return boost::make_shared >(*bayesNet); } @@ -216,6 +216,7 @@ namespace gtsam { js[0] = j; // Call joint and return the only factor in the factor graph it returns + // TODO: just call jointBayesNet and grab last conditional, then toFactor.... return (*this->jointFactorGraph(js, function))[0]; } From f272a07e29ed45abc8f7f61e5509804b79a796c1 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 18 Sep 2012 17:48:18 +0000 Subject: [PATCH 049/107] Moved DummyFactor to gtsam_unstable from MastSLAM - allows for adding sufficient connectivity for solvers to operate --- gtsam_unstable/gtsam_unstable.h | 5 ++ gtsam_unstable/slam/DummyFactor.cpp | 64 ++++++++++++++++ gtsam_unstable/slam/DummyFactor.h | 76 +++++++++++++++++++ gtsam_unstable/slam/tests/testDummyFactor.cpp | 58 ++++++++++++++ 4 files changed, 203 insertions(+) create mode 100644 gtsam_unstable/slam/DummyFactor.cpp create mode 100644 gtsam_unstable/slam/DummyFactor.h create mode 100644 gtsam_unstable/slam/tests/testDummyFactor.cpp diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index a459925fd..077d6c4e7 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -199,4 +199,9 @@ virtual class RelativeElevationFactor: gtsam::NonlinearFactor { void print(string s) const; }; +#include +virtual class DummyFactor : gtsam::NonlinearFactor { + DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2); +}; + } //\namespace gtsam diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp new file mode 100644 index 000000000..c4b973ea5 --- /dev/null +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -0,0 +1,64 @@ +/** + * @file DummyFactor.cpp + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) +: NonlinearFactor(key1, key2) +{ + dims_.push_back(dim1); + dims_.push_back(dim2); + if (dim1 > dim2) + rowDim_ = dim1; + else + rowDim_ = dim2; +} + +/* ************************************************************************* */ +void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << " DummyFactor dim = " << rowDim_ << ", keys = { "; + BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } + std::cout << "}" << std::endl; +} + +/* ************************************************************************* */ +bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { + const DummyFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_; +} + +/* ************************************************************************* */ +boost::shared_ptr +DummyFactor::linearize(const Values& c, const Ordering& ordering) const { + // Only linearize if the factor is active + if (!this->active(c)) + return boost::shared_ptr(); + + // Fill in terms with zero matrices + std::vector > terms(this->size()); + for(size_t j=0; jsize(); ++j) { + terms[j].first = ordering[this->keys()[j]]; + terms[j].second = zeros(rowDim_, dims_[j]); + } + + noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, zero(rowDim_), model)); +} + +/* ************************************************************************* */ + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h new file mode 100644 index 000000000..7d3b1a8da --- /dev/null +++ b/gtsam_unstable/slam/DummyFactor.h @@ -0,0 +1,76 @@ +/** + * @file DummyFactor.h + * + * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected. + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +class DummyFactor : public NonlinearFactor { +protected: + + // Store the dimensions of the variables and the dimension of the full system + std::vector dims_; + size_t rowDim_; ///< choose dimension for the rows + +public: + + /** Default constructor: don't use directly */ + DummyFactor() : rowDim_(1) { } + + /** standard binary constructor */ + DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2); + + virtual ~DummyFactor() {} + + // testable + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + + // access + + const std::vector& dims() const { return dims_; } + + // factor interface + + /** + * Calculate the error of the factor - zero for dummy factors + */ + virtual double error(const Values& c) const { return 0.0; } + + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const { return rowDim_; } + + /** linearize to a GaussianFactor */ + virtual boost::shared_ptr + linearize(const Values& c, const Ordering& ordering) const; + + /** + * Creates a shared_ptr clone of the factor - needs to be specialized to allow + * for subclasses + * + * By default, throws exception if subclass does not implement the function. + */ + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new DummyFactor(*this))); + } + +}; + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp new file mode 100644 index 000000000..e4b43c496 --- /dev/null +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -0,0 +1,58 @@ +/** + * @file testDummyFactor.cpp + * + * @brief A simple dummy nonlinear factor that can be used to enforce connectivity + * without adding any real information + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +using namespace gtsam; + +const double tol = 1e-9; + +/* ************************************************************************* */ +TEST( testDummyFactor, basics ) { + + gtsam::Key key1 = 7, key2 = 9; + size_t dim1 = 3, dim2 = 3; + DummyFactor dummyfactor(key1, dim1, key2, dim2); + + // verify contents + LONGS_EQUAL(2, dummyfactor.size()); + EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]); + EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]); + + LONGS_EQUAL(2, dummyfactor.dims().size()); + EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]); + EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]); + + Values c; + c.insert(key1, Point3(1.0, 2.0, 3.0)); + c.insert(key2, Point3(4.0, 5.0, 6.0)); + + // errors - all zeros + DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); + + Ordering ordering; + ordering += key1, key2; + + // linearization: all zeros + GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); + CHECK(actLinearization); + noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); + GaussianFactor::shared_ptr expLinearization(new JacobianFactor( + ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); + EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From c6f7fa8fb767015542724a7fd719ab7eca160d8b Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 18 Sep 2012 17:48:20 +0000 Subject: [PATCH 050/107] Check that P is non-empty before drawing covariance ellipses --- matlab/+gtsam/plotPoint2.m | 2 +- matlab/+gtsam/plotPose2.m | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/matlab/+gtsam/plotPoint2.m b/matlab/+gtsam/plotPoint2.m index 55e476118..cd066951d 100644 --- a/matlab/+gtsam/plotPoint2.m +++ b/matlab/+gtsam/plotPoint2.m @@ -5,6 +5,6 @@ if size(color,2)==1 else plot(p.x,p.y,color); end -if exist('P', 'var') +if exist('P', 'var') && (~isempty(P)) gtsam.covarianceEllipse([p.x;p.y],P,color(1)); end \ No newline at end of file diff --git a/matlab/+gtsam/plotPose2.m b/matlab/+gtsam/plotPose2.m index 799bad48e..7156f9b60 100644 --- a/matlab/+gtsam/plotPose2.m +++ b/matlab/+gtsam/plotPose2.m @@ -6,7 +6,7 @@ plot(pose.x,pose.y,[color '*']); c = cos(pose.theta); s = sin(pose.theta); quiver(pose.x,pose.y,c,s,axisLength,color); -if nargin>2 +if nargin>2 && (~isempty(P)) pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame gRp = [c -s;s c]; % rotation from pose to global gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); From a2474ef3545205ff510962c6300b88ab8ab248ac Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 18 Sep 2012 18:36:42 +0000 Subject: [PATCH 051/107] Starting to add support for STL containers in wrap --- wrap/Module.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 60147e23d..cef0d40ee 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -114,8 +114,11 @@ Module::Module(const string& interfacePath, Rule eigenType_p = (str_p("Vector") | "Matrix"); + + //Rule for STL Containers (class names are lowercase) + Rule stlType_p = (str_p("vector") | "list"); - Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p); + Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p; Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; @@ -491,14 +494,12 @@ map Module::appendInheretedMethods(const Class& cls, const vecto map methods; if(!cls.qualifiedParent.empty()) { - cout << "Class: " << cls.name << " Parent Name: " << cls.qualifiedParent.back() << endl; //Find Class BOOST_FOREACH(const Class& parent, classes) { //We found the class for our parent if(parent.name == cls.qualifiedParent.back()) { - cout << "Inner class: " << cls.qualifiedParent.back() << endl; Methods inhereted = appendInheretedMethods(parent, classes); methods.insert(inhereted.begin(), inhereted.end()); } @@ -506,7 +507,6 @@ map Module::appendInheretedMethods(const Class& cls, const vecto } else { - cout << "Dead end: " << cls.name << endl; methods.insert(cls.methods.begin(), cls.methods.end()); } From 0ef12f2f2012f08fb6bb46dbf57dd4a863e05738 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 19 Sep 2012 02:23:43 +0000 Subject: [PATCH 052/107] Moved in reference frame factor from MastSLAM --- .cproject | 342 +++++++++--------- gtsam_unstable/slam/ReferenceFrameFactor.h | 133 +++++++ .../slam/tests/testReferenceFrameFactor.cpp | 248 +++++++++++++ 3 files changed, 560 insertions(+), 163 deletions(-) create mode 100644 gtsam_unstable/slam/ReferenceFrameFactor.h create mode 100644 gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp diff --git a/.cproject b/.cproject index 52375afab..2bc98db3c 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -309,14 +307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +333,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +341,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +389,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +397,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +405,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +421,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +523,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +539,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +579,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +683,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -799,6 +803,14 @@ true true + + make + -j5 + testReferenceFrameFactor.run + true + true + true + make -j5 @@ -1033,6 +1045,7 @@ make + testGraph.run true false @@ -1040,6 +1053,7 @@ make + testJunctionTree.run true false @@ -1047,6 +1061,7 @@ make + testSymbolicBayesNetB.run true false @@ -1206,6 +1221,7 @@ make + testErrors.run true false @@ -1251,10 +1267,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1339,10 +1355,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1677,7 +1693,6 @@ make - testSimulated2DOriented.run true false @@ -1717,7 +1732,6 @@ make - testSimulated2D.run true false @@ -1725,7 +1739,6 @@ make - testSimulated3D.run true false @@ -1917,7 +1930,6 @@ make - tests/testGaussianISAM2 true false @@ -1939,102 +1951,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2236,6 +2152,7 @@ cpack + -G DEB true false @@ -2243,6 +2160,7 @@ cpack + -G RPM true false @@ -2250,6 +2168,7 @@ cpack + -G TGZ true false @@ -2257,6 +2176,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2390,34 +2310,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2461,6 +2445,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/slam/ReferenceFrameFactor.h b/gtsam_unstable/slam/ReferenceFrameFactor.h new file mode 100644 index 000000000..f0ea66b36 --- /dev/null +++ b/gtsam_unstable/slam/ReferenceFrameFactor.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file ReferenceFrameFactor.h + * @brief A constraint for combining graphs by common landmarks and a transform node + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * Transform function that must be specialized specific domains + * @tparam T is a Transform type + * @tparam P is a point type + */ +template +P transform_point( + const T& trans, const P& global, + boost::optional Dtrans, + boost::optional Dglobal) { + return trans.transform_from(global, Dtrans, Dglobal); +} + +/** + * A constraint between two landmarks in separate maps + * Templated on: + * Point : Type of landmark + * Transform : Transform variable class + * + * The transform is defined as transforming global to local: + * l = lTg * g + * + * The Point and Transform concepts must be Lie types, and the transform + * relationship "Point = transform_from(Transform, Point)" must exist. + * + * To implement this function in new domains, specialize a new version of + * Point transform_point(transform, global, Dtrans, Dglobal) + * to use the correct point and transform types. + * + * This base class should be specialized to implement the cost function for + * specific classes of landmarks + */ +template +class ReferenceFrameFactor : public NoiseModelFactor3 { +protected: + /** default constructor for serialization only */ + ReferenceFrameFactor() {} + +public: + typedef NoiseModelFactor3 Base; + typedef ReferenceFrameFactor This; + + typedef POINT Point; + typedef TRANSFORM Transform; + + /** + * General constructor with arbitrary noise model (constrained or otherwise) + */ + ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model) + : Base(model,globalKey, transKey, localKey) {} + + /** + * Construct a hard frame of reference reference constraint with equal mu values for + * each degree of freedom. + */ + ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey) + : Base(globalKey, transKey, localKey, Point().dim(), mu) {} + + /** + * Simple soft constraint constructor for frame of reference, with equal weighting for + * each degree of freedom. + */ + ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) + : Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma), + globalKey, transKey, localKey) {} + + virtual ~ReferenceFrameFactor(){} + + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } + + /** Combined cost and derivative function using boost::optional */ + virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local, + boost::optional Dforeign = boost::none, + boost::optional Dtrans = boost::none, + boost::optional Dlocal = boost::none) const { + Point newlocal = transform_point(trans, global, Dtrans, Dforeign); + if (Dlocal) { + Point dummy; + *Dlocal = -1* gtsam::eye(dummy.dim()); + } + return local.localCoordinates(newlocal); + } + + virtual void print(const std::string& s="", + const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << ": ReferenceFrameFactor(" + << "Global: " << keyFormatter(this->key1()) << "," + << " Transform: " << keyFormatter(this->key2()) << "," + << " Local: " << keyFormatter(this->key3()) << ")\n"; + this->noiseModel_->print(" noise model"); + } + + // access - convenience functions + Key global_key() const { return this->key1(); } + Key transform_key() const { return this->key2(); } + Key local_key() const { return this->key3(); } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor3", + boost::serialization::base_object(*this)); + } +}; + +} // \namespace gtsam diff --git a/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp b/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp new file mode 100644 index 000000000..4e7c9e455 --- /dev/null +++ b/gtsam_unstable/slam/tests/testReferenceFrameFactor.cpp @@ -0,0 +1,248 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testReferenceFrameFactor.cpp + * @author Alex Cunningham + */ + +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace boost; +using namespace gtsam; + +typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; +typedef gtsam::ReferenceFrameFactor PoseReferenceFrameFactor; + +Key lA1 = symbol_shorthand::L(1), lA2 = symbol_shorthand::L(2), lB1 = symbol_shorthand::L(11), lB2 = symbol_shorthand::L(12); +Key tA1 = symbol_shorthand::T(1), tB1 = symbol_shorthand::T(2); + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, equals ) { + PointReferenceFrameFactor + c1(lB1, tA1, lA1), + c2(lB1, tA1, lA1), + c3(lB1, tA1, lA2); + + EXPECT(assert_equal(c1, c1)); + EXPECT(assert_equal(c1, c2)); + EXPECT(!c1.equals(c3)); +} + +/* ************************************************************************* */ +LieVector evaluateError_(const PointReferenceFrameFactor& c, + const Point2& global, const Pose2& trans, const Point2& local) { + return LieVector(c.evaluateError(global, trans, local)); +} +TEST( ReferenceFrameFactor, jacobians ) { + + // from examples below + Point2 local(2.0, 3.0), global(-1.0, 2.0); + Pose2 trans(1.5, 2.5, 0.3); + + PointReferenceFrameFactor tc(lA1, tA1, lB1); + Matrix actualDT, actualDL, actualDF; + tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); + + Matrix numericalDT, numericalDL, numericalDF; + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + + EXPECT(assert_equal(numericalDF, actualDF)); + EXPECT(assert_equal(numericalDL, actualDL)); + EXPECT(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, jacobians_zero ) { + + // get values that are ideal + Pose2 trans(2.0, 3.0, 0.0); + Point2 global(5.0, 6.0); + Point2 local = trans.transform_from(global); + + PointReferenceFrameFactor tc(lA1, tA1, lB1); + Vector actCost = tc.evaluateError(global, trans, local), + expCost = zero(2); + EXPECT(assert_equal(expCost, actCost, 1e-5)); + + Matrix actualDT, actualDL, actualDF; + tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); + + Matrix numericalDT, numericalDL, numericalDF; + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), + global, trans, local, 1e-5); + + EXPECT(assert_equal(numericalDF, actualDF)); + EXPECT(assert_equal(numericalDL, actualDL)); + EXPECT(assert_equal(numericalDT, actualDT)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) { + + // initial points + Point2 local1(2.0, 2.0), local2(4.0, 5.0), + global1(-1.0, 5.0), global2(2.0, 3.0); + Pose2 transIdeal(7.0, 3.0, M_PI/2); + + // verify direction + EXPECT(assert_equal(local1, transIdeal.transform_from(global1))); + EXPECT(assert_equal(local2, transIdeal.transform_from(global2))); + + // choose transform + // Pose2 trans = transIdeal; // ideal - works + // Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works + // Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works + // Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works + Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works + // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails + + NonlinearFactorGraph graph; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(PointReferenceFrameFactor(lB2, tA1, lA2)); + + // hard constraints on points + double error_gain = 1000.0; + graph.add(NonlinearEquality(lA1, local1, error_gain)); + graph.add(NonlinearEquality(lA2, local2, error_gain)); + graph.add(NonlinearEquality(lB1, global1, error_gain)); + graph.add(NonlinearEquality(lB2, global2, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local1); + init.insert(lA2, local2); + init.insert(lB1, global1); + init.insert(lB2, global2); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + Values expected; + expected.insert(lA1, local1); + expected.insert(lA2, local2); + expected.insert(lB1, global1); + expected.insert(lB2, global2); + expected.insert(tA1, transIdeal); + + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, converge_local ) { + + // initial points + Point2 global(-1.0, 2.0); + // Pose2 trans(1.5, 2.5, 0.3); // original + // Pose2 trans(1.5, 2.5, 1.0); // larger rotation + Pose2 trans(1.5, 2.5, 3.1); // significant rotation + + Point2 idealLocal = trans.transform_from(global); + + // perturb the initial estimate + // Point2 local = idealLocal; // Ideal case - works + // Point2 local = idealLocal + Point2(1.0, 0.0); // works + Point2 local = idealLocal + Point2(-10.0, 10.0); // works + + NonlinearFactorGraph graph; + double error_gain = 1000.0; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(NonlinearEquality(lB1, global, error_gain)); + graph.add(NonlinearEquality(tA1, trans, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local); + init.insert(lB1, global); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + CHECK(actual.exists(lA1)); + EXPECT(assert_equal(idealLocal, actual.at(lA1), 1e-5)); +} + +/* ************************************************************************* */ +TEST( ReferenceFrameFactor, converge_global ) { + + // initial points + Point2 local(2.0, 3.0); + // Pose2 trans(1.5, 2.5, 0.3); // original + // Pose2 trans(1.5, 2.5, 1.0); // larger rotation + Pose2 trans(1.5, 2.5, 3.1); // significant rotation + + Point2 idealForeign = trans.inverse().transform_from(local); + + // perturb the initial estimate + // Point2 global = idealForeign; // Ideal - works + // Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works + Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works + + NonlinearFactorGraph graph; + double error_gain = 1000.0; + graph.add(PointReferenceFrameFactor(lB1, tA1, lA1)); + graph.add(NonlinearEquality(lA1, local, error_gain)); + graph.add(NonlinearEquality(tA1, trans, error_gain)); + + // create initial estimate + Values init; + init.insert(lA1, local); + init.insert(lB1, global); + init.insert(tA1, trans); + + // optimize + LevenbergMarquardtOptimizer solver(graph, init); + Values actual = solver.optimize(); + + // verify + CHECK(actual.exists(lB1)); + EXPECT(assert_equal(idealForeign, actual.at(lB1), 1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + From 1985758d358e5cf10e7921b5ff0b064ab6b7c64d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 21 Sep 2012 14:19:57 +0000 Subject: [PATCH 053/107] Updated interfaces and wrapped LinearContainerFactor --- gtsam_unstable/gtsam_unstable.h | 49 ++++++++++++++++++- .../nonlinear/LinearContainerFactor.cpp | 9 ++-- .../nonlinear/LinearContainerFactor.h | 4 +- 3 files changed, 56 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 077d6c4e7..8997b0678 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -10,6 +10,14 @@ virtual class gtsam::Pose2; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::NonlinearFactor; +virtual class gtsam::GaussianFactor; +virtual class gtsam::HessianFactor; +virtual class gtsam::JacobianFactor; +class gtsam::GaussianFactorGraph; +class gtsam::NonlinearFactorGraph; +class gtsam::Ordering; +class gtsam::Values; +class gtsam::InvertedOrdering; namespace gtsam { @@ -167,7 +175,46 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { }; //************************************************************************* -// General nonlinear factor types +// nonlinear +//************************************************************************* +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering, + const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering, + const gtsam::Values& linearizationPoint); + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering); + LinearContainerFactor(gtsam::GaussianFactor* factor); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const; + gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactor* negate() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint); + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Ordering& ordering); + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::InvertedOrdering& invOrdering); + +}; // \class LinearContainerFactor + +//************************************************************************* +// slam //************************************************************************* #include diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index b5701fb13..ee7e2eac1 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -219,17 +219,20 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const Ordering& ordering) { + const GaussianFactorGraph& linear_graph, const Ordering& ordering, + const Values& linearizationPoint) { return convertLinearGraph(linear_graph, ordering.invert()); } /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering) { + const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering, + const Values& linearizationPoint) { NonlinearFactorGraph result; BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) - result.push_back(NonlinearFactorGraph::sharedFactor(new LinearContainerFactor(f, invOrdering))); + result.push_back(NonlinearFactorGraph::sharedFactor( + new LinearContainerFactor(f, invOrdering, linearizationPoint))); return result; } diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 324556622..605a5885b 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -132,10 +132,10 @@ public: * If the inverse ordering is present, it will be faster. */ static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const Ordering& ordering); + const Ordering& ordering, const Values& linearizationPoint = Values()); static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const InvertedOrdering& invOrdering); + const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values()); protected: void rekeyFactor(const Ordering& ordering); From 0e60b8cc4d23dde94862fed5b9bba349d37d82e8 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 21 Sep 2012 20:02:26 +0000 Subject: [PATCH 054/107] Added access functions for cachedSeparatorMarginals --- gtsam/inference/BayesTreeCliqueBase.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index edde8dedc..7038ac228 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -217,10 +217,14 @@ namespace gtsam { void deleteCachedShorcuts(); /** return cached shortcut of the clique */ - const boost::optional > cachedShortcut() const { + const boost::optional >& cachedShortcut() const { return cachedShortcut_; } + const boost::optional >& cachedSeparatorMarginal() const { + return cachedSeparatorMarginal_; + } + friend class BayesTree ; protected: From df2a6bfdee2731dbfba41733880ad24ba36d5833 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 21 Sep 2012 20:57:30 +0000 Subject: [PATCH 055/107] Added reporting functions for counting number of cached shortcuts/separatorMarginals in BayesTree --- gtsam.h | 4 +++ gtsam/inference/BayesTree-inl.h | 12 +++++++++ gtsam/inference/BayesTree.h | 6 +++++ gtsam/inference/BayesTreeCliqueBase-inl.h | 30 +++++++++++++++++++++-- gtsam/inference/BayesTreeCliqueBase.h | 6 +++++ gtsam/inference/tests/testBayesTree.cpp | 2 ++ 6 files changed, 58 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index 6777106b8..308ebdf7e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -810,6 +810,8 @@ virtual class BayesTree { void clear(); void deleteCachedShorcuts(); void insert(const CLIQUE* subtree); + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; }; template @@ -822,6 +824,8 @@ virtual class BayesTreeClique { void print(string s) const; void printTree() const; // Default indent of "" void printTree(string indent) const; + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; CONDITIONAL* conditional() const; bool isRoot() const; diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index e09fdc83a..bf3b7f37c 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -56,6 +56,18 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + size_t BayesTree::numCachedShortcuts() const { + return (root_) ? root_->numCachedShortcuts() : 0; + } + + /* ************************************************************************* */ + template + size_t BayesTree::numCachedSeparatorMarginals() const { + return (root_) ? root_->numCachedSeparatorMarginals() : 0; + } + /* ************************************************************************* */ template void BayesTree::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 54e738559..5753c14c4 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -176,6 +176,12 @@ namespace gtsam { /** Gather data on all cliques */ CliqueData getCliqueData() const; + /** Collect number of cliques with cached shortcuts */ + size_t numCachedShortcuts() const; + + /** Collect number of cliques with cached separator marginals */ + size_t numCachedSeparatorMarginals() const; + /** return marginal on any variable */ typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const; diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index bc98c28b5..639e27202 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -90,14 +90,40 @@ namespace gtsam { size_t BayesTreeCliqueBase::treeSize() const { size_t size = 1; BOOST_FOREACH(const derived_ptr& child, children_) - size += child->treeSize(); + size += child->treeSize(); return size; } + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBase::numCachedShortcuts() const { + if (!cachedShortcut_) + return 0; + + size_t subtree_count = 1; + BOOST_FOREACH(const derived_ptr& child, children_) + subtree_count += child->numCachedShortcuts(); + + return subtree_count; + } + + /* ************************************************************************* */ + template + size_t BayesTreeCliqueBase::numCachedSeparatorMarginals() const { + if (!cachedSeparatorMarginal_) + return 0; + + size_t subtree_count = 1; + BOOST_FOREACH(const derived_ptr& child, children_) + subtree_count += child->numCachedSeparatorMarginals(); + + return subtree_count; + } + /* ************************************************************************* */ template void BayesTreeCliqueBase::printTree( - const std::string& indent, const IndexFormatter& indexFormatter) const { + const std::string& indent, const IndexFormatter& indexFormatter) const { asDerived(this)->print(indent, indexFormatter); BOOST_FOREACH(const derived_ptr& child, children_) child->printTree(indent + " ", indexFormatter); diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 7038ac228..19065ff8f 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -125,6 +125,12 @@ namespace gtsam { /** The size of subtree rooted at this clique, i.e., nr of Cliques */ size_t treeSize() const; + /** Collect number of cliques with cached shortcuts in subtree */ + size_t numCachedShortcuts() const; + + /** Collect number of cliques with cached separator marginals */ + size_t numCachedSeparatorMarginals() const; + /** The arrow operator accesses the conditional */ const ConditionalType* operator->() const { return conditional_.get(); diff --git a/gtsam/inference/tests/testBayesTree.cpp b/gtsam/inference/tests/testBayesTree.cpp index b99364092..b0fe5e670 100644 --- a/gtsam/inference/tests/testBayesTree.cpp +++ b/gtsam/inference/tests/testBayesTree.cpp @@ -293,6 +293,8 @@ TEST( BayesTree, shortcutCheck ) bool notCleared = clique->cachedShortcut(); CHECK( notCleared == false); } + EXPECT_LONGS_EQUAL(0, rootClique->numCachedShortcuts()); + EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals()); // BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { // clique->print("Clique#"); From 6284312a5c7c50482a471801458399795c7f8129 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 24 Sep 2012 19:42:41 +0000 Subject: [PATCH 056/107] Added unit test for Fast* containers, added generic interface to convert from containers to KeySet --- gtsam.h | 2 ++ gtsam/base/FastSet.h | 6 +++++ gtsam/base/tests/testFastContainers.cpp | 34 +++++++++++++++++++++++++ 3 files changed, 42 insertions(+) create mode 100644 gtsam/base/tests/testFastContainers.cpp diff --git a/gtsam.h b/gtsam.h index 308ebdf7e..130c5260c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1517,6 +1517,8 @@ class KeyList { class KeySet { KeySet(); KeySet(const gtsam::KeySet& other); + KeySet(const gtsam::KeyVector& other); + KeySet(const gtsam::KeyList& other); // Testable void print(string s) const; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 123a8bb31..6076c7f03 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -61,6 +61,12 @@ public: Base(first, last) { } + /** Constructor from a iterable container, passes through to base class */ + template + explicit FastSet(const INPUTCONTAINER& container) : + Base(container.begin(), container.end()) { + } + /** Copy constructor from another FastSet */ FastSet(const FastSet& x) : Base(x) { diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp new file mode 100644 index 000000000..b3e065921 --- /dev/null +++ b/gtsam/base/tests/testFastContainers.cpp @@ -0,0 +1,34 @@ +/** + * @file testFastContainers.cpp + * + * @brief Test for the Fast* containers that use boost pool allocators and interfaces + * + * @date Sep 24, 2012 + * @author Alex Cunningham + */ + +#include + +#include +#include + +#include +#include + +using namespace boost::assign; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( testFastContainers, KeySet ) { + + FastVector init_vector; + init_vector += 2, 3, 4, 5; + + FastSet actSet(init_vector); + FastSet expSet; expSet += 2, 3, 4, 5; + EXPECT(actSet == expSet); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From e3aaeedbf2c05edb7fb86a95aa492ae2930cefc4 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 24 Sep 2012 21:05:37 +0000 Subject: [PATCH 057/107] update the optimize interface --- gtsam/linear/IterativeSolver.h | 4 ++++ gtsam/linear/SubgraphSolver.cpp | 5 +++++ gtsam/linear/SubgraphSolver.h | 1 + 3 files changed, 10 insertions(+) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 7946874bb..05741d763 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -68,6 +68,10 @@ namespace gtsam { /* interface to the nonlinear optimizer */ virtual VectorValues optimize () = 0; + + /* interface to the nonlinear optimizer */ + virtual VectorValues optimize (const VectorValues &initial) = 0; + /* update interface to the nonlinear optimizer */ virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} }; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 9986fb76c..519192144 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -78,6 +78,11 @@ VectorValues SubgraphSolver::optimize() { return pc_->x(ybar); } +VectorValues SubgraphSolver::optimize(const VectorValues &initial) { + // the initial is ignored in this case ... + return optimize(); +} + void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index fe968d486..e8d72c23b 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -72,6 +72,7 @@ public: virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; + virtual VectorValues optimize (const VectorValues &initial) ; protected: From c323f41e8fa3f34f52c1210037832d302e49a397 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 25 Sep 2012 18:45:28 +0000 Subject: [PATCH 058/107] Fixed silently ignoring robust noise model when calling whiten on one - now throws exception. Also added function to whiten only the rhs vector in robust noise models. --- gtsam/linear/NoiseModel.cpp | 15 ++++ gtsam/linear/NoiseModel.h | 166 ++++++++++++++++++------------------ 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7479c2141..89a18e38d 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -463,6 +463,16 @@ Vector Base::sqrtWeight(const Vector &error) const { /** The following three functions reweight block matrices and a vector * according to their weight implementation */ +void Base::reweight(Vector& error) const { + if(reweight_ == Block) { + const double w = sqrtWeight(error.norm()); + error *= w; + } else { + const Vector w = sqrtWeight(error); + error.array() *= w.array(); + } +} + /** Reweight n block matrices with one error vector */ void Base::reweight(vector &A, Vector &error) const { if ( reweight_ == Block ) { @@ -613,6 +623,11 @@ bool Robust::equals(const Base& expected, double tol) const { return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); } +void Robust::WhitenSystem(Vector& b) const { + noise_->whitenInPlace(b); + robust_->reweight(b); +} + void Robust::WhitenSystem(vector& A, Vector& b) const { noise_->WhitenSystem(A,b); robust_->reweight(A,b); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 8588c738b..31584499e 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -570,97 +570,98 @@ namespace gtsam { } }; - // TODO: should not really exist - /// The MEstimator namespace contains all robust error functions (not models) - namespace MEstimator { + // TODO: should not really exist + /// The MEstimator namespace contains all robust error functions (not models) + namespace MEstimator { - //--------------------------------------------------------------------------------------- + //--------------------------------------------------------------------------------------- - class Base { - public: - enum ReweightScheme { Scalar, Block }; - typedef boost::shared_ptr shared_ptr; + class Base { + public: + enum ReweightScheme { Scalar, Block }; + typedef boost::shared_ptr shared_ptr; - protected: - /** the rows can be weighted independently accordint to the error - * or uniformly with the norm of the right hand side */ - ReweightScheme reweight_; + protected: + /** the rows can be weighted independently according to the error + * or uniformly with the norm of the right hand side */ + ReweightScheme reweight_; - public: - Base(): reweight_(Block) {} - Base(const ReweightScheme reweight):reweight_(reweight) {} - virtual ~Base() {} + public: + Base(): reweight_(Block) {} + Base(const ReweightScheme reweight):reweight_(reweight) {} + virtual ~Base() {} - /// robust error function to implement - virtual double weight(const double &error) const = 0; + /// robust error function to implement + virtual double weight(const double &error) const = 0; - virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + virtual void print(const std::string &s) const = 0; + virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; - inline double sqrtWeight(const double &error) const - { return std::sqrt(weight(error)); } + inline double sqrtWeight(const double &error) const + { return std::sqrt(weight(error)); } - /** produce a weight vector according to an error vector and the implemented - * robust function */ - Vector weight(const Vector &error) const; + /** produce a weight vector according to an error vector and the implemented + * robust function */ + Vector weight(const Vector &error) const; - /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const; + /** square root version of the weight function */ + Vector sqrtWeight(const Vector &error) const; - /** reweight block matrices and a vector according to their weight implementation */ - void reweight(std::vector &A, Vector &error) const; - void reweight(Matrix &A, Vector &error) const; - void reweight(Matrix &A1, Matrix &A2, Vector &error) const; - void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; - }; + /** reweight block matrices and a vector according to their weight implementation */ + void reweight(Vector &error) const; + void reweight(std::vector &A, Vector &error) const; + void reweight(Matrix &A, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; + }; - /// Null class is not robust so is a Gaussian ? - class Null : public Base { - public: - typedef boost::shared_ptr shared_ptr; - Null(const ReweightScheme reweight = Block) : Base(reweight) {} - virtual ~Null() {} - virtual double weight(const double &error) const { return 1.0; } - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } - static shared_ptr Create() ; - }; + /// Null class is not robust so is a Gaussian ? + class Null : public Base { + public: + typedef boost::shared_ptr shared_ptr; + Null(const ReweightScheme reweight = Block) : Base(reweight) {} + virtual ~Null() {} + virtual double weight(const double &error) const { return 1.0; } + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } + static shared_ptr Create() ; + }; - /// Fair implements the "Fair" robust error model (Zhang97ivc) - class Fair : public Base { - public: - typedef boost::shared_ptr shared_ptr; - protected: - double c_; - public: - Fair(const double c, const ReweightScheme reweight = Block); - virtual ~Fair() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; - private: - Fair(){} - }; + /// Fair implements the "Fair" robust error model (Zhang97ivc) + class Fair : public Base { + public: + typedef boost::shared_ptr shared_ptr; + protected: + double c_; + public: + Fair(const double c, const ReweightScheme reweight = Block); + virtual ~Fair() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; + private: + Fair(){} + }; - /// Huber implements the "Huber" robust error model (Zhang97ivc) - class Huber : public Base { - public: - typedef boost::shared_ptr shared_ptr; - protected: - double k_; - public: - Huber(const double k, const ReweightScheme reweight = Block); - virtual ~Huber() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; - private: - Huber(){} - }; + /// Huber implements the "Huber" robust error model (Zhang97ivc) + class Huber : public Base { + public: + typedef boost::shared_ptr shared_ptr; + protected: + double k_; + public: + Huber(const double k, const ReweightScheme reweight = Block); + virtual ~Huber() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + private: + Huber(){} + }; - } ///\namespace MEstimator + } ///\namespace MEstimator /// Base class for robust error models class Robust : public Base { @@ -692,17 +693,16 @@ namespace gtsam { /// Return the contained noise model const NoiseModel::shared_ptr& noise() const { return noise_; } - // TODO: all function below are dummy but necessary for the noiseModel::Base - + // TODO: functions below are dummy but necessary for the noiseModel::Base inline virtual Vector whiten(const Vector& v) const - { return noise_->whiten(v); } + { Vector r = v; this->WhitenSystem(r); return r; } inline virtual Vector unwhiten(const Vector& v) const - { return noise_->unwhiten(v); } + { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const - { return noise_->distance(v); } + { throw std::invalid_argument("distance is not currently supported for robust noise models."); } // TODO: these are really robust iterated re-weighting support functions - + virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; virtual void WhitenSystem(Matrix& A, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; From f6ef1e1d9d828e7bf5f3e7d8bc17920e1b85f0af Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 27 Sep 2012 19:06:30 +0000 Subject: [PATCH 059/107] added Cheirality exception as in ProjectionFactor --- gtsam/geometry/StereoCamera.cpp | 2 ++ gtsam/geometry/StereoCamera.h | 8 +++++++- gtsam/slam/StereoFactor.h | 16 +++++++++++----- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index c6eca2780..1fe86485b 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,6 +39,8 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); #endif + if ( q.z() <= 0 ) throw StereoCheiralityException(); + // get calibration const Cal3_S2Stereo& K = *K_; const double fx = K.fx(), fy = K.fy(), b = K.baseline(); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 140ab1bdf..b178b9b60 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -11,7 +11,7 @@ /** * @file StereoCamera.h - * @brief A Stereo Camera based on two Simple Cameras + * @brief A Rectified Stereo Camera * @author Chris Beall */ @@ -26,6 +26,12 @@ namespace gtsam { +class StereoCheiralityException: public std::runtime_error { +public: + StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} +}; + + /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 87cb95d25..b00a2c499 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -88,8 +88,16 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - StereoCamera stereoCam(pose, K_); - return (stereoCam.project(point, H1, H2) - measured_).vector(); + try { + StereoCamera stereoCam(pose, K_); + return (stereoCam.project(point, H1, H2) - measured_).vector(); + } catch(StereoCheiralityException& e) { + if (H1) *H1 = zeros(3,6); + if (H2) *H2 = zeros(3,3); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + } + return ones(3) * 2.0 * K_->fx(); } /** return the measured */ @@ -102,7 +110,6 @@ public: return K_; } - private: /** Serialization function */ friend class boost::serialization::access; @@ -115,5 +122,4 @@ private: } }; - -} +} // \ namespace gtsam From 784e9f575571ed5acd95df21e24fa332e3935e38 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 28 Sep 2012 17:48:49 +0000 Subject: [PATCH 060/107] Added overall timing scripts for batch and incremental (ISAM2) solving --- tests/timeBatch.cpp | 54 +++++++++++++++ tests/timeIncremental.cpp | 140 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 194 insertions(+) create mode 100644 tests/timeBatch.cpp create mode 100644 tests/timeIncremental.cpp diff --git a/tests/timeBatch.cpp b/tests/timeBatch.cpp new file mode 100644 index 000000000..0548d7f94 --- /dev/null +++ b/tests/timeBatch.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file timeBatch.cpp + * @brief Overall timing tests for batch solving + * @author Richard Roberts + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + cout << "Loading data..." << endl; + + string datasetFile = findExampleDataFile("w10000-odom"); + std::pair data = + load2D(datasetFile); + + NonlinearFactorGraph graph = *data.first; + Values initial = *data.second; + + cout << "Optimizing..." << endl; + + tic_(1, "Create optimizer"); + LevenbergMarquardtOptimizer optimizer(graph, initial); + toc_(1, "Create optimizer"); + tictoc_print_(); + double lastError = optimizer.error(); + do { + tic_(2, "Iterate optimizer"); + optimizer.iterate(); + toc_(2, "Iterate optimizer"); + tictoc_finishedIteration_(); + tictoc_print_(); + cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl; + } while(!checkConvergence(optimizer.params().relativeErrorTol, + optimizer.params().absoluteErrorTol, optimizer.params().errorTol, + lastError, optimizer.error(), optimizer.params().verbosity)); + + return 0; +} diff --git a/tests/timeIncremental.cpp b/tests/timeIncremental.cpp new file mode 100644 index 000000000..333d0e777 --- /dev/null +++ b/tests/timeIncremental.cpp @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file timeIncremental.cpp + * @brief Overall timing tests for incremental solving + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +typedef Pose2 Pose; + +double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { + // Compute degrees of freedom (observations - variables) + // In ocaml, +1 was added to the observations to account for the prior, but + // the factor graph already includes a factor for the prior/equality constraint. + // double dof = graph.size() - config.size(); + int graph_dim = 0; + BOOST_FOREACH(const boost::shared_ptr& nlf, graph) { + graph_dim += nlf->dim(); + } + double dof = graph_dim - config.dim(); // kaess: changed to dim + return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error +} + +int main(int argc, char *argv[]) { + + cout << "Loading data..." << endl; + + tic_(1, "Find datafile (script only)"); + string datasetFile = findExampleDataFile("w10000-odom"); + std::pair data = + load2D(datasetFile); + toc_(1, "Find datafile (script only)"); + + NonlinearFactorGraph measurements = *data.first; + Values initial = *data.second; + + cout << "Playing forward time steps..." << endl; + + ISAM2 isam2; + + size_t nextMeasurement = 0; + for(size_t step=1; nextMeasurement < measurements.size(); ++step) { + + Values newVariables; + NonlinearFactorGraph newFactors; + + // Collect measurements and new variables for the current step + tic_(2, "Collect measurements (script only)"); + if(step == 1) { + // cout << "Initializing " << 0 << endl; + newVariables.insert(0, Pose()); + // Add prior + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + } + while(nextMeasurement < measurements.size()) { + + NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement]; + + if(BetweenFactor::shared_ptr measurement = + boost::dynamic_pointer_cast >(measurementf)) + { + // Stop collecting measurements that are for future steps + if(measurement->key1() > step || measurement->key2() > step) + break; + + // Require that one of the nodes is the current one + if(measurement->key1() != step && measurement->key2() != step) + throw runtime_error("Problem in data file, out-of-sequence measurements"); + + // Add a new factor + newFactors.push_back(measurement); + + // Initialize the new variable + if(measurement->key1() == step && measurement->key2() == step-1) { + if(step == 1) + newVariables.insert(step, measurement->measured().inverse()); + else { + Pose prevPose = isam2.calculateEstimate(step-1); + newVariables.insert(step, prevPose * measurement->measured().inverse()); + } + // cout << "Initializing " << step << endl; + } else if(measurement->key2() == step && measurement->key1() == step-1) { + if(step == 1) + newVariables.insert(step, measurement->measured()); + else { + Pose prevPose = isam2.calculateEstimate(step-1); + newVariables.insert(step, prevPose * measurement->measured()); + } + // cout << "Initializing " << step << endl; + } + } else { + throw std::runtime_error("Unknown factor type read from data file"); + } + ++ nextMeasurement; + } + toc_(2, "Collect measurements (script only)"); + + // Update iSAM2 + tic_(3, "Update ISAM2"); + isam2.update(newFactors, newVariables); + toc_(3, "Update ISAM2"); + + if(step % 20 == 0) { + tic_(4, "chi2 (script only)"); + Values estimate(isam2.calculateEstimate()); + double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate); + cout << "chi2 = " << chi2 << endl; + toc_(4, "chi2 (script only)"); + } + + if(step % 100 == 0) { + cout << "Step " << step << endl; + tictoc_print_(); + } + + tictoc_finishedIteration_(); + } + + return 0; +} From d7af0b9b5b64c66f98880e3341800397120e2712 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 1 Oct 2012 16:12:34 +0000 Subject: [PATCH 061/107] Renamed arguments for consistency --- gtsam/nonlinear/ISAM2-impl.cpp | 14 +++++++------- gtsam/nonlinear/ISAM2-impl.h | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index f3b27b138..dd4b6675b 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector& replacedKeys, + VectorValues& deltaNewton, VectorValues& RgProd, vector& replacedKeys, Ordering& ordering, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -46,8 +46,8 @@ void ISAM2::Impl::AddVariables( delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); deltaNewton.append(dims); deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - deltaGradSearch.append(dims); - deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + RgProd.append(dims); + RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { @@ -64,7 +64,7 @@ void ISAM2::Impl::AddVariables( /* ************************************************************************* */ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, + VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, GaussianFactorGraph& linearFactors) { @@ -101,7 +101,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli for(size_t j = 0; j < dims.size(); ++j) { newDelta[j] = delta[unusedToEnd[j]]; newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; - newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]]; + newDeltaGradSearch[j] = RgProd[unusedToEnd[j]]; newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; newNodes[j] = nodes[unusedToEnd[j]]; } @@ -109,7 +109,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli // Swap the new data structures with the outputs of this function delta.swap(newDelta); deltaNewton.swap(newDeltaNewton); - deltaGradSearch.swap(newDeltaGradSearch); + RgProd.swap(newDeltaGradSearch); replacedKeys.swap(newReplacedKeys); nodes.swap(newNodes); } @@ -438,7 +438,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { -void updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, +void updateDoglegDeltas(const boost::shared_ptr& clique, const std::vector& replacedKeys, const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index b873c87bf..4f3ab7cd9 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -47,7 +47,7 @@ struct ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, + VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -55,7 +55,7 @@ struct ISAM2::Impl { */ static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, GaussianFactorGraph& linearFactors); /** From fb409a2cc7d89179e1b0a73bdbe40122c2982f3f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 1 Oct 2012 16:12:41 +0000 Subject: [PATCH 062/107] Implemented partial elimination and sparse variable index remapping (Reduction) to enable Frank's new marginal code --- gtsam/discrete/DecisionTreeFactor.h | 8 +++ gtsam/discrete/DiscreteFactor.h | 7 ++ gtsam/discrete/DiscreteFactorGraph.cpp | 18 +++++ gtsam/discrete/DiscreteFactorGraph.h | 6 ++ gtsam/discrete/Potentials.cpp | 49 ++++++++------ gtsam/discrete/Potentials.h | 9 +++ gtsam/inference/BayesTree-inl.h | 4 ++ gtsam/inference/BayesTreeCliqueBase-inl.h | 35 ++++++++-- gtsam/inference/EliminationTree-inl.h | 66 ++++++++++++------- gtsam/inference/EliminationTree.h | 7 ++ gtsam/inference/GenericSequentialSolver-inl.h | 25 +++---- gtsam/inference/GenericSequentialSolver.h | 10 +-- gtsam/inference/IndexFactor.cpp | 8 +++ gtsam/inference/IndexFactor.h | 5 ++ gtsam/inference/Permutation.cpp | 60 +++++++++++++++++ gtsam/inference/Permutation.h | 18 +++++ gtsam/inference/SymbolicFactorGraph.cpp | 18 +++++ gtsam/inference/SymbolicFactorGraph.h | 8 ++- .../inference/tests/testSymbolicBayesTree.cpp | 3 - .../tests/testSymbolicSequentialSolver.cpp | 21 ------ gtsam/linear/GaussianFactor.cpp | 3 +- gtsam/linear/GaussianFactor.h | 7 ++ gtsam/linear/GaussianFactorGraph.cpp | 9 +++ gtsam/linear/GaussianFactorGraph.h | 3 + 24 files changed, 309 insertions(+), 98 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 64f37c174..f75c658a2 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -139,6 +139,14 @@ namespace gtsam { DiscreteFactor::permuteWithInverse(inversePermutation); Potentials::permuteWithInverse(inversePermutation); } + + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + DiscreteFactor::reduceWithInverse(inverseReduction); + Potentials::reduceWithInverse(inverseReduction); + } /// @} }; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 9b1130bae..2f411997c 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -108,6 +108,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + IndexFactor::reduceWithInverse(inverseReduction); + } + /// @} }; // DiscreteFactor diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index f3c89721b..467611c5d 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -74,6 +74,24 @@ namespace gtsam { if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); } } + + /* ************************************************************************* */ + void DiscreteFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void DiscreteFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } /* ************************************************************************* */ std::pair // diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 0f399abdd..323adca7b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -79,6 +79,12 @@ public: /// print void print(const std::string& s = "DiscreteFactorGraph", const IndexFormatter& formatter =DefaultIndexFormatter) const; + + /** Permute the variables in the factors */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); }; // DiscreteFactorGraph diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index 2a2260d97..cf903a3a2 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -60,28 +60,39 @@ namespace gtsam { ADT::print(" "); } + /* ************************************************************************* */ + template + void Potentials::remapIndices(const P& remapping) { + // Permute the _cardinalities (TODO: Inefficient Consider Improving) + DiscreteKeys keys; + map ordering; + + // Get the original keys from cardinalities_ + BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + keys & key; + + // Perform Permutation + BOOST_FOREACH(DiscreteKey& key, keys) { + ordering[key.first] = remapping[key.first]; + key.first = ordering[key.first]; + } + + // Change *this + AlgebraicDecisionTree permuted((*this), ordering); + *this = permuted; + cardinalities_ = keys.cardinalities(); + } + /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& permutation) { - // Permute the _cardinalities (TODO: Inefficient Consider Improving) - DiscreteKeys keys; - map ordering; - - // Get the orginal keys from cardinalities_ - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - keys & key; - - // Perform Permutation - BOOST_FOREACH(DiscreteKey& key, keys) { - ordering[key.first] = permutation[key.first]; - key.first = ordering[key.first]; - } - - // Change *this - AlgebraicDecisionTree permuted((*this), ordering); - *this = permuted; - cardinalities_ = keys.cardinalities(); + void Potentials::permuteWithInverse(const Permutation& inversePermutation) { + remapIndices(inversePermutation); } + /* ************************************************************************* */ + void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { + remapIndices(inverseReduction); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 3ca222b5f..5d8ace371 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -49,6 +49,10 @@ namespace gtsam { // Safe division for probabilities static double safe_div(const double& a, const double& b); + // Apply either a permutation or a reduction + template + void remapIndices(const P& remapping); + public: /** Default constructor for I/O */ @@ -79,6 +83,11 @@ namespace gtsam { */ virtual void permuteWithInverse(const Permutation& inversePermutation); + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction); + }; // Potentials } // namespace gtsam diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index bf3b7f37c..fd37c4a4c 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -498,7 +498,11 @@ namespace gtsam { sharedClique clique = (*this)[j]; // calculate or retrieve its marginal P(C) = P(F,S) +#ifdef MARGINAL2 FactorGraph cliqueMarginal = clique->marginal2(root_,function); +#else + FactorGraph cliqueMarginal = clique->marginal(root_,function); +#endif // now, marginalize out everything that is not variable j GenericSequentialSolver solver(cliqueMarginal); diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 639e27202..6f090b587 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -17,6 +17,7 @@ #pragma once #include +#include namespace gtsam { @@ -53,8 +54,10 @@ namespace gtsam { std::vector &indicesB = B->conditional()->keys(); std::vector S_setminus_B = separator_setminus_B(B); // TODO, get as argument? std::vector keep; + // keep = S\B intersect allKeys std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // allKeys.begin(), allKeys.end(), back_inserter(keep)); + // keep += B intersect allKeys std::set_intersection(indicesB.begin(), indicesB.end(), // allKeys.begin(), allKeys.end(), back_inserter(keep)); // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl; @@ -190,18 +193,42 @@ namespace gtsam { // systems and exceptions are thrown. However, we should be able to omit // this if we can get ATTEMPT_AT_NOT_ELIMINATING_ALL in // GenericSequentialSolver.* working... +#ifndef ATTEMPT_AT_NOT_ELIMINATING_ALL p_Cp_B.push_back(B->conditional()->toFactor()); // P(B) +#endif + + // Determine the variables we want to keepSet, S union B + std::vector keep = shortcut_indices(B, p_Cp_B); + //std::set keepSet; + //BOOST_FOREACH(Index j, this->conditional()->parents()) { + // keepSet.insert(j); } + //BOOST_FOREACH(Index j, (*B->conditional())) { + // keepSet.insert(j); } + //std::vector keep(keepSet.begin(), keepSet.end()); + + // Reduce the variable indices to start at zero + const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); + internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { + if(factor) factor->reduceWithInverse(inverseReduction); } + inverseReduction.applyInverse(keep); // Create solver that will marginalize for us GenericSequentialSolver solver(p_Cp_B); - // Determine the variables we want to keep - std::vector keep = shortcut_indices(B, p_Cp_B); - // Finally, we only want to have S\B variables in the Bayes net, so size_t nrFrontals = S_setminus_B.size(); cachedShortcut_ = // *solver.conditionalBayesNet(keep, nrFrontals, function); + + // Undo the reduction + BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { + if (factor) { + factor->permuteWithInverse(reduction); + } + } + cachedShortcut_->permuteWithInverse(reduction); + assertInvariants(); } else { BayesNet empty; @@ -264,7 +291,7 @@ namespace gtsam { // Create solver that will marginalize for us GenericSequentialSolver solver(p_Cp); - // The variables we want to keep are exactly the ones in S + // The variables we want to keepSet are exactly the ones in S sharedConditional p_F_S = this->conditional(); std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 2e977aa4b..f2c16ac14 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -35,34 +35,42 @@ namespace gtsam { /* ************************************************************************* */ template typename EliminationTree::sharedFactor EliminationTree::eliminate_( - Eliminate function, Conditionals& conditionals) const { + Eliminate function, Conditionals& conditionals) const { - static const bool debug = false; + static const bool debug = false; - if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; + if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; - // Create the list of factors to be eliminated, initially empty, and reserve space - FactorGraph factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); + if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable + // Create the list of factors to be eliminated, initially empty, and reserve space + FactorGraph factors; + factors.reserve(this->factors_.size() + this->subTrees_.size()); - // Add all factors associated with the current node - factors.push_back(this->factors_.begin(), this->factors_.end()); + // Add all factors associated with the current node + factors.push_back(this->factors_.begin(), this->factors_.end()); - // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) - factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread - // TODO: wait for completion of all threads + // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] + BOOST_FOREACH(const shared_ptr& child, subTrees_) + factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread + // TODO: wait for completion of all threads - // Combine all factors (from this node and from subtrees) into a joint factor - typename FactorGraph::EliminationResult - eliminated(function(factors, 1)); - conditionals[this->key_] = eliminated.first; + // Combine all factors (from this node and from subtrees) into a joint factor + typename FactorGraph::EliminationResult + eliminated(function(factors, 1)); + conditionals[this->key_] = eliminated.first; - if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n"; - if(debug) eliminated.first->print("Conditional: "); - if(debug) eliminated.second->print("Factor: "); + if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n"; + if(debug) eliminated.first->print("Conditional: "); + if(debug) eliminated.second->print("Factor: "); - return eliminated.second; + return eliminated.second; + } else { + // Eliminate each child but discard the result. + BOOST_FOREACH(const shared_ptr& child, subTrees_) { + (void)child->eliminate_(function, conditionals); + } + return sharedFactor(); // Return a NULL factor + } } /* ************************************************************************* */ @@ -138,7 +146,8 @@ typename EliminationTree::shared_ptr EliminationTree::Create( if(derivedFactor) { sharedFactor factor(derivedFactor); Index j = *std::min_element(factor->begin(), factor->end()); - trees[j]->add(factor); + if(j < structure.size()) + trees[j]->add(factor); } } toc(3, "hang factors"); @@ -193,12 +202,13 @@ bool EliminationTree::equals(const EliminationTree& ex /* ************************************************************************* */ template typename EliminationTree::BayesNet::shared_ptr -EliminationTree::eliminate(Eliminate function) const { + EliminationTree::eliminatePartial(typename EliminationTree::Eliminate function, size_t nrToEliminate) const { // call recursive routine tic(1, "ET recursive eliminate"); - size_t nrConditionals = this->key_ + 1; // root key has highest index - Conditionals conditionals(nrConditionals); // reserve a vector of conditional shared pointers + if(nrToEliminate > this->key_ + 1) + throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist"); + Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers (void)eliminate_(function, conditionals); // modify in place toc(1, "ET recursive eliminate"); @@ -214,4 +224,12 @@ EliminationTree::eliminate(Eliminate function) const { return bayesNet; } +/* ************************************************************************* */ +template +typename EliminationTree::BayesNet::shared_ptr +EliminationTree::eliminate(Eliminate function) const { + size_t nrConditionals = this->key_ + 1; // root key has highest index + return eliminatePartial(function, nrConditionals); +} + } diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index f2b0c1eaf..62540c1fe 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -110,6 +110,13 @@ public: */ typename BayesNet::shared_ptr eliminate(Eliminate function) const; + /** Eliminate the factors to a Bayes Net and return the remaining factor + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The BayesNet resulting from elimination and the remaining factor + */ + typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const; + /// @} /// @name Testable /// @{ diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 7800f4adb..171b2f078 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,11 +89,7 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - , boost::optional nrToEliminate -#endif - ) const { + Eliminate function, boost::optional nrToEliminate) const { // Create inverse permutation Permutation::shared_ptr permutationInverse(permutation.inverse()); @@ -106,15 +102,12 @@ namespace gtsam { factor->permuteWithInverse(*permutationInverse); // Eliminate using elimination tree provided - typename EliminationTree::shared_ptr etree; -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - if (nrToEliminate) { - VariableIndex structure(*factors_, *nrToEliminate); - etree = EliminationTree::Create(*factors_, structure); - } else -#endif - etree = EliminationTree::Create(*factors_); - sharedBayesNet bayesNet = etree->eliminate(function); + typename EliminationTree::shared_ptr etree = EliminationTree::Create(*factors_); + sharedBayesNet bayesNet; + if(nrToEliminate) + bayesNet = etree->eliminatePartial(function, *nrToEliminate); + else + bayesNet = etree->eliminate(function); // Undo the permutation on the original factors and on the structure. BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) @@ -147,13 +140,13 @@ namespace gtsam { // my trick below (passing nrToEliminate to eliminate) sometimes leads // to a disconnected graph. // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = factors_->keys().size();// TODO expensive! + size_t nrVariables = structure_->size(); size_t nrMarginalized = nrVariables - js.size(); size_t nrToEliminate = nrMarginalized + nrFrontals; sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); // Get rid of conditionals on variables that we want to marginalize out for (int i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); + bayesNet->pop_front(); #else // Eliminate all variables sharedBayesNet fullBayesNet = eliminate(*permutation, function); diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 6f0bf3b5b..fffcb209f 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -88,13 +88,9 @@ namespace gtsam { /** * Eliminate in a different order, given a permutation */ - sharedBayesNet - eliminate(const Permutation& permutation, Eliminate function -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - , boost::optional nrToEliminate = boost::none - // If given a number of variables to eliminate, will only eliminate that many -#endif - ) const; + sharedBayesNet eliminate(const Permutation& permutation, Eliminate function, + boost::optional nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many + ) const; public: diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index b04338d5b..306defce4 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -61,5 +61,13 @@ namespace gtsam { key = inversePermutation[key]; assertInvariants(); } + /* ************************************************************************* */ + void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) { + BOOST_FOREACH(Index& key, keys()) + key = inverseReduction[key]; + assertInvariants(); + } + + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 8ab5a880a..7d0fdcc4f 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -156,6 +156,11 @@ namespace gtsam { */ void permuteWithInverse(const Permutation& inversePermutation); + /** + * Apply a reduction, which is a remapping of variable indices. + */ + void reduceWithInverse(const internal::Reduction& inverseReduction); + virtual ~IndexFactor() { } diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index 5bcb1fef8..f94b4e116 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -152,4 +152,64 @@ void Permutation::print(const std::string& str) const { std::cout << std::endl; } +namespace internal { +/* ************************************************************************* */ + Reduction Reduction::CreateAsInverse(const Permutation& p) { + Reduction result; + for(Index j = 0; j < p.size(); ++j) + result.insert(std::make_pair(p[j], j)); + return result; + } + + /* ************************************************************************* */ + void Reduction::applyInverse(std::vector& js) const { + BOOST_FOREACH(Index& j, js) { + j = this->find(j)->second; + } + } + + /* ************************************************************************* */ + Permutation Reduction::inverse() const { + Index maxIndex = 0; + BOOST_FOREACH(const value_type& target_source, *this) { + if(target_source.second > maxIndex) + maxIndex = target_source.second; + } + Permutation result(maxIndex + 1); + BOOST_FOREACH(const value_type& target_source, *this) { + result[target_source.second] = target_source.first; + } + return result; + } + + /* ************************************************************************* */ + Index& Reduction::operator[](const Index& j) { + iterator it = this->find(j); + if(it == this->end()) + throw std::out_of_range("Index to Reduction::operator[] not present"); + else + return it->second; + } + + /* ************************************************************************* */ + const Index& Reduction::operator[](const Index& j) const { + const_iterator it = this->find(j); + if(it == this->end()) + throw std::out_of_range("Index to Reduction::operator[] not present"); + else + return it->second; + } + + /* ************************************************************************* */ + Permutation createReducingPermutation(const std::set& indices) { + Permutation p(indices.size()); + Index newJ = 0; + BOOST_FOREACH(Index j, indices) { + p[newJ] = j; + ++ newJ; + } + return p; + } +} + } diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 4e32b1f52..deec7367c 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -19,10 +19,12 @@ #include #include +#include #include #include #include +#include namespace gtsam { @@ -182,4 +184,20 @@ protected: /// @} }; + +namespace internal { + // An internal class used for storing and applying a permutation from a map + class Reduction : public gtsam::FastMap { + public: + static Reduction CreateAsInverse(const Permutation& p); + void applyInverse(std::vector& js) const; + Permutation inverse() const; + Index& operator[](const Index& j); + const Index& operator[](const Index& j) const; + }; + + // Reduce the variable indices so that those in the set are mapped to start at zero + Permutation createReducingPermutation(const std::set& indices); +} + } diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index ff7a91ca6..70da5a4c1 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -69,6 +69,24 @@ namespace gtsam { { return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); } + + /* ************************************************************************* */ + void SymbolicFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } /* ************************************************************************* */ IndexFactor::shared_ptr CombineSymbolic( diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h index 1b20ac239..d18c3ba6f 100644 --- a/gtsam/inference/SymbolicFactorGraph.h +++ b/gtsam/inference/SymbolicFactorGraph.h @@ -77,8 +77,6 @@ namespace gtsam { */ FastSet keys() const; - - /// @} /// @name Advanced Interface /// @{ @@ -94,6 +92,12 @@ namespace gtsam { /** Push back 4-way factor */ void push_factor(Index key1, Index key2, Index key3, Index key4); + + /** Permute the variables in the factors */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); }; diff --git a/gtsam/inference/tests/testSymbolicBayesTree.cpp b/gtsam/inference/tests/testSymbolicBayesTree.cpp index b69bdac5e..68693817b 100644 --- a/gtsam/inference/tests/testSymbolicBayesTree.cpp +++ b/gtsam/inference/tests/testSymbolicBayesTree.cpp @@ -30,9 +30,6 @@ using namespace gtsam; static bool debug = false; -typedef BayesNet SymbolicBayesNet; -typedef BayesTree SymbolicBayesTree; - /* ************************************************************************* */ TEST_UNSAFE( SymbolicBayesTree, thinTree ) { diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp index ef6f415e1..c017083a2 100644 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -122,27 +122,6 @@ TEST( SymbolicSequentialSolver, inference ) { } } -/* ************************************************************************* */ -// This test shows a problem with my (Frank) attempt at a faster conditionalBayesNet -TEST( SymbolicSequentialSolver, problematicConditional ) { - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(9, 12, 14); - - // eliminate - SymbolicSequentialSolver solver(fg); - // conditionalBayesNet - vector js; - js.push_back(12); - js.push_back(14); - size_t nrFrontals = 1; - SymbolicBayesNet::shared_ptr actualBN = // - solver.conditionalBayesNet(js, nrFrontals); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(12,14)); - EXPECT( assert_equal(expectedBN,*actualBN)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 72bd1df72..0ad35c2e3 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -25,7 +25,6 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactor::GaussianFactor(const GaussianConditional& c) : - IndexFactor(c) { - } + IndexFactor(c) {} } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 0d435ce17..efe6f2036 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -115,6 +115,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + IndexFactor::reduceWithInverse(inverseReduction); + } + /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 222f025a0..5ba150601 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -63,6 +63,15 @@ namespace gtsam { } } + /* ************************************************************************* */ + void GaussianFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } + /* ************************************************************************* */ void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { for (const_iterator factor = lfg.factors_.begin(); factor diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5e6e792be..811623947 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -127,6 +127,9 @@ namespace gtsam { /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); + /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; From 96ce28625bfb43b9ba6d5699b7356da3d9dde0c1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 1 Oct 2012 16:12:43 +0000 Subject: [PATCH 063/107] Timing marginals in timeIncremental --- tests/timeIncremental.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/tests/timeIncremental.cpp b/tests/timeIncremental.cpp index 333d0e777..422607c05 100644 --- a/tests/timeIncremental.cpp +++ b/tests/timeIncremental.cpp @@ -21,6 +21,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -120,7 +121,7 @@ int main(int argc, char *argv[]) { isam2.update(newFactors, newVariables); toc_(3, "Update ISAM2"); - if(step % 20 == 0) { + if(step % 100 == 0) { tic_(4, "chi2 (script only)"); Values estimate(isam2.calculateEstimate()); double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate); @@ -128,12 +129,25 @@ int main(int argc, char *argv[]) { toc_(4, "chi2 (script only)"); } - if(step % 100 == 0) { + tictoc_finishedIteration_(); + + if(step % 1000 == 0) { cout << "Step " << step << endl; tictoc_print_(); } + } + // Compute marginals + Marginals marginals(isam2.getFactorsUnsafe(), isam2.calculateEstimate()); + int i=0; + BOOST_FOREACH(Key key, initial.keys()) { + tic_(5, "marginalInformation"); + Matrix info = marginals.marginalInformation(key); + toc_(5, "marginalInformation"); tictoc_finishedIteration_(); + if(i % 1000 == 0) + tictoc_print_(); + ++i; } return 0; From 4297d24c968e477c2c9958aba2d256af38a4a976 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 2 Oct 2012 14:40:07 +0000 Subject: [PATCH 064/107] changed tabs to spaces for consistent indentation in all of GTSAM --- CppUnitLite/Failure.h | 68 +- CppUnitLite/Test.cpp | 54 +- CppUnitLite/Test.h | 54 +- CppUnitLite/TestRegistry.cpp | 72 +- CppUnitLite/TestRegistry.h | 16 +- CppUnitLite/TestResult.cpp | 40 +- CppUnitLite/TestResult.h | 10 +- examples/CameraResectioning.cpp | 94 +- examples/DiscreteBayesNet_FG.cpp | 144 +- examples/PlanarSLAMExample.cpp | 98 +- examples/Pose2SLAMExample.cpp | 82 +- examples/Pose2SLAMExample_graph.cpp | 34 +- examples/SimpleRotation.cpp | 122 +- examples/UGM_chain.cpp | 94 +- examples/UGM_small.cpp | 90 +- examples/elaboratePoint2KalmanFilter.cpp | 12 +- gtsam.h | 1254 ++++----- gtsam/3rdparty/CCOLAMD/Include/ccolamd.h | 150 +- .../3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h | 6 +- .../src/Eigenvalues/ComplexEigenSolver.h | 2 +- .../Eigen/Eigen/src/Eigenvalues/EigenSolver.h | 2 +- .../src/Eigenvalues/HessenbergDecomposition.h | 4 +- .../src/Eigenvalues/MatrixBaseEigenvalues.h | 8 +- .../src/Eigenvalues/Tridiagonalization.h | 4 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 2 +- .../Eigen/Eigen/src/QR/ColPivHouseholderQR.h | 2 +- .../Eigen/src/SuperLUSupport/SuperLUSupport.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h | 340 +-- gtsam/3rdparty/Eigen/bench/benchmarkX.cpp | 24 +- .../3rdparty/Eigen/bench/benchmarkXcwise.cpp | 24 +- .../bench/btl/generic_bench/utils/utilities.h | 30 +- .../3rdparty/Eigen/bench/btl/libs/BLAS/blas.h | 340 +-- .../snippets/TopicStorageOrders_example.cpp | 2 +- .../doc/snippets/Tutorial_commainit_01b.cpp | 2 +- gtsam/3rdparty/Eigen/test/corners.cpp | 4 +- gtsam/3rdparty/Eigen/test/eigen2/main.h | 4 +- gtsam/3rdparty/Eigen/test/hessenberg.cpp | 2 +- gtsam/3rdparty/Eigen/test/schur_complex.cpp | 2 +- .../Eigen/src/IterativeSolvers/GMRES.h | 102 +- .../src/MatrixFunctions/MatrixExponential.h | 6 +- .../src/MatrixFunctions/MatrixFunction.h | 32 +- .../MatrixFunctions/MatrixFunctionAtomic.h | 2 +- .../src/MatrixFunctions/MatrixLogarithm.h | 6 +- .../src/MatrixFunctions/MatrixSquareRoot.h | 38 +- .../Eigen/src/MatrixFunctions/StemFunction.h | 50 +- .../Eigen/src/SparseExtra/MarketIO.h | 6 +- .../unsupported/test/matrix_square_root.cpp | 2 +- .../Eigen/unsupported/test/mpreal/mpreal.cpp | 590 ++-- .../Eigen/unsupported/test/mpreal/mpreal.h | 2490 ++++++++--------- gtsam/3rdparty/UFconfig/UFconfig.h | 40 +- gtsam/base/DSFVector.cpp | 82 +- gtsam/base/DSFVector.h | 78 +- gtsam/base/DerivedValue.h | 66 +- gtsam/base/FastList.h | 2 +- gtsam/base/FastVector.h | 8 +- gtsam/base/Group.h | 22 +- gtsam/base/Lie.h | 62 +- gtsam/base/LieMatrix.h | 132 +- gtsam/base/LieScalar.h | 114 +- gtsam/base/LieVector.h | 136 +- gtsam/base/Manifold.h | 42 +- gtsam/base/Matrix.cpp | 716 ++--- gtsam/base/Matrix.h | 106 +- gtsam/base/Testable.h | 146 +- gtsam/base/TestableAssertions.h | 354 +-- gtsam/base/Value.h | 70 +- gtsam/base/Vector.cpp | 414 +-- gtsam/base/Vector.h | 32 +- gtsam/base/blockMatrices.h | 68 +- gtsam/base/cholesky.cpp | 76 +- gtsam/base/lieProxies.h | 26 +- gtsam/base/numericalDerivative.h | 794 +++--- gtsam/base/tests/testBlockMatrices.cpp | 64 +- gtsam/base/tests/testCholesky.cpp | 68 +- gtsam/base/tests/testDSFVector.cpp | 146 +- gtsam/base/tests/testFastContainers.cpp | 10 +- gtsam/base/tests/testLieMatrix.cpp | 26 +- gtsam/base/tests/testLieScalar.cpp | 16 +- gtsam/base/tests/testLieVector.cpp | 30 +- gtsam/base/tests/testMatrix.cpp | 1326 ++++----- gtsam/base/tests/testTestableAssertions.cpp | 12 +- gtsam/base/tests/testVector.cpp | 130 +- gtsam/base/tests/timeMatrix.cpp | 300 +- gtsam/base/timing.cpp | 198 +- gtsam/base/types.h | 74 +- gtsam/discrete/AlgebraicDecisionTree.h | 206 +- gtsam/discrete/Assignment.h | 116 +- gtsam/discrete/DecisionTree-inl.h | 1078 +++---- gtsam/discrete/DecisionTree.h | 292 +- gtsam/discrete/DecisionTreeFactor.cpp | 122 +- gtsam/discrete/DecisionTreeFactor.h | 182 +- gtsam/discrete/DiscreteBayesNet.cpp | 52 +- gtsam/discrete/DiscreteBayesNet.h | 18 +- gtsam/discrete/DiscreteConditional.cpp | 278 +- gtsam/discrete/DiscreteConditional.h | 174 +- gtsam/discrete/DiscreteFactor.cpp | 6 +- gtsam/discrete/DiscreteFactor.h | 140 +- gtsam/discrete/DiscreteFactorGraph.cpp | 130 +- gtsam/discrete/DiscreteFactorGraph.h | 82 +- gtsam/discrete/DiscreteKey.cpp | 48 +- gtsam/discrete/DiscreteKey.h | 64 +- gtsam/discrete/DiscreteMarginals.h | 64 +- gtsam/discrete/DiscreteSequentialSolver.cpp | 70 +- gtsam/discrete/DiscreteSequentialSolver.h | 146 +- gtsam/discrete/Potentials.cpp | 70 +- gtsam/discrete/Potentials.h | 84 +- gtsam/discrete/Signature.cpp | 338 +-- gtsam/discrete/Signature.h | 170 +- .../tests/testAlgebraicDecisionTree.cpp | 644 ++--- gtsam/discrete/tests/testDecisionTree.cpp | 282 +- .../discrete/tests/testDecisionTreeFactor.cpp | 94 +- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 138 +- .../tests/testDiscreteConditional.cpp | 72 +- gtsam/discrete/tests/testDiscreteFactor.cpp | 4 +- .../tests/testDiscreteFactorGraph.cpp | 284 +- .../discrete/tests/testDiscreteMarginals.cpp | 58 +- gtsam/discrete/tests/testSignature.cpp | 48 +- gtsam/geometry/Cal3Bundler.cpp | 140 +- gtsam/geometry/Cal3Bundler.h | 100 +- gtsam/geometry/Cal3DS2.cpp | 106 +- gtsam/geometry/Cal3DS2.h | 122 +- gtsam/geometry/Cal3_S2.cpp | 88 +- gtsam/geometry/Cal3_S2.h | 210 +- gtsam/geometry/Cal3_S2Stereo.h | 124 +- gtsam/geometry/CalibratedCamera.cpp | 96 +- gtsam/geometry/CalibratedCamera.h | 184 +- gtsam/geometry/PinholeCamera.h | 12 +- gtsam/geometry/Point2.cpp | 6 +- gtsam/geometry/Point2.h | 210 +- gtsam/geometry/Point3.cpp | 40 +- gtsam/geometry/Point3.h | 72 +- gtsam/geometry/Pose2.cpp | 306 +- gtsam/geometry/Pose2.h | 350 +-- gtsam/geometry/Pose3.cpp | 206 +- gtsam/geometry/Pose3.h | 40 +- gtsam/geometry/Rot2.cpp | 74 +- gtsam/geometry/Rot2.h | 340 +-- gtsam/geometry/Rot3.h | 46 +- gtsam/geometry/Rot3M.cpp | 212 +- gtsam/geometry/Rot3Q.cpp | 24 +- gtsam/geometry/SimpleCamera.cpp | 38 +- gtsam/geometry/SimpleCamera.h | 4 +- gtsam/geometry/StereoCamera.cpp | 120 +- gtsam/geometry/StereoCamera.h | 158 +- gtsam/geometry/StereoPoint2.cpp | 2 +- gtsam/geometry/StereoPoint2.h | 170 +- gtsam/geometry/concepts.h | 42 +- gtsam/geometry/tests/testCal3_S2.cpp | 44 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 74 +- gtsam/geometry/tests/testPoint2.cpp | 44 +- gtsam/geometry/tests/testPoint3.cpp | 18 +- gtsam/geometry/tests/testPose2.cpp | 408 +-- gtsam/geometry/tests/testPose3.cpp | 520 ++-- gtsam/geometry/tests/testRot2.cpp | 108 +- gtsam/geometry/tests/testRot3M.cpp | 476 ++-- gtsam/geometry/tests/testRot3Q.cpp | 442 +-- gtsam/geometry/tests/testSimpleCamera.cpp | 100 +- gtsam/geometry/tests/testStereoCamera.cpp | 100 +- gtsam/geometry/tests/timeCalibratedCamera.cpp | 120 +- gtsam/geometry/tests/timePose3.cpp | 20 +- gtsam/geometry/tests/timeRot3.cpp | 22 +- gtsam/geometry/tests/timeStereoCamera.cpp | 38 +- gtsam/inference/BayesNet-inl.h | 44 +- gtsam/inference/BayesNet.h | 40 +- gtsam/inference/BayesTree-inl.h | 926 +++--- gtsam/inference/BayesTree.h | 420 +-- gtsam/inference/BayesTreeCliqueBase-inl.h | 28 +- gtsam/inference/BayesTreeCliqueBase.h | 10 +- gtsam/inference/ClusterTree-inl.h | 118 +- gtsam/inference/ClusterTree.h | 132 +- gtsam/inference/EliminationTree-inl.h | 6 +- gtsam/inference/EliminationTree.h | 36 +- gtsam/inference/Factor-inl.h | 146 +- gtsam/inference/Factor.h | 68 +- gtsam/inference/FactorGraph-inl.h | 224 +- gtsam/inference/FactorGraph.h | 278 +- .../inference/GenericMultifrontalSolver-inl.h | 110 +- gtsam/inference/GenericMultifrontalSolver.h | 136 +- gtsam/inference/ISAM-inl.h | 10 +- gtsam/inference/ISAM.h | 72 +- gtsam/inference/IndexConditional.cpp | 4 +- gtsam/inference/IndexConditional.h | 58 +- gtsam/inference/IndexFactor.cpp | 64 +- gtsam/inference/IndexFactor.h | 202 +- gtsam/inference/JunctionTree-inl.h | 38 +- gtsam/inference/JunctionTree.h | 154 +- gtsam/inference/Permutation.h | 54 +- gtsam/inference/SymbolicFactorGraph.cpp | 62 +- gtsam/inference/SymbolicFactorGraph.h | 140 +- gtsam/inference/VariableIndex.cpp | 22 +- gtsam/inference/VariableIndex.h | 66 +- gtsam/inference/VariableSlots.h | 12 +- gtsam/inference/graph-inl.h | 318 +-- gtsam/inference/graph.h | 128 +- gtsam/inference/inference-inl.h | 24 +- gtsam/inference/inference.h | 144 +- gtsam/inference/tests/testBayesTree.cpp | 540 ++-- gtsam/inference/tests/testClusterTree.cpp | 6 +- gtsam/inference/tests/testConditional.cpp | 12 +- gtsam/inference/tests/testFactorGraph.cpp | 40 +- gtsam/inference/tests/testISAM.cpp | 136 +- gtsam/inference/tests/testJunctionTree.cpp | 36 +- .../inference/tests/testSymbolicBayesNet.cpp | 86 +- gtsam/inference/tests/testSymbolicFactor.cpp | 4 +- .../tests/testSymbolicFactorGraph.cpp | 84 +- .../tests/testSymbolicSequentialSolver.cpp | 2 +- gtsam/inference/tests/testVariableIndex.cpp | 4 +- gtsam/linear/Errors.cpp | 62 +- gtsam/linear/Errors.h | 8 +- gtsam/linear/GaussianBayesNet.cpp | 102 +- gtsam/linear/GaussianBayesNet.h | 76 +- gtsam/linear/GaussianConditional.cpp | 116 +- gtsam/linear/GaussianConditional.h | 206 +- gtsam/linear/GaussianDensity.cpp | 58 +- gtsam/linear/GaussianDensity.h | 68 +- gtsam/linear/GaussianFactor.cpp | 6 +- gtsam/linear/GaussianFactor.h | 28 +- gtsam/linear/GaussianFactorGraph.cpp | 978 +++---- gtsam/linear/GaussianFactorGraph.h | 188 +- gtsam/linear/GaussianISAM.cpp | 16 +- gtsam/linear/GaussianISAM.h | 30 +- gtsam/linear/GaussianJunctionTree.cpp | 28 +- gtsam/linear/GaussianJunctionTree.h | 70 +- gtsam/linear/GaussianMultifrontalSolver.cpp | 38 +- gtsam/linear/GaussianMultifrontalSolver.h | 4 +- gtsam/linear/GaussianSequentialSolver.cpp | 70 +- gtsam/linear/GaussianSequentialSolver.h | 4 +- gtsam/linear/HessianFactor.cpp | 502 ++-- gtsam/linear/HessianFactor.h | 54 +- gtsam/linear/JacobianFactor.cpp | 134 +- gtsam/linear/JacobianFactor.h | 18 +- gtsam/linear/KalmanFilter.cpp | 182 +- gtsam/linear/KalmanFilter.h | 180 +- gtsam/linear/NoiseModel.cpp | 384 +-- gtsam/linear/NoiseModel.h | 868 +++--- gtsam/linear/Sampler.cpp | 44 +- gtsam/linear/Sampler.h | 84 +- gtsam/linear/SubgraphPreconditioner.cpp | 184 +- gtsam/linear/SubgraphPreconditioner.h | 100 +- gtsam/linear/SubgraphSolver.h | 18 +- gtsam/linear/VectorValues.cpp | 78 +- gtsam/linear/VectorValues.h | 28 +- gtsam/linear/iterative-inl.h | 190 +- gtsam/linear/iterative.h | 158 +- gtsam/linear/linearExceptions.h | 158 +- gtsam/linear/tests/testErrors.cpp | 18 +- .../linear/tests/testGaussianConditional.cpp | 186 +- .../linear/tests/testGaussianFactorGraph.cpp | 174 +- gtsam/linear/tests/testHessianFactor.cpp | 94 +- gtsam/linear/tests/testJacobianFactor.cpp | 338 +-- .../linear/tests/testJacobianFactorGraph.cpp | 40 +- gtsam/linear/tests/testKalmanFilter.cpp | 380 +-- gtsam/linear/tests/testNoiseModel.cpp | 308 +- gtsam/linear/tests/testSampler.cpp | 22 +- gtsam/linear/tests/testVectorValues.cpp | 66 +- gtsam/linear/tests/timeGaussianFactor.cpp | 62 +- gtsam/linear/tests/timeSLAMlike.cpp | 2 +- gtsam/nonlinear/DoglegOptimizer.cpp | 26 +- gtsam/nonlinear/DoglegOptimizer.h | 18 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 22 +- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 206 +- gtsam/nonlinear/ExtendedKalmanFilter.h | 102 +- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 2 +- gtsam/nonlinear/GaussNewtonOptimizer.h | 6 +- gtsam/nonlinear/ISAM2-impl.cpp | 108 +- gtsam/nonlinear/ISAM2-impl.h | 10 +- gtsam/nonlinear/ISAM2.cpp | 68 +- gtsam/nonlinear/ISAM2.h | 20 +- gtsam/nonlinear/Key.h | 8 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 32 +- gtsam/nonlinear/Marginals.cpp | 34 +- gtsam/nonlinear/Marginals.h | 30 +- gtsam/nonlinear/NonlinearEquality.h | 434 +-- gtsam/nonlinear/NonlinearFactor.h | 118 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 196 +- gtsam/nonlinear/NonlinearFactorGraph.h | 152 +- gtsam/nonlinear/NonlinearISAM.cpp | 14 +- gtsam/nonlinear/NonlinearISAM.h | 112 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 80 +- gtsam/nonlinear/NonlinearOptimizer.h | 18 +- gtsam/nonlinear/Ordering.cpp | 74 +- gtsam/nonlinear/Ordering.h | 56 +- .../SuccessiveLinearizationOptimizer.cpp | 58 +- .../SuccessiveLinearizationOptimizer.h | 48 +- gtsam/nonlinear/Symbol.cpp | 32 +- gtsam/nonlinear/Values.cpp | 24 +- gtsam/nonlinear/Values.h | 10 +- gtsam/nonlinear/WhiteNoiseFactor.h | 244 +- gtsam/nonlinear/tests/testOrdering.cpp | 70 +- gtsam/nonlinear/tests/testValues.cpp | 142 +- gtsam/slam/AntiFactor.h | 142 +- gtsam/slam/BearingFactor.h | 106 +- gtsam/slam/BearingRangeFactor.h | 152 +- gtsam/slam/BetweenFactor.h | 184 +- gtsam/slam/BoundingConstraint.h | 202 +- gtsam/slam/GeneralSFMFactor.h | 150 +- gtsam/slam/PriorFactor.h | 114 +- gtsam/slam/ProjectionFactor.h | 152 +- gtsam/slam/RangeFactor.h | 104 +- gtsam/slam/StereoFactor.h | 126 +- gtsam/slam/dataset.cpp | 238 +- gtsam/slam/dataset.h | 84 +- gtsam/slam/tests/testAntiFactor.cpp | 28 +- gtsam/slam/tests/testDataset.cpp | 10 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 98 +- gtsam/slam/tests/testProjectionFactor.cpp | 92 +- gtsam/slam/tests/testStereoFactor.cpp | 48 +- gtsam_unstable/base/BTree.h | 644 ++--- gtsam_unstable/base/DSF.h | 236 +- gtsam_unstable/base/FixedVector.h | 136 +- gtsam_unstable/base/tests/testBTree.cpp | 230 +- gtsam_unstable/base/tests/testDSF.cpp | 310 +- gtsam_unstable/base/tests/testFixedVector.cpp | 66 +- gtsam_unstable/discrete/AllDiff.cpp | 174 +- gtsam_unstable/discrete/AllDiff.h | 76 +- gtsam_unstable/discrete/BinaryAllDiff.h | 126 +- gtsam_unstable/discrete/CSP.cpp | 144 +- gtsam_unstable/discrete/CSP.h | 118 +- gtsam_unstable/discrete/Constraint.h | 96 +- gtsam_unstable/discrete/Domain.cpp | 142 +- gtsam_unstable/discrete/Domain.h | 144 +- gtsam_unstable/discrete/Scheduler.cpp | 466 +-- gtsam_unstable/discrete/Scheduler.h | 240 +- gtsam_unstable/discrete/SingleValue.cpp | 108 +- gtsam_unstable/discrete/SingleValue.h | 86 +- .../discrete/examples/schedulingExample.cpp | 488 ++-- .../discrete/examples/schedulingQuals12.cpp | 348 +-- gtsam_unstable/discrete/tests/testCSP.cpp | 318 +-- .../discrete/tests/testPlanning.cpp | 18 +- .../discrete/tests/testScheduler.cpp | 230 +- gtsam_unstable/discrete/tests/testSudoku.cpp | 276 +- gtsam_unstable/dynamics/DynamicsPriors.h | 104 +- gtsam_unstable/dynamics/FullIMUFactor.h | 146 +- gtsam_unstable/dynamics/IMUFactor.h | 128 +- gtsam_unstable/dynamics/PoseRTV.cpp | 326 +-- gtsam_unstable/dynamics/PoseRTV.h | 244 +- gtsam_unstable/dynamics/VelocityConstraint.h | 142 +- .../dynamics/tests/testIMUSystem.cpp | 220 +- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 222 +- .../dynamics/tests/testVelocityConstraint.cpp | 48 +- gtsam_unstable/geometry/InvDepthCamera3.h | 240 +- .../geometry/tests/testInvDepthCamera3.cpp | 154 +- gtsam_unstable/gtsam_unstable.h | 196 +- .../nonlinear/LinearContainerFactor.cpp | 224 +- .../nonlinear/LinearContainerFactor.h | 74 +- gtsam_unstable/nonlinear/LinearizedFactor.cpp | 160 +- gtsam_unstable/nonlinear/LinearizedFactor.h | 134 +- .../tests/testLinearContainerFactor.cpp | 176 +- .../nonlinear/tests/testLinearizedFactor.cpp | 70 +- gtsam_unstable/slam/DummyFactor.cpp | 28 +- gtsam_unstable/slam/DummyFactor.h | 22 +- gtsam_unstable/slam/InvDepthFactor3.h | 150 +- gtsam_unstable/slam/PartialPriorFactor.h | 200 +- gtsam_unstable/slam/PoseRotationPrior.h | 100 +- gtsam_unstable/slam/PoseTranslationPrior.h | 112 +- gtsam_unstable/slam/ReferenceFrameFactor.h | 126 +- .../slam/RelativeElevationFactor.cpp | 38 +- gtsam_unstable/slam/RelativeElevationFactor.h | 54 +- gtsam_unstable/slam/tests/testDummyFactor.cpp | 48 +- .../slam/tests/testPoseRotationPrior.cpp | 60 +- .../slam/tests/testPoseTranslationPrior.cpp | 116 +- .../slam/tests/testReferenceFrameFactor.cpp | 284 +- .../tests/testRelativeElevationFactor.cpp | 156 +- tests/simulated2D.h | 20 +- tests/simulated2DConstraints.h | 36 +- tests/simulated2DOriented.h | 14 +- tests/simulated3D.cpp | 20 +- tests/simulated3D.h | 94 +- tests/smallExample.cpp | 744 ++--- tests/smallExample.h | 206 +- tests/testBoundingConstraint.cpp | 332 +-- tests/testGaussianBayesNet.cpp | 60 +- tests/testGaussianFactor.cpp | 192 +- tests/testGaussianFactorGraphB.cpp | 246 +- tests/testGaussianISAM.cpp | 400 +-- tests/testGaussianISAM2.cpp | 296 +- tests/testGaussianJunctionTreeB.cpp | 92 +- tests/testGraph.cpp | 236 +- tests/testInferenceB.cpp | 6 +- tests/testIterative.cpp | 84 +- tests/testMarginals.cpp | 90 +- tests/testNonlinearEquality.cpp | 684 ++--- tests/testNonlinearFactor.cpp | 112 +- tests/testNonlinearFactorGraph.cpp | 102 +- tests/testNonlinearISAM.cpp | 84 +- tests/testNonlinearOptimizer.cpp | 132 +- tests/testOccupancyGrid.cpp | 424 +-- tests/testSerializationSLAM.cpp | 40 +- tests/testSimulated2D.cpp | 6 +- tests/testSimulated2DOriented.cpp | 14 +- tests/testSimulated3D.cpp | 32 +- tests/testSubgraphPreconditioner.cpp | 8 +- tests/testSymbolicBayesNetB.cpp | 30 +- tests/testSymbolicFactorGraphB.cpp | 96 +- tests/timeGaussianFactorGraph.cpp | 174 +- tests/timeIncremental.cpp | 2 +- tests/timeMultifrontalOnDataset.cpp | 10 +- tests/timeSequentialOnDataset.cpp | 10 +- wrap/Argument.cpp | 46 +- wrap/Argument.h | 52 +- wrap/Class.cpp | 416 +-- wrap/Class.h | 28 +- wrap/Constructor.cpp | 58 +- wrap/Constructor.h | 58 +- wrap/Deconstructor.cpp | 34 +- wrap/Deconstructor.h | 42 +- wrap/FileWriter.cpp | 48 +- wrap/FileWriter.h | 16 +- wrap/ForwardDeclaration.h | 10 +- wrap/GlobalFunction.cpp | 200 +- wrap/GlobalFunction.h | 40 +- wrap/Method.cpp | 174 +- wrap/Method.h | 52 +- wrap/Module.cpp | 666 ++--- wrap/Module.h | 30 +- wrap/ReturnValue.cpp | 158 +- wrap/ReturnValue.h | 48 +- wrap/StaticMethod.cpp | 174 +- wrap/StaticMethod.h | 50 +- wrap/TemplateInstantiationTypedef.cpp | 58 +- wrap/TemplateInstantiationTypedef.h | 12 +- wrap/TypeAttributesTable.cpp | 52 +- wrap/TypeAttributesTable.h | 20 +- wrap/matlab.h | 188 +- wrap/spirit_actors.h | 50 +- wrap/tests/geometry.h | 6 +- wrap/tests/testDependencies.h | 8 +- wrap/tests/testNamespaces.h | 20 +- wrap/tests/testSpirit.cpp | 28 +- wrap/tests/testWrap.cpp | 382 +-- wrap/utilities.cpp | 96 +- wrap/utilities.h | 66 +- wrap/wrap.cpp | 20 +- 434 files changed, 29825 insertions(+), 29825 deletions(-) diff --git a/CppUnitLite/Failure.h b/CppUnitLite/Failure.h index 93c99df12..67ac5ba38 100644 --- a/CppUnitLite/Failure.h +++ b/CppUnitLite/Failure.h @@ -29,42 +29,42 @@ class Failure { public: - Failure (const std::string& theTestName, - const std::string& theFileName, - long theLineNumber, - const std::string& theCondition) - : message (theCondition), - testName (theTestName), - fileName (theFileName), - lineNumber (theLineNumber) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + long theLineNumber, + const std::string& theCondition) + : message (theCondition), + testName (theTestName), + fileName (theFileName), + lineNumber (theLineNumber) + { + } - Failure (const std::string& theTestName, - const std::string& theFileName, - const std::string& theCondition) - : message (theCondition), - testName (theTestName), - fileName (theFileName), - lineNumber (-1) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + const std::string& theCondition) + : message (theCondition), + testName (theTestName), + fileName (theFileName), + lineNumber (-1) + { + } - Failure (const std::string& theTestName, - const std::string& theFileName, - long theLineNumber, - const std::string& expected, - const std::string& actual) - : message("expected " + expected + " but was: " + actual), - testName (theTestName), - fileName (theFileName), - lineNumber (theLineNumber) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + long theLineNumber, + const std::string& expected, + const std::string& actual) + : message("expected " + expected + " but was: " + actual), + testName (theTestName), + fileName (theFileName), + lineNumber (theLineNumber) + { + } - std::string message; - std::string testName; - std::string fileName; - long lineNumber; + std::string message; + std::string testName; + std::string fileName; + long lineNumber; }; diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp index e6a2ad51f..481fceb07 100644 --- a/CppUnitLite/Test.cpp +++ b/CppUnitLite/Test.cpp @@ -18,58 +18,58 @@ #include Test::Test (const std::string& testName) - : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) + : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) { - TestRegistry::addTest (this); + TestRegistry::addTest (this); } Test::Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck) - : name_(testName), next_(0), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck) + : name_(testName), next_(0), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck) { - TestRegistry::addTest (this); + TestRegistry::addTest (this); } Test *Test::getNext() const { - return next_; + return next_; } void Test::setNext(Test *test) -{ - next_ = test; +{ + next_ = test; } bool Test::check(long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber) { - if (expected == actual) - return true; - result.addFailure ( - Failure ( - name_, - boost::lexical_cast (__FILE__), - __LINE__, - boost::lexical_cast (expected), - boost::lexical_cast (actual))); + if (expected == actual) + return true; + result.addFailure ( + Failure ( + name_, + boost::lexical_cast (__FILE__), + __LINE__, + boost::lexical_cast (expected), + boost::lexical_cast (actual))); - return false; + return false; } bool Test::check(const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber) { - if (expected == actual) - return true; - result.addFailure ( - Failure ( - name_, - boost::lexical_cast (__FILE__), - __LINE__, - expected, - actual)); + if (expected == actual) + return true; + result.addFailure ( + Failure ( + name_, + boost::lexical_cast (__FILE__), + __LINE__, + expected, + actual)); - return false; + return false; } diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 820ed48cf..52b79f65c 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -30,30 +30,30 @@ class TestResult; class Test { public: - Test (const std::string& testName); - Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck); + Test (const std::string& testName); + Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck); virtual ~Test() {}; - virtual void run (TestResult& result) = 0; + virtual void run (TestResult& result) = 0; - void setNext(Test *test); - Test *getNext () const; - std::string getName() const {return name_;} - std::string getFilename() const {return filename_;} - long getLineNumber() const {return lineNumber_;} - bool safe() const {return safeCheck_;} + void setNext(Test *test); + Test *getNext () const; + std::string getName() const {return name_;} + std::string getFilename() const {return filename_;} + long getLineNumber() const {return lineNumber_;} + bool safe() const {return safeCheck_;} protected: - bool check (long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber); - bool check (const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber); + bool check (long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber); + bool check (const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber); - std::string name_; - Test *next_; - std::string filename_; - long lineNumber_; /// This is the line line number of the test, rather than the a single check - bool safeCheck_; + std::string name_; + Test *next_; + std::string filename_; + long lineNumber_; /// This is the line line number of the test, rather than the a single check + bool safeCheck_; }; @@ -62,11 +62,11 @@ protected: */ #define TEST(testName, testGroup)\ class testGroup##testName##Test : public Test \ - { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ + { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /** * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this @@ -74,11 +74,11 @@ protected: */ #define TEST_UNSAFE(testName, testGroup)\ class testGroup##testName##Test : public Test \ - { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ - virtual ~testGroup##testName##Test () {};\ + { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ + virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /* * Convention for tests: @@ -100,18 +100,18 @@ protected: #define THROWS_EXCEPTION(condition)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ - return; } \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + return; } \ catch (...) {} } #define CHECK_EXCEPTION(condition, exception_name)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ - return; } \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + return; } \ catch (exception_name&) {} \ catch (...) { \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast(#condition) + boost::lexical_cast(", expected: ") + boost::lexical_cast(#exception_name))); \ - return; } } + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast(#condition) + boost::lexical_cast(", expected: ") + boost::lexical_cast(#exception_name))); \ + return; } } #define EQUALITY(expected,actual)\ { if (!assert_equal(expected,actual)) \ diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp index 1d06b225c..b493e81a6 100644 --- a/CppUnitLite/TestRegistry.cpp +++ b/CppUnitLite/TestRegistry.cpp @@ -20,64 +20,64 @@ void TestRegistry::addTest (Test *test) { - instance ().add (test); + instance ().add (test); } int TestRegistry::runAllTests (TestResult& result) { - return instance ().run (result); + return instance ().run (result); } TestRegistry& TestRegistry::instance () { - static TestRegistry registry; - return registry; + static TestRegistry registry; + return registry; } void TestRegistry::add (Test *test) { - if (tests == 0) { - test->setNext(0); - tests = test; - lastTest = test; - return; - } + if (tests == 0) { + test->setNext(0); + tests = test; + lastTest = test; + return; + } - test->setNext (0); - lastTest->setNext(test); - lastTest = test; + test->setNext (0); + lastTest->setNext(test); + lastTest = test; } int TestRegistry::run (TestResult& result) { - result.testsStarted (); + result.testsStarted (); - for (Test *test = tests; test != 0; test = test->getNext ()) { - if (test->safe()) { - try { - test->run (result); - } catch (std::exception& e) { - // catch standard exceptions and derivatives - result.addFailure( - Failure(test->getName(), test->getFilename(), test->getLineNumber(), - std::string("Exception: ") + std::string(e.what()))); - } catch (...) { - // catch all other exceptions - result.addFailure( - Failure(test->getName(), test->getFilename(), test->getLineNumber(), - "ExceptionThrown!")); - } - } - else { - test->run (result); - } - } - result.testsEnded (); - return result.getFailureCount(); + for (Test *test = tests; test != 0; test = test->getNext ()) { + if (test->safe()) { + try { + test->run (result); + } catch (std::exception& e) { + // catch standard exceptions and derivatives + result.addFailure( + Failure(test->getName(), test->getFilename(), test->getLineNumber(), + std::string("Exception: ") + std::string(e.what()))); + } catch (...) { + // catch all other exceptions + result.addFailure( + Failure(test->getName(), test->getFilename(), test->getLineNumber(), + "ExceptionThrown!")); + } + } + else { + test->run (result); + } + } + result.testsEnded (); + return result.getFailureCount(); } diff --git a/CppUnitLite/TestRegistry.h b/CppUnitLite/TestRegistry.h index 4053e815c..56db991ad 100644 --- a/CppUnitLite/TestRegistry.h +++ b/CppUnitLite/TestRegistry.h @@ -29,18 +29,18 @@ class TestResult; class TestRegistry { public: - static void addTest (Test *test); - static int runAllTests (TestResult& result); + static void addTest (Test *test); + static int runAllTests (TestResult& result); private: - static TestRegistry& instance (); - void add (Test *test); - int run (TestResult& result); + static TestRegistry& instance (); + void add (Test *test); + int run (TestResult& result); - - Test *tests; - Test *lastTest; + + Test *tests; + Test *lastTest; }; diff --git a/CppUnitLite/TestResult.cpp b/CppUnitLite/TestResult.cpp index a9e599989..2519c94a9 100644 --- a/CppUnitLite/TestResult.cpp +++ b/CppUnitLite/TestResult.cpp @@ -17,7 +17,7 @@ TestResult::TestResult () - : failureCount (0) + : failureCount (0) { } @@ -29,29 +29,29 @@ void TestResult::testsStarted () void TestResult::addFailure (const Failure& failure) { - if (failure.lineNumber < 0) // allow for no line number - fprintf (stdout, "%s%s%s%s\n", - "Failure: \"", - failure.message.c_str (), - "\" in ", - failure.fileName.c_str ()); - else - fprintf (stdout, "%s%s%ld%s%s%s\n", - failure.fileName.c_str(), // Format matches Eclipse error flagging - ":", - failure.lineNumber, - ": Failure: \"", - failure.message.c_str(), - "\" "); + if (failure.lineNumber < 0) // allow for no line number + fprintf (stdout, "%s%s%s%s\n", + "Failure: \"", + failure.message.c_str (), + "\" in ", + failure.fileName.c_str ()); + else + fprintf (stdout, "%s%s%ld%s%s%s\n", + failure.fileName.c_str(), // Format matches Eclipse error flagging + ":", + failure.lineNumber, + ": Failure: \"", + failure.message.c_str(), + "\" "); - failureCount++; + failureCount++; } void TestResult::testsEnded () { - if (failureCount > 0) - fprintf (stdout, "There were %d failures\n", failureCount); - else - fprintf (stdout, "There were no test failures\n"); + if (failureCount > 0) + fprintf (stdout, "There were %d failures\n", failureCount); + else + fprintf (stdout, "There were no test failures\n"); } diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index e5f5abfb1..a30275647 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -28,16 +28,16 @@ class Failure; class TestResult { public: - TestResult (); + TestResult (); virtual ~TestResult() {}; - virtual void testsStarted (); - virtual void addFailure (const Failure& failure); - virtual void testsEnded (); + virtual void testsStarted (); + virtual void addFailure (const Failure& failure); + virtual void testsEnded (); int getFailureCount() {return failureCount;} private: - int failureCount; + int failureCount; }; #endif diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 4133d1e1e..e024d832c 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file CameraResectioning.cpp + * @file CameraResectioning.cpp * @brief An example of gtsam for solving the camera resectioning problem - * @author Duy-Nguyen Ta - * @date Aug 23, 2011 + * @author Duy-Nguyen Ta + * @date Aug 23, 2011 */ #include @@ -30,27 +30,27 @@ using symbol_shorthand::X; * a known 3D point in the image */ class ResectioningFactor: public NoiseModelFactor1 { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; - shared_ptrK K_; // camera's intrinsic parameters - Point3 P_; // 3D point on the calibration rig - Point2 p_; // 2D measurement of the 3D point + shared_ptrK K_; // camera's intrinsic parameters + Point3 P_; // 3D point on the calibration rig + Point2 p_; // 2D measurement of the 3D point public: - /// Construct factor given known point P and its projection p - ResectioningFactor(const SharedNoiseModel& model, const Key& key, - const shared_ptrK& calib, const Point2& p, const Point3& P) : - Base(model, key), K_(calib), P_(P), p_(p) { - } + /// Construct factor given known point P and its projection p + ResectioningFactor(const SharedNoiseModel& model, const Key& key, + const shared_ptrK& calib, const Point2& p, const Point3& P) : + Base(model, key), K_(calib), P_(P), p_(p) { + } - /// evaluate the error - virtual Vector evaluateError(const Pose3& pose, boost::optional H = - boost::none) const { - SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H) - p_); - return reprojectionError.vector(); - } + /// evaluate the error + virtual Vector evaluateError(const Pose3& pose, boost::optional H = + boost::none) const { + SimpleCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(P_, H) - p_); + return reprojectionError.vector(); + } }; /******************************************************************************* @@ -62,37 +62,37 @@ public: * 2D Point: (55,45) (45,45) (45,55) (55,55) *******************************************************************************/ int main(int argc, char* argv[]) { - /* read camera intrinsic parameters */ - shared_ptrK calib(new Cal3_S2(1, 1, 0, 50, 50)); + /* read camera intrinsic parameters */ + shared_ptrK calib(new Cal3_S2(1, 1, 0, 50, 50)); - /* 1. create graph */ - NonlinearFactorGraph graph; + /* 1. create graph */ + NonlinearFactorGraph graph; - /* 2. add factors to the graph */ - // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); - boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + /* 2. add factors to the graph */ + // add measurement factors + SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); + boost::shared_ptr factor; + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0))); - /* 3. Create an initial estimate for the camera pose */ - Values initial; - initial.insert(X(1), - Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2))); + /* 3. Create an initial estimate for the camera pose */ + Values initial; + initial.insert(X(1), + Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2))); - /* 4. Optimize the graph using Levenberg-Marquardt*/ - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Final result:\n"); + /* 4. Optimize the graph using Levenberg-Marquardt*/ + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("Final result:\n"); - return 0; + return 0; } diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 6572236a0..64c17c3db 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file DiscreteBayesNet_FG.cpp - * @brief Discrete Bayes Net example using Factor Graphs - * @author Abhijit - * @date Jun 4, 2012 + * @file DiscreteBayesNet_FG.cpp + * @brief Discrete Bayes Net example using Factor Graphs + * @author Abhijit + * @date Jun 4, 2012 * * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529] * You may be familiar with other graphical model packages like BNT (available @@ -31,89 +31,89 @@ using namespace gtsam; int main(int argc, char **argv) { - // We assume binary state variables - // we have 0 == "False" and 1 == "True" - const size_t nrStates = 2; + // We assume binary state variables + // we have 0 == "False" and 1 == "True" + const size_t nrStates = 2; - // define variables - DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates), - WetGrass(4, nrStates); + // define variables + DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates), + WetGrass(4, nrStates); - // create Factor Graph of the bayes net - DiscreteFactorGraph graph; + // create Factor Graph of the bayes net + DiscreteFactorGraph graph; - // add factors - graph.add(Cloudy, "0.5 0.5"); //P(Cloudy) - graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy) - graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy) - graph.add(Sprinkler & Rain & WetGrass, - "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain) + // add factors + graph.add(Cloudy, "0.5 0.5"); //P(Cloudy) + graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy) + graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy) + graph.add(Sprinkler & Rain & WetGrass, + "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain) - // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional - // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp) + // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional + // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp) - // Since this is a relatively small distribution, we can as well print - // the whole distribution.. - cout << "Distribution of Example: " << endl; - cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10) - << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" - << endl; - for (size_t a = 0; a < nrStates; a++) - for (size_t m = 0; m < nrStates; m++) - for (size_t h = 0; h < nrStates; h++) - for (size_t c = 0; c < nrStates; c++) { - DiscreteFactor::Values values; - values[Cloudy.first] = c; - values[Sprinkler.first] = h; - values[Rain.first] = m; - values[WetGrass.first] = a; - double prodPot = graph(values); - cout << boolalpha << setw(8) << (bool) c << setw(14) - << (bool) h << setw(12) << (bool) m << setw(13) - << (bool) a << setw(16) << prodPot << endl; - } + // Since this is a relatively small distribution, we can as well print + // the whole distribution.. + cout << "Distribution of Example: " << endl; + cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10) + << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" + << endl; + for (size_t a = 0; a < nrStates; a++) + for (size_t m = 0; m < nrStates; m++) + for (size_t h = 0; h < nrStates; h++) + for (size_t c = 0; c < nrStates; c++) { + DiscreteFactor::Values values; + values[Cloudy.first] = c; + values[Sprinkler.first] = h; + values[Rain.first] = m; + values[WetGrass.first] = a; + double prodPot = graph(values); + cout << boolalpha << setw(8) << (bool) c << setw(14) + << (bool) h << setw(12) << (bool) m << setw(13) + << (bool) a << setw(16) << prodPot << endl; + } - // "Most Probable Explanation", i.e., configuration with largest value - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - cout <<"\nMost Probable Explanation (MPE):" << endl; - cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first] - << " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first] - << " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first] - << " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl; + // "Most Probable Explanation", i.e., configuration with largest value + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + cout <<"\nMost Probable Explanation (MPE):" << endl; + cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first] + << " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first] + << " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first] + << " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl; - // "Inference" We show an inference query like: probability that the Sprinkler was on; - // given that the grass is wet i.e. P( S | W=1) =? - cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl; + // "Inference" We show an inference query like: probability that the Sprinkler was on; + // given that the grass is wet i.e. P( S | W=1) =? + cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl; - // Method 1: we can compute the joint marginal P(S,W) and from that we can compute - // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps.. + // Method 1: we can compute the joint marginal P(S,W) and from that we can compute + // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps.. - //Step1: Compute P(S,W) - DiscreteFactorGraph jointFG; - jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); - DecisionTreeFactor probSW = jointFG.product(); + //Step1: Compute P(S,W) + DiscreteFactorGraph jointFG; + jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); + DecisionTreeFactor probSW = jointFG.product(); - //Step2: Compute P(W) - DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first); + //Step2: Compute P(W) + DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first); - //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1) - DiscreteFactor::Values values; - values[WetGrass.first] = 1; + //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1) + DiscreteFactor::Values values; + values[WetGrass.first] = 1; - //print P(S=0|W=1) - values[Sprinkler.first] = 0; - cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl; + //print P(S=0|W=1) + values[Sprinkler.first] = 0; + cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl; - //print P(S=1|W=1) - values[Sprinkler.first] = 1; - cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl; + //print P(S=1|W=1) + values[Sprinkler.first] = 1; + cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl; - // TODO: Method 2 : One way is to modify the factor graph to - // incorporate the evidence node and compute the marginal - // TODO: graph.addEvidence(Cloudy,0); + // TODO: Method 2 : One way is to modify the factor graph to + // incorporate the evidence node and compute the marginal + // TODO: graph.addEvidence(Cloudy,0); - return 0; + return 0; } diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index c6f6b636f..88b8da3e0 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -72,71 +72,71 @@ using namespace gtsam; int main(int argc, char** argv) { // Create a factor graph - NonlinearFactorGraph graph; + NonlinearFactorGraph graph; - // Create the keys we need for this simple example - static Symbol x1('x',1), x2('x',2), x3('x',3); - static Symbol l1('l',1), l2('l',2); + // Create the keys we need for this simple example + static Symbol x1('x',1), x2('x',2), x3('x',3); + static Symbol l1('l',1), l2('l',2); - // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) - Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph - // Add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + // Add two odometry factors + Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); - // Add Range-Bearing measurements to two different landmarks - // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range - // create the measurement values - indices are (pose id, landmark id) - Rot2 bearing11 = Rot2::fromDegrees(45), - bearing21 = Rot2::fromDegrees(90), - bearing32 = Rot2::fromDegrees(90); - double range11 = std::sqrt(4.0+4.0), - range21 = 2.0, - range32 = 2.0; + // Add Range-Bearing measurements to two different landmarks + // create a noise model for the landmark measurements + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = std::sqrt(4.0+4.0), + range21 = 2.0, + range32 = 2.0; - // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + // Add Bearing-Range factors + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); - // Print - graph.print("Factor Graph:\n"); + // Print + graph.print("Factor Graph:\n"); - // Create (deliberately inaccurate) initial estimate - Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + // Create (deliberately inaccurate) initial estimate + Values initialEstimate; + initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insert(l1, Point2(1.8, 2.1)); + initialEstimate.insert(l2, Point2(4.1, 1.8)); - // Print - initialEstimate.print("Initial Estimate:\n"); + // Print + initialEstimate.print("Initial Estimate:\n"); - // Optimize using Levenberg-Marquardt optimization. The optimizer - // accepts an optional set of configuration parameters, controlling - // things like convergence criteria, the type of linear system solver - // to use, and the amount of information displayed during optimization. - // Here we will use the default set of parameters. See the - // documentation for the full set of parameters. - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); + // Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); // Calculate and print marginal covariances for all variables - Marginals marginals(graph, result); + Marginals marginals(graph, result); print(marginals.marginalCovariance(x1), "x1 covariance"); print(marginals.marginalCovariance(x2), "x2 covariance"); print(marginals.marginalCovariance(x3), "x3 covariance"); print(marginals.marginalCovariance(l1), "l1 covariance"); print(marginals.marginalCovariance(l2), "l2 covariance"); - return 0; + return 0; } diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 4885e72c8..cbe97ae12 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -66,54 +66,54 @@ using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + // 2a. Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); - // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // For simplicity, we will use the same noise model for odometry and loop closures + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - // 2b. Add odometry factors - // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + // 2b. Add odometry factors + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); - // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); - graph.print("\nFactor Graph:\n"); // print + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. We will use another Between Factor to enforce this constraint: + graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.print("\nFactor Graph:\n"); // print - // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values - Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); - initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer // The optimizer accepts an optional set of configuration parameters, - // controlling things like convergence criteria, the type of linear - // system solver to use, and the amount of information displayed during - // optimization. We will set a few parameters as a demonstration. - GaussNewtonParams parameters; - // Stop iterating once the change in error between steps is less than this value - parameters.relativeErrorTol = 1e-5; - // Do not perform more than N iteration steps - parameters.maxIterations = 100; - // Create the optimizer ... - GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); - // ... and optimize + // controlling things like convergence criteria, the type of linear + // system solver to use, and the amount of information displayed during + // optimization. We will set a few parameters as a demonstration. + GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + parameters.maxIterations = 100; + // Create the optimizer ... + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + // ... and optimize Values result = optimizer.optimize(); result.print("Final Result:\n"); @@ -126,5 +126,5 @@ int main(int argc, char** argv) { cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; - return 0; + return 0; } diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index cc873eb08..000a071da 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -31,26 +31,26 @@ using namespace gtsam; int main(int argc, char** argv) { - // Read File and create graph and initial estimate - // we are in build/examples, data is in examples/Data - NonlinearFactorGraph::shared_ptr graph ; - Values::shared_ptr initial; - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); - boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); - initial->print("Initial estimate:\n"); + // Read File and create graph and initial estimate + // we are in build/examples, data is in examples/Data + NonlinearFactorGraph::shared_ptr graph ; + Values::shared_ptr initial; + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); + boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); + initial->print("Initial estimate:\n"); - // Add a Gaussian prior on first poses - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->add(PriorFactor(0, priorMean, priorNoise)); + // Add a Gaussian prior on first poses + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); + graph->add(PriorFactor(0, priorMean, priorNoise)); - // Single Step Optimization using Levenberg-Marquardt - Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); - result.print("\nFinal result:\n"); + // Single Step Optimization using Levenberg-Marquardt + Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + result.print("\nFinal result:\n"); - // Plot the covariance of the last pose - Marginals marginals(*graph, result); - cout.precision(2); + // Plot the covariance of the last pose + Marginals marginals(*graph, result); + cout.precision(2); cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl; return 0; diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index cff9d754e..54c64e820 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -60,69 +60,69 @@ const double degree = M_PI / 180; int main() { - /** - * Step 1: Create a factor to express a unary constraint - * The "prior" in this case is the measurement from a sensor, - * with a model of the noise on the measurement. - * - * The "Key" created here is a label used to associate parts of the - * state (stored in "RotValues") with particular factors. They require - * an index to allow for lookup, and should be unique. - * - * In general, creating a factor requires: - * - A key or set of keys labeling the variables that are acted upon - * - A measurement value - * - A measurement model with the correct dimensionality for the factor - */ - Rot2 prior = Rot2::fromAngle(30 * degree); - prior.print("goal angle"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Symbol key('x',1); - PriorFactor factor(key, prior, model); + /** + * Step 1: Create a factor to express a unary constraint + * The "prior" in this case is the measurement from a sensor, + * with a model of the noise on the measurement. + * + * The "Key" created here is a label used to associate parts of the + * state (stored in "RotValues") with particular factors. They require + * an index to allow for lookup, and should be unique. + * + * In general, creating a factor requires: + * - A key or set of keys labeling the variables that are acted upon + * - A measurement value + * - A measurement model with the correct dimensionality for the factor + */ + Rot2 prior = Rot2::fromAngle(30 * degree); + prior.print("goal angle"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); + Symbol key('x',1); + PriorFactor factor(key, prior, model); - /** - * Step 2: Create a graph container and add the factor to it - * Before optimizing, all factors need to be added to a Graph container, - * which provides the necessary top-level functionality for defining a - * system of constraints. - * - * In this case, there is only one factor, but in a practical scenario, - * many more factors would be added. - */ - NonlinearFactorGraph graph; - graph.add(factor); - graph.print("full graph"); + /** + * Step 2: Create a graph container and add the factor to it + * Before optimizing, all factors need to be added to a Graph container, + * which provides the necessary top-level functionality for defining a + * system of constraints. + * + * In this case, there is only one factor, but in a practical scenario, + * many more factors would be added. + */ + NonlinearFactorGraph graph; + graph.add(factor); + graph.print("full graph"); - /** - * Step 3: Create an initial estimate - * An initial estimate of the solution for the system is necessary to - * start optimization. This system state is the "RotValues" structure, - * which is similar in structure to a STL map, in that it maps - * keys (the label created in step 1) to specific values. - * - * The initial estimate provided to optimization will be used as - * a linearization point for optimization, so it is important that - * all of the variables in the graph have a corresponding value in - * this structure. - * - * The interface to all RotValues types is the same, it only depends - * on the type of key used to find the appropriate value map if there - * are multiple types of variables. - */ - Values initial; - initial.insert(key, Rot2::fromAngle(20 * degree)); - initial.print("initial estimate"); + /** + * Step 3: Create an initial estimate + * An initial estimate of the solution for the system is necessary to + * start optimization. This system state is the "RotValues" structure, + * which is similar in structure to a STL map, in that it maps + * keys (the label created in step 1) to specific values. + * + * The initial estimate provided to optimization will be used as + * a linearization point for optimization, so it is important that + * all of the variables in the graph have a corresponding value in + * this structure. + * + * The interface to all RotValues types is the same, it only depends + * on the type of key used to find the appropriate value map if there + * are multiple types of variables. + */ + Values initial; + initial.insert(key, Rot2::fromAngle(20 * degree)); + initial.print("initial estimate"); - /** - * Step 4: Optimize - * After formulating the problem with a graph of constraints - * and an initial estimate, executing optimization is as simple - * as calling a general optimization function with the graph and - * initial estimate. This will yield a new RotValues structure - * with the final state of the optimization. - */ - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("final result"); + /** + * Step 4: Optimize + * After formulating the problem with a graph of constraints + * and an initial estimate, executing optimization is as simple + * as calling a general optimization function with the graph and + * initial estimate. This will yield a new RotValues structure + * with the final state of the optimization. + */ + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("final result"); - return 0; + return 0; } diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 56dbd4726..b029379f2 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -34,23 +34,23 @@ int main(int argc, char** argv) { // Set Number of Nodes in the Graph const int nrNodes = 60; - // Each node takes 1 of 7 possible states denoted by 0-6 in following order: - // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" - // "Industry(with PhD)" "Academia" "Deceased"] - const size_t nrStates = 7; + // Each node takes 1 of 7 possible states denoted by 0-6 in following order: + // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" + // "Industry(with PhD)" "Academia" "Deceased"] + const size_t nrStates = 7; - // define variables + // define variables vector nodes; for (int i = 0; i < nrNodes; i++){ DiscreteKey dk(i, nrStates); nodes.push_back(dk); } - // create graph - DiscreteFactorGraph graph; + // create graph + DiscreteFactorGraph graph; - // add node potentials - graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); + // add node potentials + graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1"); @@ -62,53 +62,53 @@ int main(int argc, char** argv) { "0 0 0 .01 .01 .97 .01 " "0 0 0 0 0 0 1"; - // add edge potentials - for (int i = 0; i < nrNodes - 1; i++) - graph.add(nodes[i] & nodes[i + 1], edgePotential); + // add edge potentials + for (int i = 0; i < nrNodes - 1; i++) + graph.add(nodes[i] & nodes[i + 1], edgePotential); - cout << "Created Factor Graph with " << nrNodes << " variable nodes and " - << graph.size() << " factors (Unary+Edge)."; + cout << "Created Factor Graph with " << nrNodes << " variable nodes and " + << graph.size() << " factors (Unary+Edge)."; - // "Decoding", i.e., configuration with largest value - // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); + // "Decoding", i.e., configuration with largest value + // We use sequential variable elimination + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); - // "Inference" Computing marginals for each node + // "Inference" Computing marginals for each node - cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl; - tic_(1, "Sequential"); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = solver.marginalProbabilities(*itr); + cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl; + tic_(1, "Sequential"); + for (vector::iterator itr = nodes.begin(); itr != nodes.end(); + ++itr) { + //Compute the marginal + Vector margProbs = solver.marginalProbabilities(*itr); - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; - print(margProbs); - } - toc_(1, "Sequential"); + //Print the marginals + cout << "Node#" << setw(4) << itr->first << " : "; + print(margProbs); + } + toc_(1, "Sequential"); - // Here we'll make use of DiscreteMarginals class, which makes use of - // bayes-tree based shortcut evaluation of marginals - DiscreteMarginals marginals(graph); + // Here we'll make use of DiscreteMarginals class, which makes use of + // bayes-tree based shortcut evaluation of marginals + DiscreteMarginals marginals(graph); - cout << "\nComputing Node Marginals ..(BayesTree based)" << endl; - tic_(2, "Multifrontal"); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = marginals.marginalProbabilities(*itr); + cout << "\nComputing Node Marginals ..(BayesTree based)" << endl; + tic_(2, "Multifrontal"); + for (vector::iterator itr = nodes.begin(); itr != nodes.end(); + ++itr) { + //Compute the marginal + Vector margProbs = marginals.marginalProbabilities(*itr); - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; - print(margProbs); - } - toc_(2, "Multifrontal"); + //Print the marginals + cout << "Node#" << setw(4) << itr->first << " : "; + print(margProbs); + } + toc_(2, "Multifrontal"); - tictoc_print_(); - return 0; + tictoc_print_(); + return 0; } diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index e9424d369..43cd9b34c 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -25,62 +25,62 @@ using namespace gtsam; int main(int argc, char** argv) { - // We will assume 2-state variables, where, to conform to the "small" example - // we have 0 == "right answer" and 1 == "wrong answer" - size_t nrStates = 2; + // We will assume 2-state variables, where, to conform to the "small" example + // we have 0 == "right answer" and 1 == "wrong answer" + size_t nrStates = 2; - // define variables - DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), - Allison(4, nrStates); + // define variables + DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), + Allison(4, nrStates); - // create graph - DiscreteFactorGraph graph; + // create graph + DiscreteFactorGraph graph; - // add node potentials - graph.add(Cathy, "1 3"); - graph.add(Heather, "9 1"); - graph.add(Mark, "1 3"); - graph.add(Allison, "9 1"); + // add node potentials + graph.add(Cathy, "1 3"); + graph.add(Heather, "9 1"); + graph.add(Mark, "1 3"); + graph.add(Allison, "9 1"); - // add edge potentials - graph.add(Cathy & Heather, "2 1 1 2"); - graph.add(Heather & Mark, "2 1 1 2"); - graph.add(Mark & Allison, "2 1 1 2"); + // add edge potentials + graph.add(Cathy & Heather, "2 1 1 2"); + graph.add(Heather & Mark, "2 1 1 2"); + graph.add(Mark & Allison, "2 1 1 2"); - // Print the UGM distribution - cout << "\nUGM distribution:" << endl; - vector allPosbValues = cartesianProduct( - Cathy & Heather & Mark & Allison); - for (size_t i = 0; i < allPosbValues.size(); ++i) { - DiscreteFactor::Values values = allPosbValues[i]; - double prodPot = graph(values); - cout << values[Cathy.first] << " " << values[Heather.first] << " " - << values[Mark.first] << " " << values[Allison.first] << " :\t" - << prodPot << "\t" << prodPot / 3790 << endl; - } + // Print the UGM distribution + cout << "\nUGM distribution:" << endl; + vector allPosbValues = cartesianProduct( + Cathy & Heather & Mark & Allison); + for (size_t i = 0; i < allPosbValues.size(); ++i) { + DiscreteFactor::Values values = allPosbValues[i]; + double prodPot = graph(values); + cout << values[Cathy.first] << " " << values[Heather.first] << " " + << values[Mark.first] << " " << values[Allison.first] << " :\t" + << prodPot << "\t" << prodPot / 3790 << endl; + } - // "Decoding", i.e., configuration with largest value (MPE) - // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - optimalDecoding->print("\noptimalDecoding"); + // "Decoding", i.e., configuration with largest value (MPE) + // We use sequential variable elimination + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + optimalDecoding->print("\noptimalDecoding"); - // "Inference" Computing marginals - cout << "\nComputing Node Marginals .." << endl; - Vector margProbs; + // "Inference" Computing marginals + cout << "\nComputing Node Marginals .." << endl; + Vector margProbs; - margProbs = solver.marginalProbabilities(Cathy); - print(margProbs, "Cathy's Node Marginal:"); + margProbs = solver.marginalProbabilities(Cathy); + print(margProbs, "Cathy's Node Marginal:"); - margProbs = solver.marginalProbabilities(Heather); - print(margProbs, "Heather's Node Marginal"); + margProbs = solver.marginalProbabilities(Heather); + print(margProbs, "Heather's Node Marginal"); - margProbs = solver.marginalProbabilities(Mark); - print(margProbs, "Mark's Node Marginal"); + margProbs = solver.marginalProbabilities(Mark); + print(margProbs, "Mark's Node Marginal"); - margProbs = solver.marginalProbabilities(Allison); - print(margProbs, "Allison's Node Marginal"); + margProbs = solver.marginalProbabilities(Allison); + print(margProbs, "Allison's Node Marginal"); - return 0; + return 0; } diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 0fc4db330..3c18b220a 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -35,7 +35,7 @@ using namespace gtsam; int main() { - // [code below basically does SRIF with Cholesky] + // [code below basically does SRIF with Cholesky] // Create a factor graph to perform the inference GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph); @@ -46,11 +46,11 @@ int main() { // Create a structure to hold the linearization points Values linearizationPoints; - // Ground truth example - // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 - // Motion model is just moving to the right (x'-x)^2 - // Measurements are GPS like, (x-z)^2, where z is a 2D measurement - // i.e., we should get 0,0 0,1 0,2 if there is no noise + // Ground truth example + // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 + // Motion model is just moving to the right (x'-x)^2 + // Measurements are GPS like, (x-z)^2, where z is a 2D measurement + // i.e., we should get 0,0 0,1 0,2 if there is no noise // Create new state variable Symbol x0('x',0); diff --git a/gtsam.h b/gtsam.h index 130c5260c..f67cb6303 100644 --- a/gtsam.h +++ b/gtsam.h @@ -6,7 +6,7 @@ * * Requirements: * Classes must start with an uppercase letter - * - Can wrap a typedef + * - Can wrap a typedef * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines * Methods can return * - Eigen types: Matrix, Vector @@ -27,20 +27,20 @@ * - The first letter will be made uppercase in the generated MATLAB interface * - Overloads are supported * Arguments to functions any of - * - Eigen types: Matrix, Vector - * - Eigen types and classes as an optionally const reference + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char * - Any class with which be copied with boost::make_shared() (except Eigen) * - boost::shared_ptr of any object type (except Eigen) * Comments can use either C++ or C style, with multiple lines * Namespace definitions * - Names of namespaces must start with a lowercase letter - * - start a namespace with "namespace {" - * - end a namespace with exactly "}" - * - Namespaces can be nested + * - start a namespace with "namespace {" + * - end a namespace with exactly "}" + * - Namespaces can be nested * Namespace usage - * - Namespaces can be specified for classes in arguments and return values - * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" * Includes in C++ wrappers * - All includes will be collected and added in a single file * - All namespaces must have angle brackets: @@ -61,7 +61,7 @@ * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead * of using the copy constructor (which is used for non-virtual objects). * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree - * virtual boost::shared_ptr clone() const; + * virtual boost::shared_ptr clone() const; * Templates * - Basic templates are supported either with an explicit list of types to instantiate, * e.g. template class Class1 { ... }; @@ -124,100 +124,100 @@ namespace gtsam { bool linear_independent(Matrix A, Matrix B, double tol); virtual class Value { - // No constructors because this is an abstract class + // No constructors because this is an abstract class - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Manifold - size_t dim() const; + // Manifold + size_t dim() const; }; #include virtual class LieScalar : gtsam::Value { - // Standard constructors - LieScalar(); - LieScalar(double d); + // Standard constructors + LieScalar(); + LieScalar(double d); - // Standard interface - double value() const; + // Standard interface + double value() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieScalar& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; - // Group - static gtsam::LieScalar identity(); - gtsam::LieScalar inverse() const; - gtsam::LieScalar compose(const gtsam::LieScalar& p) const; - gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; - // Manifold - size_t dim() const; - gtsam::LieScalar retract(Vector v) const; - Vector localCoordinates(const gtsam::LieScalar& t2) const; + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; - // Lie group - static gtsam::LieScalar Expmap(Vector v); - static Vector Logmap(const gtsam::LieScalar& p); + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); }; #include virtual class LieVector : gtsam::Value { - // Standard constructors - LieVector(); - LieVector(Vector v); + // Standard constructors + LieVector(); + LieVector(Vector v); - // Standard interface - Vector vector() const; + // Standard interface + Vector vector() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieVector& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; - // Group - static gtsam::LieVector identity(); - gtsam::LieVector inverse() const; - gtsam::LieVector compose(const gtsam::LieVector& p) const; - gtsam::LieVector between(const gtsam::LieVector& l2) const; + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; - // Manifold - size_t dim() const; - gtsam::LieVector retract(Vector v) const; - Vector localCoordinates(const gtsam::LieVector& t2) const; + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; - // Lie group - static gtsam::LieVector Expmap(Vector v); - static Vector Logmap(const gtsam::LieVector& p); + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); }; #include virtual class LieMatrix : gtsam::Value { - // Standard constructors - LieMatrix(); - LieMatrix(Matrix v); + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); - // Standard interface - Matrix matrix() const; + // Standard interface + Matrix matrix() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieMatrix& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; - // Group - static gtsam::LieMatrix identity(); - gtsam::LieMatrix inverse() const; - gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; - gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; - // Manifold - size_t dim() const; - gtsam::LieMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::LieMatrix & t2) const; + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; - // Lie group - static gtsam::LieMatrix Expmap(Vector v); - static Vector Logmap(const gtsam::LieMatrix& p); + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); }; //************************************************************************* @@ -226,9 +226,9 @@ virtual class LieMatrix : gtsam::Value { virtual class Point2 : gtsam::Value { // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); + Point2(); + Point2(double x, double y); + Point2(Vector v); // Testable void print(string s) const; @@ -288,9 +288,9 @@ virtual class StereoPoint2 : gtsam::Value { virtual class Point3 : gtsam::Value { // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); // Testable void print(string s) const; @@ -310,19 +310,19 @@ virtual class Point3 : gtsam::Value { // Lie Group static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); + static Vector Logmap(const gtsam::Point3& p); // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; + Vector vector() const; + double x() const; + double y() const; + double z() const; }; virtual class Rot2 : gtsam::Value { // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); + Rot2(); + Rot2(double theta); static gtsam::Rot2 fromAngle(double theta); static gtsam::Rot2 fromDegrees(double theta); static gtsam::Rot2 fromCosSin(double c, double s); @@ -345,47 +345,47 @@ virtual class Rot2 : gtsam::Value { // Lie Group static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); + static Vector Logmap(const gtsam::Rot2& p); // Group Action on Point2 gtsam::Point2 rotate(const gtsam::Point2& point) const; gtsam::Point2 unrotate(const gtsam::Point2& point) const; // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; Matrix matrix() const; }; virtual class Rot3 : gtsam::Value { // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); - static gtsam::Rot3 rodriguez(Vector v); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 ypr(double y, double p, double r); + static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 rodriguez(Vector v); // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; // Group - static gtsam::Rot3 identity(); + static gtsam::Rot3 identity(); gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; // Manifold static size_t Dim(); @@ -395,31 +395,31 @@ virtual class Rot3 : gtsam::Value { Vector localCoordinates(const gtsam::Rot3& p) const; // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly }; virtual class Pose2 : gtsam::Value { // Standard Constructor - Pose2(); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); + Pose2(); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); // Testable void print(string s) const; @@ -438,8 +438,8 @@ virtual class Pose2 : gtsam::Value { Vector localCoordinates(const gtsam::Pose2& p) const; // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); Matrix adjointMap() const; Vector adjoint(const Vector& xi) const; static Matrix wedge(double vx, double vy, double w); @@ -449,70 +449,70 @@ virtual class Pose2 : gtsam::Value { gtsam::Point2 transform_to(const gtsam::Point2& p) const; // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; Matrix matrix() const; }; virtual class Pose3 : gtsam::Value { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) + Pose3(Matrix t); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Pose3 retract(Vector v) const; - gtsam::Pose3 retractFirstOrder(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retractFirstOrder(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& T2) const; - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix adjointMap() const; - Vector adjoint(const Vector& xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& p); + Matrix adjointMap() const; + Vector adjoint(const Vector& xi) const; + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + // Group Action on Point3 + gtsam::Point3 transform_from(const gtsam::Point3& p) const; + gtsam::Point3 transform_to(const gtsam::Point3& p) const; - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); }; virtual class Cal3_S2 : gtsam::Value { // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); // Testable void print(string s) const; @@ -542,34 +542,34 @@ virtual class Cal3_S2 : gtsam::Value { #include virtual class Cal3DS2 : gtsam::Value { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); + Cal3DS2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // TODO: D2d functions that start with an uppercase letter - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase }; class Cal3_S2Stereo { @@ -593,14 +593,14 @@ class Cal3_S2Stereo { virtual class CalibratedCamera : gtsam::Value { // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(const Vector& v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold static size_t Dim(); @@ -617,13 +617,13 @@ virtual class CalibratedCamera : gtsam::Value { static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); // Standard Interface - gtsam::Pose3 pose() const; + gtsam::Pose3 pose() const; double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods }; virtual class SimpleCamera : gtsam::Value { // Standard Constructors and Named Constructors - SimpleCamera(); + SimpleCamera(); SimpleCamera(const gtsam::Pose3& pose); SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, @@ -634,14 +634,14 @@ virtual class SimpleCamera : gtsam::Value { const gtsam::Cal3_S2& K); // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; // Standard Interface gtsam::Pose3 pose() const; gtsam::Cal3_S2 calibration(); - // Manifold + // Manifold gtsam::SimpleCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::SimpleCamera& T2) const; size_t dim() const; @@ -650,8 +650,8 @@ virtual class SimpleCamera : gtsam::Value { // Transformations and measurement functions static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& point); // FIXME, overload }; @@ -659,38 +659,38 @@ virtual class SimpleCamera : gtsam::Value { // TODO: Add this back in when Cal3DS2 has a calibrate function //template //virtual class PinholeCamera : gtsam::Value { -// // Standard Constructors and Named Constructors -// PinholeCamera(); -// PinholeCamera(const gtsam::Pose3& pose); -// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); -// static This Level(const gtsam::Cal3DS2& K, -// const gtsam::Pose2& pose, double height); -// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload -// static This Lookat(const gtsam::Point3& eye, -// const gtsam::Point3& target, const gtsam::Point3& upVector, -// const gtsam::Cal3DS2& K); +// // Standard Constructors and Named Constructors +// PinholeCamera(); +// PinholeCamera(const gtsam::Pose3& pose); +// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); +// static This Level(const gtsam::Cal3DS2& K, +// const gtsam::Pose2& pose, double height); +// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload +// static This Lookat(const gtsam::Point3& eye, +// const gtsam::Point3& target, const gtsam::Point3& upVector, +// const gtsam::Cal3DS2& K); // -// // Testable -// void print(string s) const; -// bool equals(const This& camera, double tol) const; +// // Testable +// void print(string s) const; +// bool equals(const This& camera, double tol) const; // -// // Standard Interface -// gtsam::Pose3 pose() const; -// CALIBRATION calibration() const; +// // Standard Interface +// gtsam::Pose3 pose() const; +// CALIBRATION calibration() const; // -// // Manifold -// This retract(const Vector& d) const; -// Vector localCoordinates(const This& T2) const; -// size_t dim() const; -// static size_t Dim(); +// // Manifold +// This retract(const Vector& d) const; +// Vector localCoordinates(const This& T2) const; +// size_t dim() const; +// static size_t Dim(); // -// // Transformations and measurement functions -// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); -// pair projectSafe(const gtsam::Point3& pw) const; -// gtsam::Point2 project(const gtsam::Point3& point); -// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; -// double range(const gtsam::Point3& point); -// double range(const gtsam::Pose3& point); // FIXME, overload +// // Transformations and measurement functions +// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); +// pair projectSafe(const gtsam::Point3& pw) const; +// gtsam::Point2 project(const gtsam::Point3& point); +// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; +// double range(const gtsam::Point3& point); +// double range(const gtsam::Pose3& point); // FIXME, overload //}; //************************************************************************* @@ -698,25 +698,25 @@ virtual class SimpleCamera : gtsam::Value { //************************************************************************* #include class Permutation { - // Standard Constructors and Named Constructors - Permutation(); - Permutation(size_t nVars); - static gtsam::Permutation Identity(size_t nVars); + // Standard Constructors and Named Constructors + Permutation(); + Permutation(size_t nVars); + static gtsam::Permutation Identity(size_t nVars); - // Testable - void print(string s) const; - bool equals(const gtsam::Permutation& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Permutation& rhs, double tol) const; - // Standard interface - size_t at(size_t variable) const; - size_t size() const; - bool empty() const; - void resize(size_t newSize); - gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; - gtsam::Permutation* inverse() const; - // FIXME: Cannot currently wrap std::vector - //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); - //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); + // Standard interface + size_t at(size_t variable) const; + size_t size() const; + bool empty() const; + void resize(size_t newSize); + gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; + gtsam::Permutation* inverse() const; + // FIXME: Cannot currently wrap std::vector + //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); + //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); gtsam::Permutation* partialPermutation(const gtsam::Permutation& selector, const gtsam::Permutation& partialPermutation) const; }; @@ -772,23 +772,23 @@ class IndexConditional { #include template virtual class BayesNet { - // Testable - void print(string s) const; - bool equals(const This& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const This& other, double tol) const; - // Standard interface - size_t size() const; - void printStats(string s) const; + // Standard interface + size_t size() const; + void printStats(string s) const; void saveGraph(string s) const; - CONDITIONAL* front() const; - CONDITIONAL* back() const; - void push_back(CONDITIONAL* conditional); - void push_front(CONDITIONAL* conditional); - void push_back(This& conditional); - void push_front(This& conditional); - void pop_front(); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); + CONDITIONAL* front() const; + CONDITIONAL* back() const; + void push_back(CONDITIONAL* conditional); + void push_front(CONDITIONAL* conditional); + void push_back(This& conditional); + void push_front(This& conditional); + void pop_front(); + void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -803,15 +803,15 @@ virtual class BayesTree { bool equals(const This& other, double tol) const; //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; + //size_t findParentClique(const gtsam::IndexVector& parents) const; size_t size(); void saveGraph(string s) const; CLIQUE* root() const; void clear(); void deleteCachedShorcuts(); void insert(const CLIQUE* subtree); - size_t numCachedShortcuts() const; - size_t numCachedSeparatorMarginals() const; + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; }; template @@ -824,8 +824,8 @@ virtual class BayesTreeClique { void print(string s) const; void printTree() const; // Default indent of "" void printTree(string indent) const; - size_t numCachedShortcuts() const; - size_t numCachedSeparatorMarginals() const; + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; CONDITIONAL* conditional() const; bool isRoot() const; @@ -972,19 +972,19 @@ virtual class Base { }; virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - //Matrix R() const; // FIXME: cannot parse!!! - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + //Matrix R() const; // FIXME: cannot parse!!! + bool equals(gtsam::noiseModel::Base& expected, double tol); + void print(string s) const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); -// Matrix R() const; // FIXME: cannot parse!!! - void print(string s) const; + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); +// Matrix R() const; // FIXME: cannot parse!!! + void print(string s) const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -1002,83 +1002,83 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { }; virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + void print(string s) const; }; virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; + static gtsam::noiseModel::Unit* Create(size_t dim); + void print(string s) const; }; }///\namespace noiseModel #include class Sampler { //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + Sampler(int seed); //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); + Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; class VectorValues { //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - VectorValues(size_t nVars, size_t varDim); + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + VectorValues(size_t nVars, size_t varDim); - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - static gtsam::VectorValues Zero(size_t nVars, size_t varDim); - static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other); + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + static gtsam::VectorValues Zero(size_t nVars, size_t varDim); + static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other); - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - size_t dim() const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + size_t dim() const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; - //Advanced Interface - void resizeLike(const gtsam::VectorValues& other); - void resize(size_t nVars, size_t varDim); - void setZero(); + //Advanced Interface + void resizeLike(const gtsam::VectorValues& other); + void resize(size_t nVars, size_t varDim); + void setZero(); //FIXME: Parse errors with vector() - //const Vector& vector() const; - //Vector& vector(); - bool hasSameStructure(const gtsam::VectorValues& other) const; + //const Vector& vector() const; + //Vector& vector(); + bool hasSameStructure(const gtsam::VectorValues& other) const; double dot(const gtsam::VectorValues& V) const; double norm() const; }; class GaussianConditional { //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, Vector sigmas); - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - size_t dim() const; + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + size_t dim() const; - //Advanced Interface - Matrix computeInformation() const; + //Advanced Interface + Matrix computeInformation() const; gtsam::JacobianFactor* toFactor() const; void solveInPlace(gtsam::VectorValues& x) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; @@ -1087,20 +1087,20 @@ class GaussianConditional { class GaussianDensity { //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas); + GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas); - //Standard Interface - void print(string s) const; - Vector mean() const; - Matrix information() const; - Matrix covariance() const; + //Standard Interface + void print(string s) const; + Vector mean() const; + Matrix information() const; + Matrix covariance() const; }; typedef gtsam::BayesNet GaussianBayesNetBase; virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase { //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); }; //Non-Class methods found in GaussianBayesNet.h @@ -1126,41 +1126,41 @@ typedef gtsam::BayesTreeClique GaussianBayesTreeCliq typedef gtsam::BayesTree GaussianBayesTreeBase; virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase { // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesNet& bn); - GaussianBayesTree(const gtsam::GaussianBayesNet& other); + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesNet& bn); + GaussianBayesTree(const gtsam::GaussianBayesNet& other); }; virtual class GaussianFactor { - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* negate() const; - size_t size() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* negate() const; + size_t size() const; }; virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors - JacobianFactor(); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactor& factor); - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; - //Standard Interface + //Standard Interface gtsam::GaussianFactor* negate() const; bool empty() const; Matrix getA() const; @@ -1190,81 +1190,81 @@ virtual class JacobianFactor : gtsam::GaussianFactor { virtual class HessianFactor : gtsam::GaussianFactor { //Constructors - HessianFactor(const gtsam::HessianFactor& gf); - HessianFactor(); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianConditional& cg); - HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(const gtsam::HessianFactor& gf); + HessianFactor(); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianConditional& cg); + HessianFactor(const gtsam::GaussianFactor& factor); - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; - //Standard Interface - size_t rows() const; - gtsam::GaussianFactor* negate() const; - Matrix info() const; - double constantTerm() const; - Vector linearTerm() const; - Matrix computeInformation() const; + //Standard Interface + size_t rows() const; + gtsam::GaussianFactor* negate() const; + Matrix info() const; + double constantTerm() const; + Vector linearTerm() const; + Matrix computeInformation() const; - //Advanced Interface - void partialCholesky(size_t nrFrontals); + //Advanced Interface + void partialCholesky(size_t nrFrontals); gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); void assertInvariants() const; }; class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN); + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN); - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; - // Inference - pair eliminateFrontals(size_t nFrontals) const; + // Inference + pair eliminateFrontals(size_t nFrontals) const; - // Building the graph - void push_back(gtsam::GaussianFactor* factor); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); + // Building the graph + void push_back(gtsam::GaussianFactor* factor); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); //Permutations - void permuteWithInverse(const gtsam::Permutation& inversePermutation); + void permuteWithInverse(const gtsam::Permutation& inversePermutation); - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; - // combining - static gtsam::GaussianFactorGraph combine2( - const gtsam::GaussianFactorGraph& lfg1, - const gtsam::GaussianFactorGraph& lfg2); - void combine(const gtsam::GaussianFactorGraph& lfg); + // combining + static gtsam::GaussianFactorGraph combine2( + const gtsam::GaussianFactorGraph& lfg1, + const gtsam::GaussianFactorGraph& lfg2); + void combine(const gtsam::GaussianFactorGraph& lfg); - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - Matrix augmentedHessian() const; - pair hessian() const; + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + Matrix augmentedHessian() const; + pair hessian() const; }; //Non-Class functions in GaussianFactorGraph.h @@ -1304,15 +1304,15 @@ virtual class GaussianISAM : gtsam::GaussianBayesTree { #include class GaussianSequentialSolver { //Constructors - GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, - bool useQR); + GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, + bool useQR); - //Standard Interface - void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); - gtsam::GaussianBayesNet* eliminate() const; - gtsam::VectorValues* optimize() const; - gtsam::GaussianFactor* marginalFactor(size_t j) const; - Matrix marginalCovariance(size_t j) const; + //Standard Interface + void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); + gtsam::GaussianBayesNet* eliminate() const; + gtsam::VectorValues* optimize() const; + gtsam::GaussianFactor* marginalFactor(size_t j) const; + Matrix marginalCovariance(size_t j) const; }; #include @@ -1360,21 +1360,21 @@ class SubgraphSolver { #include class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); }; //************************************************************************* @@ -1391,12 +1391,12 @@ class Ordering { // Standard Constructors and Named Constructors Ordering(); - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; - // Standard interface - size_t nVars() const; + // Standard interface + size_t nVars() const; size_t size() const; size_t at(size_t key) const; bool exists(size_t key) const; @@ -1407,80 +1407,80 @@ class Ordering { }; class InvertedOrdering { - InvertedOrdering(); + InvertedOrdering(); - // FIXME: add bracket operator overload + // FIXME: add bracket operator overload - bool empty() const; - size_t size() const; - bool count(size_t index) const; // Use as a boolean function with implicit cast + bool empty() const; + size_t size() const; + bool count(size_t index) const; // Use as a boolean function with implicit cast - void clear(); + void clear(); }; class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t i) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - void add(const gtsam::NonlinearFactor* factor); - gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, - const gtsam::Ordering& ordering) const; - gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactorGraph clone() const; + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + void add(const gtsam::NonlinearFactor* factor); + gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, + const gtsam::Ordering& ordering) const; + gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactorGraph clone() const; }; virtual class NonlinearFactor { - void print(string s) const; - void printKeys(string s) const; - void equals(const gtsam::NonlinearFactor& other, double tol) const; - gtsam::KeyVector keys() const; - size_t size() const; - size_t dim() const; - double error(const gtsam::Values& c) const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen + void print(string s) const; + void printKeys(string s) const; + void equals(const gtsam::NonlinearFactor& other, double tol) const; + gtsam::KeyVector keys() const; + size_t size() const; + size_t dim() const; + double error(const gtsam::Values& c) const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen }; #include class Values { - Values(); - Values(const gtsam::Values& other); + Values(); + Values(const gtsam::Values& other); - size_t size() const; - bool empty() const; + size_t size() const; + bool empty() const; void clear(); size_t dim() const; - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; - void insert(size_t j, const gtsam::Value& value); + void insert(size_t j, const gtsam::Value& value); void insert(const gtsam::Values& values); void update(size_t j, const gtsam::Value& val); void update(const gtsam::Values& values); void erase(size_t j); void swap(gtsam::Values& values); - bool exists(size_t j) const; - gtsam::Value at(size_t j) const; - gtsam::KeyList keys() const; + bool exists(size_t j) const; + gtsam::Value at(size_t j) const; + gtsam::KeyList keys() const; gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const; gtsam::Ordering* orderingArbitrary(size_t firstVar) const; @@ -1493,86 +1493,86 @@ class Values { // Actually a FastList #include class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); + KeyList(); + KeyList(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void sort(); - void remove(size_t key); + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void sort(); + void remove(size_t key); }; // Actually a FastSet #include class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); + KeySet(); + KeySet(const gtsam::KeySet& other); + KeySet(const gtsam::KeyVector& other); + KeySet(const gtsam::KeyList& other); - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t key); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + // structure specific methods + void insert(size_t key); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists }; // Actually a FastVector #include class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - KeyVector(const gtsam::KeySet& other); - KeyVector(const gtsam::KeyList& other); + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + KeyVector(const gtsam::KeySet& other); + KeyVector(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; }; #include class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; }; class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; }; //************************************************************************* @@ -1581,31 +1581,31 @@ class JointMarginal { #include virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s) const; + NonlinearOptimizerParams(); + void print(string s) const; - size_t getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; + size_t getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; - void setMaxIterations(size_t value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + void setMaxIterations(size_t value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); }; #include virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); - string getLinearSolverType() const; - - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); + string getLinearSolverType() const; + + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); bool isMultifrontal() const; bool isSequential() const; @@ -1615,12 +1615,12 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { #include virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { - GaussNewtonParams(); + GaussNewtonParams(); }; #include virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { - LevenbergMarquardtParams(); + LevenbergMarquardtParams(); double getlambdaInitial() const; double getlambdaFactor() const; @@ -1635,39 +1635,39 @@ virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { #include virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { - DoglegParams(); + DoglegParams(); - double getDeltaInitial() const; - string getVerbosityDL() const; + double getDeltaInitial() const; + string getVerbosityDL() const; - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; }; virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - void iterate() const; + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; }; virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); }; virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; }; virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; void print(string str) const; }; @@ -1798,12 +1798,12 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { #include class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); + NonlinearISAM(); + NonlinearISAM(int reorderInterval); void print(string s) const; void printStats() const; void saveGraph(string s) const; - gtsam::Values estimate() const; + gtsam::Values estimate() const; Matrix marginalCovariance(size_t key) const; int reorderInterval() const; int reorderCounter() const; @@ -1832,31 +1832,31 @@ class NonlinearISAM { #include template virtual class PriorFactor : gtsam::NonlinearFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; #include template virtual class BetweenFactor : gtsam::NonlinearFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; #include template virtual class NonlinearEquality : gtsam::NonlinearFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); }; #include template virtual class RangeFactor : gtsam::NonlinearFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::RangeFactor RangeFactorPosePoint2; @@ -1872,7 +1872,7 @@ typedef gtsam::RangeFactor RangeFactor #include template virtual class BearingFactor : gtsam::NonlinearFactor { - BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingFactor BearingFactor2D; @@ -1881,7 +1881,7 @@ typedef gtsam::BearingFactor BearingFa #include template virtual class BearingRangeFactor : gtsam::NonlinearFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; @@ -1890,10 +1890,10 @@ typedef gtsam::BearingRangeFactor Bear #include template virtual class GenericProjectionFactor : gtsam::NonlinearFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; // FIXME: Add Cal3DS2 when it has a 'calibrate' function @@ -1903,8 +1903,8 @@ typedef gtsam::GenericProjectionFactor template virtual class GeneralSFMFactor : gtsam::NonlinearFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; // FIXME: Add Cal3DS2 when it has a 'calibrate' function @@ -1913,18 +1913,18 @@ typedef gtsam::GeneralSFMFactor GeneralSFMFa // FIXME: Add Cal3DS2 when it has a 'calibrate' function template virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; }; #include template virtual class GenericStereoFactor : gtsam::NonlinearFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; }; typedef gtsam::GenericStereoFactor GenericStereoFactor3D; @@ -1932,11 +1932,11 @@ typedef gtsam::GenericStereoFactor GenericStereoFac pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); + gtsam::noiseModel::Diagonal* model, int maxID); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); + gtsam::noiseModel::Diagonal* model); //************************************************************************* diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index c253ed522..55693d47d 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -32,15 +32,15 @@ extern "C" { /* All versions of CCOLAMD will include the following definitions. * As an example, to test if the version you are using is 1.3 or later: * - * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... + * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... * * This also works during compile-time: * - * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) - * printf ("This is version 1.3 or later\n") ; - * #else - * printf ("This is an early version\n") ; - * #endif + * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) + * printf ("This is version 1.3 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif */ #define CCOLAMD_DATE "Jan 25, 2011" @@ -49,7 +49,7 @@ extern "C" { #define CCOLAMD_SUB_VERSION 7 #define CCOLAMD_SUBSUB_VERSION 3 #define CCOLAMD_VERSION \ - CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) + CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) /* ========================================================================== */ /* === Knob and statistics definitions ====================================== */ @@ -94,20 +94,20 @@ extern "C" { #define CCOLAMD_NEWLY_EMPTY_COL 10 /* error codes returned in stats [3]: */ -#define CCOLAMD_OK (0) -#define CCOLAMD_OK_BUT_JUMBLED (1) -#define CCOLAMD_ERROR_A_not_present (-1) -#define CCOLAMD_ERROR_p_not_present (-2) -#define CCOLAMD_ERROR_nrow_negative (-3) -#define CCOLAMD_ERROR_ncol_negative (-4) -#define CCOLAMD_ERROR_nnz_negative (-5) -#define CCOLAMD_ERROR_p0_nonzero (-6) -#define CCOLAMD_ERROR_A_too_small (-7) -#define CCOLAMD_ERROR_col_length_negative (-8) -#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) -#define CCOLAMD_ERROR_out_of_memory (-10) -#define CCOLAMD_ERROR_invalid_cmember (-11) -#define CCOLAMD_ERROR_internal_error (-999) +#define CCOLAMD_OK (0) +#define CCOLAMD_OK_BUT_JUMBLED (1) +#define CCOLAMD_ERROR_A_not_present (-1) +#define CCOLAMD_ERROR_p_not_present (-2) +#define CCOLAMD_ERROR_nrow_negative (-3) +#define CCOLAMD_ERROR_ncol_negative (-4) +#define CCOLAMD_ERROR_nnz_negative (-5) +#define CCOLAMD_ERROR_p0_nonzero (-6) +#define CCOLAMD_ERROR_A_too_small (-7) +#define CCOLAMD_ERROR_col_length_negative (-8) +#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) +#define CCOLAMD_ERROR_out_of_memory (-10) +#define CCOLAMD_ERROR_invalid_cmember (-11) +#define CCOLAMD_ERROR_internal_error (-999) /* ========================================================================== */ /* === Prototypes of user-callable routines ================================= */ @@ -116,45 +116,45 @@ extern "C" { /* define UF_long */ #include "UFconfig.h" -size_t ccolamd_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - int nnz, /* nonzeros in A */ - int n_row, /* number of rows in A */ - int n_col /* number of columns in A */ + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ ) ; -size_t ccolamd_l_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - UF_long nnz, /* nonzeros in A */ - UF_long n_row, /* number of rows in A */ - UF_long n_col /* number of columns in A */ + UF_long nnz, /* nonzeros in A */ + UF_long n_row, /* number of rows in A */ + UF_long n_col /* number of columns in A */ ) ; -void ccolamd_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -void ccolamd_l_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -int ccolamd /* returns (1) if successful, (0) otherwise*/ -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +int ccolamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ - int cmember [ ] /* Constraint set of A, of size n_col */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int cmember [ ] /* Constraint set of A, of size n_col */ ) ; -UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ +UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ ( UF_long n_row, UF_long n_col, @@ -166,23 +166,23 @@ UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ UF_long cmember [ ] ) ; -int csymamd /* return (1) if OK, (0) otherwise */ +int csymamd /* return (1) if OK, (0) otherwise */ ( - int n, /* number of rows and columns of A */ - int A [ ], /* row indices of A */ - int p [ ], /* column pointers of A */ - int perm [ ], /* output permutation, size n_col+1 */ + int n, /* number of rows and columns of A */ + int A [ ], /* row indices of A */ + int p [ ], /* column pointers of A */ + int perm [ ], /* output permutation, size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */ - int stats [CCOLAMD_STATS], /* output statistics and error codes */ + int stats [CCOLAMD_STATS], /* output statistics and error codes */ void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */ - /* mxCalloc (for MATLAB mexFunction) */ - void (*release) (void *), /* pointer to free (ANSI C) or */ - /* mxFree (for MATLAB mexFunction) */ - int cmember [ ], /* Constraint set of A */ - int stype /* 0: use both parts, >0: upper, <0: lower */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *), /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ + int cmember [ ], /* Constraint set of A */ + int stype /* 0: use both parts, >0: upper, <0: lower */ ) ; -UF_long csymamd_l /* same as csymamd, but with UF_long integers */ +UF_long csymamd_l /* same as csymamd, but with UF_long integers */ ( UF_long n, UF_long A [ ], @@ -227,26 +227,26 @@ void csymamd_l_report */ int ccolamd2 -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ /* each Front_ array is of size n_col+1: */ - int Front_npivcol [ ], /* # pivot cols in each front */ - int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ - int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ - int Front_parent [ ], /* parent of each front */ - int Front_cols [ ], /* link list of pivot columns for each front */ - int *p_nfr, /* total number of frontal matrices */ - int InFront [ ], /* InFront [row] = f if row in front f */ - int cmember [ ] /* Constraint set of A */ + int Front_npivcol [ ], /* # pivot cols in each front */ + int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ + int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ + int Front_parent [ ], /* parent of each front */ + int Front_cols [ ], /* link list of pivot columns for each front */ + int *p_nfr, /* total number of frontal matrices */ + int InFront [ ], /* InFront [row] = f if row in front f */ + int cmember [ ] /* Constraint set of A */ ) ; -UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ +UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ ( UF_long n_row, UF_long n_col, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index a5e3d5469..ef257aded 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -514,12 +514,12 @@ struct solve_retval, Rhs> typedef typename LDLTType::RealScalar RealScalar; const Diagonal vectorD = dec().vectorD(); RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits::epsilon(), - RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS + RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS for (Index i = 0; i < vectorD.size(); ++i) { if(abs(vectorD(i)) > tolerance) - dst.row(i) /= vectorD(i); + dst.row(i) /= vectorD(i); else - dst.row(i).setZero(); + dst.row(i).setZero(); } // dst = L^-T (D^-1 L^-1 P b) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h index 91b4fa1e2..327c3f3ea 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -324,7 +324,7 @@ void ComplexEigenSolver::sortEigenvalues(bool computeEigenvectors) k += i; std::swap(m_eivalues[k],m_eivalues[i]); if(computeEigenvectors) - m_eivec.col(i).swap(m_eivec.col(k)); + m_eivec.col(i).swap(m_eivec.col(k)); } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h index f9365ae59..95d5dcdcb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h @@ -547,7 +547,7 @@ void EigenSolver::doComputeEigenvectors() if ((vr == 0.0) && (vi == 0.0)) vr = eps * norm * (internal::abs(w) + internal::abs(q) + internal::abs(x) + internal::abs(y) + internal::abs(lastw)); - std::complex cc = cdiv(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra,vr,vi); + std::complex cc = cdiv(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra,vr,vi); m_matT.coeffRef(i,n-1) = internal::real(cc); m_matT.coeffRef(i,n) = internal::imag(cc); if (internal::abs(x) > (internal::abs(lastw) + internal::abs(q))) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h index 88e63eba4..4fce56f65 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h @@ -197,8 +197,8 @@ template class HessenbergDecomposition /** \brief Returns the internal representation of the decomposition * - * \returns a const reference to a matrix with the internal representation - * of the decomposition. + * \returns a const reference to a matrix with the internal representation + * of the decomposition. * * \pre Either the constructor HessenbergDecomposition(const MatrixType&) * or the member function compute(const MatrixType&) has been called diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h index a004e7e63..bd0f6d93f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h @@ -141,10 +141,10 @@ MatrixBase::operatorNorm() const // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough. return internal::sqrt((m_eval*m_eval.adjoint()) .eval() - .template selfadjointView() - .eigenvalues() - .maxCoeff() - ); + .template selfadjointView() + .eigenvalues() + .maxCoeff() + ); } /** \brief Computes the L2 operator norm diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h index e8f0ac5d1..5b03a4a52 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -200,8 +200,8 @@ template class Tridiagonalization /** \brief Returns the internal representation of the decomposition * - * \returns a const reference to a matrix with the internal representation - * of the decomposition. + * \returns a const reference to a matrix with the internal representation + * of the decomposition. * * \pre Either the constructor Tridiagonalization(const MatrixType&) or * the member function compute(const MatrixType&) has been called before diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 75083363c..99f069801 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -179,7 +179,7 @@ public: bool isApprox(const QuaternionBase& other, RealScalar prec = NumTraits::dummy_precision()) const { return coeffs().isApprox(other.coeffs(), prec); } - /** return the result vector of \a v through the rotation*/ + /** return the result vector of \a v through the rotation*/ EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; /** \returns \c *this with scalar type casted to \a NewScalarType diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 9550b6bf6..a98829ade 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -487,7 +487,7 @@ struct solve_retval, Rhs> // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T c.applyOnTheLeft(householderSequence(dec().matrixQR(), dec().hCoeffs()) .setLength(dec().nonzeroPivots()) - .transpose() + .transpose() ); dec().matrixQR() diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h index 6c3eb6858..60c258be4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h @@ -27,7 +27,7 @@ namespace Eigen { -#define DECL_GSSVX(PREFIX,FLOATTYPE,KEYTYPE) \ +#define DECL_GSSVX(PREFIX,FLOATTYPE,KEYTYPE) \ extern "C" { \ typedef struct { FLOATTYPE for_lu; FLOATTYPE total_needed; int expansions; } PREFIX##mem_usage_t; \ extern void PREFIX##gssvx(superlu_options_t *, SuperMatrix *, int *, int *, int *, \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h b/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h index 6fce99ed5..8fefd80ef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h @@ -159,49 +159,49 @@ int BLASFUNC(qrotm) (int *, double *, int *, double *, int *, double *); /* Level 2 routines */ int BLASFUNC(sger)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(dger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(qger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(cgeru)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(cgerc)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(zgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(zgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(sgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(strsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpsv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpsv) (char *, char *, char *, int *, double *, double *, int *); @@ -211,17 +211,17 @@ int BLASFUNC(ztpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(xtpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(strmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpmv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpmv) (char *, char *, char *, int *, double *, double *, int *); @@ -245,121 +245,121 @@ int BLASFUNC(ztbsv) (char *, char *, char *, int *, int *, double *, int *, doub int BLASFUNC(xtbsv) (char *, char *, char *, int *, int *, double *, int *, double *, int *); int BLASFUNC(ssymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(sspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(csyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ssyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(dsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(qsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(csyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(sspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(dspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(qspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(cspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(zspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(xspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(sspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(dspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(qspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cher) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(chpr) (char *, int *, float *, float *, int *, float *); int BLASFUNC(zhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(xhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(cher2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(chpr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(chemv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chpmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(snorm)(char *, int *, int *, float *, int *); int BLASFUNC(dnorm)(char *, int *, int *, double *, int *); @@ -367,205 +367,205 @@ int BLASFUNC(cnorm)(char *, int *, int *, float *, int *); int BLASFUNC(znorm)(char *, int *, int *, double *, int *); int BLASFUNC(sgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); /* Level 3 routines */ int BLASFUNC(sgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(dgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(qgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm3m)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(sge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(dge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(cge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(zge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(strsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(strmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ssymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(dsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(qsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(csyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(ssyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(qsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(csyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(chemm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chemm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cherk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(cher2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(cher2m)(char *, char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(sgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(cgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(sgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgetf2)(int *, int *, float *, int *, int *, int *); int BLASFUNC(dgetf2)(int *, int *, double *, int *, int *, int *); diff --git a/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp b/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp index 8e4b60c2b..b0950c8f1 100644 --- a/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp +++ b/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp @@ -21,16 +21,16 @@ using namespace Eigen; int main(int argc, char *argv[]) { - MATTYPE I = MATTYPE::Ones(MATSIZE,MATSIZE); - MATTYPE m(MATSIZE,MATSIZE); - for(int i = 0; i < MATSIZE; i++) for(int j = 0; j < MATSIZE; j++) - { - m(i,j) = (i+j+1)/(MATSIZE*MATSIZE); - } - for(int a = 0; a < REPEAT; a++) - { - m = I + 0.0001 * (m + m*m); - } - cout << m(0,0) << endl; - return 0; + MATTYPE I = MATTYPE::Ones(MATSIZE,MATSIZE); + MATTYPE m(MATSIZE,MATSIZE); + for(int i = 0; i < MATSIZE; i++) for(int j = 0; j < MATSIZE; j++) + { + m(i,j) = (i+j+1)/(MATSIZE*MATSIZE); + } + for(int a = 0; a < REPEAT; a++) + { + m = I + 0.0001 * (m + m*m); + } + cout << m(0,0) << endl; + return 0; } diff --git a/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp b/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp index 62437435e..b798d4f51 100644 --- a/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp +++ b/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp @@ -20,16 +20,16 @@ using namespace Eigen; int main(int argc, char *argv[]) { - VECTYPE I = VECTYPE::Ones(VECSIZE); - VECTYPE m(VECSIZE,1); - for(int i = 0; i < VECSIZE; i++) - { - m[i] = 0.1 * i/VECSIZE; - } - for(int a = 0; a < REPEAT; a++) - { - m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4); - } - cout << m[0] << endl; - return 0; + VECTYPE I = VECTYPE::Ones(VECSIZE); + VECTYPE m(VECSIZE,1); + for(int i = 0; i < VECSIZE; i++) + { + m[i] = 0.1 * i/VECSIZE; + } + for(int a = 0; a < REPEAT; a++) + { + m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4); + } + cout << m[0] << endl; + return 0; } diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h index d2330d06b..bee46e454 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h @@ -25,30 +25,30 @@ /* --- To print date and time of compilation of current source on stdout --- */ # if defined ( __GNUC__ ) -# define COMPILER "g++" ; +# define COMPILER "g++" ; # elif defined ( __sun ) -# define COMPILER "CC" ; +# define COMPILER "CC" ; # elif defined ( __KCC ) -# define COMPILER "KCC" ; +# define COMPILER "KCC" ; # elif defined ( __PGI ) -# define COMPILER "pgCC" ; +# define COMPILER "pgCC" ; # else -# define COMPILER "undefined" ; +# define COMPILER "undefined" ; # endif # ifdef INFOS_COMPILATION # error INFOS_COMPILATION already defined # endif -# define INFOS_COMPILATION {\ - cerr << flush;\ - cout << __FILE__ ;\ - cout << " [" << __LINE__ << "] : " ;\ - cout << "COMPILED with " << COMPILER ;\ - cout << ", " << __DATE__ ; \ - cout << " at " << __TIME__ << endl ;\ - cout << "\n\n" ;\ - cout << flush ;\ - } +# define INFOS_COMPILATION {\ + cerr << flush;\ + cout << __FILE__ ;\ + cout << " [" << __LINE__ << "] : " ;\ + cout << "COMPILED with " << COMPILER ;\ + cout << ", " << __DATE__ ; \ + cout << " at " << __TIME__ << endl ;\ + cout << "\n\n" ;\ + cout << flush ;\ + } # ifdef _DEBUG_ diff --git a/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h b/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h index 28f3a4e57..2ab9c4b9d 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h +++ b/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h @@ -180,49 +180,49 @@ int BLASFUNC(qrotm) (int *, double *, int *, double *, int *, double *); /* Level 2 routines */ int BLASFUNC(sger)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(dger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(qger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(cgeru)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(cgerc)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(zgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(zgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(sgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(strsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpsv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpsv) (char *, char *, char *, int *, double *, double *, int *); @@ -232,17 +232,17 @@ int BLASFUNC(ztpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(xtpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(strmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpmv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpmv) (char *, char *, char *, int *, double *, double *, int *); @@ -266,121 +266,121 @@ int BLASFUNC(ztbsv) (char *, char *, char *, int *, int *, double *, int *, doub int BLASFUNC(xtbsv) (char *, char *, char *, int *, int *, double *, int *, double *, int *); int BLASFUNC(ssymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(sspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(csyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ssyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(dsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(qsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(csyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(sspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(dspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(qspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(cspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(zspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(xspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(sspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(dspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(qspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cher) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(chpr) (char *, int *, float *, float *, int *, float *); int BLASFUNC(zhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(xhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(cher2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(chpr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(chemv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chpmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(snorm)(char *, int *, int *, float *, int *); int BLASFUNC(dnorm)(char *, int *, int *, double *, int *); @@ -388,205 +388,205 @@ int BLASFUNC(cnorm)(char *, int *, int *, float *, int *); int BLASFUNC(znorm)(char *, int *, int *, double *, int *); int BLASFUNC(sgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); /* Level 3 routines */ int BLASFUNC(sgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(dgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(qgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm3m)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(sge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(dge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(cge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(zge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(strsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(strmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ssymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(dsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(qsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(csyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(ssyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(qsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(csyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(chemm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chemm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cherk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(cher2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(cher2m)(char *, char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(sgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(cgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(sgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgetf2)(int *, int *, float *, int *, int *, int *); int BLASFUNC(dgetf2)(int *, int *, double *, int *, int *, int *); diff --git a/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp b/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp index 0623ef0c2..e88f1eb69 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp +++ b/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp @@ -1,7 +1,7 @@ Matrix Acolmajor; Acolmajor << 8, 2, 2, 9, 9, 1, 4, 4, - 3, 5, 4, 5; + 3, 5, 4, 5; cout << "The matrix A:" << endl; cout << Acolmajor << endl << endl; diff --git a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp index 2adb2e213..aa90c2d1d 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp +++ b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp @@ -1,5 +1,5 @@ Matrix3f m; m.row(0) << 1, 2, 3; m.block(1,0,2,2) << 4, 5, 7, 8; -m.col(2).tail(2) << 6, 9; +m.col(2).tail(2) << 6, 9; std::cout << m; diff --git a/gtsam/3rdparty/Eigen/test/corners.cpp b/gtsam/3rdparty/Eigen/test/corners.cpp index 8d12c6146..d813b6b63 100644 --- a/gtsam/3rdparty/Eigen/test/corners.cpp +++ b/gtsam/3rdparty/Eigen/test/corners.cpp @@ -68,8 +68,8 @@ template void c cols = MatrixType::ColsAtCompileTime, r = CRows, c = CCols, - sr = SRows, - sc = SCols + sr = SRows, + sc = SCols }; VERIFY_IS_EQUAL((matrix.template topLeftCorner()), (matrix.template block(0,0))); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/main.h b/gtsam/3rdparty/Eigen/test/eigen2/main.h index 9d0defa39..6f126e1cf 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/main.h +++ b/gtsam/3rdparty/Eigen/test/eigen2/main.h @@ -114,7 +114,7 @@ namespace Eigen // see bug 89. The copy_bool here is working around a bug in gcc <= 4.3 #define eigen_assert(a) \ - if( (!Eigen::internal::copy_bool(a)) && (!no_more_assert) ) \ + if( (!Eigen::internal::copy_bool(a)) && (!no_more_assert) ) \ { \ Eigen::no_more_assert = true; \ throw Eigen::eigen_assert_exception(); \ @@ -400,7 +400,7 @@ int main(int argc, char *argv[]) if(!has_set_repeat) repeat = DEFAULT_REPEAT; std::cout << "Initializing random number generator with seed " << seed << std::endl; - std::srand(seed); + std::srand(seed); std::cout << "Repeating each test " << repeat << " times" << std::endl; Eigen::g_repeat = repeat; diff --git a/gtsam/3rdparty/Eigen/test/hessenberg.cpp b/gtsam/3rdparty/Eigen/test/hessenberg.cpp index 9d3dc7c84..1e1090f74 100644 --- a/gtsam/3rdparty/Eigen/test/hessenberg.cpp +++ b/gtsam/3rdparty/Eigen/test/hessenberg.cpp @@ -39,7 +39,7 @@ template void hessenberg(int size = Size) VERIFY_IS_APPROX(m, Q * H * Q.adjoint()); for(int row = 2; row < size; ++row) { for(int col = 0; col < row-1; ++col) { - VERIFY(H(row,col) == (typename MatrixType::Scalar)0); + VERIFY(H(row,col) == (typename MatrixType::Scalar)0); } } } diff --git a/gtsam/3rdparty/Eigen/test/schur_complex.cpp b/gtsam/3rdparty/Eigen/test/schur_complex.cpp index 15532e2cc..b2a9d6d6b 100644 --- a/gtsam/3rdparty/Eigen/test/schur_complex.cpp +++ b/gtsam/3rdparty/Eigen/test/schur_complex.cpp @@ -40,7 +40,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim ComplexMatrixType T = schurOfA.matrixT(); for(int row = 1; row < size; ++row) { for(int col = 0; col < row; ++col) { - VERIFY(T(row,col) == (typename MatrixType::Scalar)0); + VERIFY(T(row,col) == (typename MatrixType::Scalar)0); } } VERIFY_IS_APPROX(A.template cast(), U * T * U.adjoint()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h index e100617d1..c882d87a2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -69,74 +69,74 @@ namespace internal { */ template bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Preconditioner & precond, - int &iters, const int &restart, typename Dest::RealScalar & tol_error) { + int &iters, const int &restart, typename Dest::RealScalar & tol_error) { - using std::sqrt; - using std::abs; + using std::sqrt; + using std::abs; - typedef typename Dest::RealScalar RealScalar; - typedef typename Dest::Scalar Scalar; - typedef Matrix < RealScalar, Dynamic, 1 > RealVectorType; - typedef Matrix < Scalar, Dynamic, 1 > VectorType; - typedef Matrix < Scalar, Dynamic, Dynamic > FMatrixType; + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix < RealScalar, Dynamic, 1 > RealVectorType; + typedef Matrix < Scalar, Dynamic, 1 > VectorType; + typedef Matrix < Scalar, Dynamic, Dynamic > FMatrixType; - RealScalar tol = tol_error; - const int maxIters = iters; - iters = 0; + RealScalar tol = tol_error; + const int maxIters = iters; + iters = 0; - const int m = mat.rows(); + const int m = mat.rows(); - VectorType p0 = rhs - mat*x; - VectorType r0 = precond.solve(p0); -// RealScalar r0_sqnorm = r0.squaredNorm(); + VectorType p0 = rhs - mat*x; + VectorType r0 = precond.solve(p0); +// RealScalar r0_sqnorm = r0.squaredNorm(); - VectorType w = VectorType::Zero(restart + 1); + VectorType w = VectorType::Zero(restart + 1); - FMatrixType H = FMatrixType::Zero(m, restart + 1); - VectorType tau = VectorType::Zero(restart + 1); - std::vector < JacobiRotation < Scalar > > G(restart); + FMatrixType H = FMatrixType::Zero(m, restart + 1); + VectorType tau = VectorType::Zero(restart + 1); + std::vector < JacobiRotation < Scalar > > G(restart); - // generate first Householder vector - VectorType e; - RealScalar beta; - r0.makeHouseholder(e, tau.coeffRef(0), beta); - w(0)=(Scalar) beta; - H.bottomLeftCorner(m - 1, 1) = e; + // generate first Householder vector + VectorType e; + RealScalar beta; + r0.makeHouseholder(e, tau.coeffRef(0), beta); + w(0)=(Scalar) beta; + H.bottomLeftCorner(m - 1, 1) = e; - for (int k = 1; k <= restart; ++k) { + for (int k = 1; k <= restart; ++k) { - ++iters; + ++iters; - VectorType v = VectorType::Unit(m, k - 1), workspace(m); + VectorType v = VectorType::Unit(m, k - 1), workspace(m); - // apply Householder reflections H_{1} ... H_{k-1} to v - for (int i = k - 1; i >= 0; --i) { - v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); - } + // apply Householder reflections H_{1} ... H_{k-1} to v + for (int i = k - 1; i >= 0; --i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } - // apply matrix M to v: v = mat * v; - VectorType t=mat*v; - v=precond.solve(t); + // apply matrix M to v: v = mat * v; + VectorType t=mat*v; + v=precond.solve(t); - // apply Householder reflections H_{k-1} ... H_{1} to v - for (int i = 0; i < k; ++i) { - v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); - } + // apply Householder reflections H_{k-1} ... H_{1} to v + for (int i = 0; i < k; ++i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } - if (v.tail(m - k).norm() != 0.0) { + if (v.tail(m - k).norm() != 0.0) { - if (k <= restart) { + if (k <= restart) { - // generate new Householder vector + // generate new Householder vector VectorType e(m - k - 1); - RealScalar beta; - v.tail(m - k).makeHouseholder(e, tau.coeffRef(k), beta); - H.col(k).tail(m - k - 1) = e; + RealScalar beta; + v.tail(m - k).makeHouseholder(e, tau.coeffRef(k), beta); + H.col(k).tail(m - k - 1) = e; - // apply Householder reflection H_{k} to v - v.tail(m - k).applyHouseholderOnTheLeft(H.col(k).tail(m - k - 1), tau.coeffRef(k), workspace.data()); + // apply Householder reflection H_{k} to v + v.tail(m - k).applyHouseholderOnTheLeft(H.col(k).tail(m - k - 1), tau.coeffRef(k), workspace.data()); - } + } } if (k > 1) { @@ -207,9 +207,9 @@ bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Precondition - } - - return false; + } + + return false; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index 6cdd65748..e328badda 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -248,7 +248,7 @@ template EIGEN_STRONG_INLINE void MatrixExponential::pade9(const MatrixType &A) { const RealScalar b[] = {17643225600., 8821612800., 2075673600., 302702400., 30270240., - 2162160., 110880., 3960., 90., 1.}; + 2162160., 110880., 3960., 90., 1.}; MatrixType A2 = A * A; MatrixType A4 = A2 * A2; MatrixType A6 = A4 * A2; @@ -262,8 +262,8 @@ template EIGEN_STRONG_INLINE void MatrixExponential::pade13(const MatrixType &A) { const RealScalar b[] = {64764752532480000., 32382376266240000., 7771770303897600., - 1187353796428800., 129060195264000., 10559470521600., 670442572800., - 33522128640., 1323241920., 40840800., 960960., 16380., 182., 1.}; + 1187353796428800., 129060195264000., 10559470521600., 670442572800., + 33522128640., 1323241920., 40840800., 960960., 16380., 182., 1.}; MatrixType A2 = A * A; MatrixType A4 = A2 * A2; m_tmp1.noalias() = A4 * A2; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h index 859de7288..7c493e24d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h @@ -47,7 +47,7 @@ namespace Eigen { * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic */ template ::Scalar>::IsComplex> class MatrixFunction { @@ -267,13 +267,13 @@ void MatrixFunction::partitionEigenvalues() // Look for other element to add to the set for (Index j=i+1; jbegin(), qi->end(), diag(j)) == qi->end()) { - typename ListOfClusters::iterator qj = findCluster(diag(j)); - if (qj == m_clusters.end()) { - qi->push_back(diag(j)); - } else { - qi->insert(qi->end(), qj->begin(), qj->end()); - m_clusters.erase(qj); - } + typename ListOfClusters::iterator qj = findCluster(diag(j)); + if (qj == m_clusters.end()) { + qi->push_back(diag(j)); + } else { + qi->insert(qi->end(), qj->begin(), qj->end()); + m_clusters.erase(qj); + } } } } @@ -412,8 +412,8 @@ void MatrixFunction::computeOffDiagonal() DynMatrixType C = block(m_fT, blockIndex, blockIndex) * block(m_T, blockIndex, blockIndex+diagIndex); C -= block(m_T, blockIndex, blockIndex+diagIndex) * block(m_fT, blockIndex+diagIndex, blockIndex+diagIndex); for (Index k = blockIndex + 1; k < blockIndex + diagIndex; k++) { - C += block(m_fT, blockIndex, k) * block(m_T, k, blockIndex+diagIndex); - C -= block(m_T, blockIndex, k) * block(m_fT, k, blockIndex+diagIndex); + C += block(m_fT, blockIndex, k) * block(m_T, k, blockIndex+diagIndex); + C -= block(m_T, blockIndex, k) * block(m_fT, k, blockIndex+diagIndex); } block(m_fT, blockIndex, blockIndex+diagIndex) = solveTriangularSylvester(A, B, C); } @@ -466,19 +466,19 @@ typename MatrixFunction::DynMatrixType MatrixFunction AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i); - AX = AXmatrix(0,0); + Matrix AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i); + AX = AXmatrix(0,0); } // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj} Scalar XB; if (j == 0) { - XB = 0; + XB = 0; } else { - Matrix XBmatrix = X.row(i).head(j) * B.col(j).head(j); - XB = XBmatrix(0,0); + Matrix XBmatrix = X.row(i).head(j) * B.col(j).head(j); + XB = XBmatrix(0,0); } X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j)); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h index 97ab662fe..078545b90 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h @@ -118,7 +118,7 @@ void MatrixFunctionAtomic::computeMu() /** \brief Determine whether Taylor series has converged */ template bool MatrixFunctionAtomic::taylorConverged(Index s, const MatrixType& F, - const MatrixType& Fincr, const MatrixType& P) + const MatrixType& Fincr, const MatrixType& P) { const Index n = F.rows(); const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff(); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h index e6f25b73c..361c3df0c 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h @@ -168,7 +168,7 @@ void MatrixLogarithmAtomic::computeBig(const MatrixType& A, MatrixTy degree = getPadeDegree(normTminusI); int degree2 = getPadeDegree(normTminusI / RealScalar(2)); if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) - break; + break; ++numberOfExtraSquareRoots; } MatrixType sqrtT; @@ -309,10 +309,10 @@ void MatrixLogarithmAtomic::computePade6(MatrixType& result, const M const int degree = 6; const RealScalar nodes[] = { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L, 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L, - 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L }; + 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L }; const RealScalar weights[] = { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L, 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L, - 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L }; + 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L }; assert(degree <= maxPadeDegree); MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); result.setZero(T.rows(), T.rows()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h index 658cd334c..2f8f28363 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h @@ -75,17 +75,17 @@ class MatrixSquareRootQuasiTriangular void computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T); void compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i); void compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); template static void solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, - const SmallMatrixType& B, const SmallMatrixType& C); + const SmallMatrixType& B, const SmallMatrixType& C); const MatrixType& m_A; }; @@ -112,7 +112,7 @@ void MatrixSquareRootQuasiTriangular::compute(ResultType &result) // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T template void MatrixSquareRootQuasiTriangular::computeDiagonalPartOfSqrt(MatrixType& sqrtT, - const MatrixType& T) + const MatrixType& T) { const Index size = m_A.rows(); for (Index i = 0; i < size; i++) { @@ -131,25 +131,25 @@ void MatrixSquareRootQuasiTriangular::computeDiagonalPartOfSqrt(Matr // post: sqrtT is the square root of T. template void MatrixSquareRootQuasiTriangular::computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, - const MatrixType& T) + const MatrixType& T) { const Index size = m_A.rows(); for (Index j = 1; j < size; j++) { if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block - continue; + continue; for (Index i = j-1; i >= 0; i--) { if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block - continue; + continue; bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0); bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0); if (iBlockIs2x2 && jBlockIs2x2) - compute2x2offDiagonalBlock(sqrtT, T, i, j); + compute2x2offDiagonalBlock(sqrtT, T, i, j); else if (iBlockIs2x2 && !jBlockIs2x2) - compute2x1offDiagonalBlock(sqrtT, T, i, j); + compute2x1offDiagonalBlock(sqrtT, T, i, j); else if (!iBlockIs2x2 && jBlockIs2x2) - compute1x2offDiagonalBlock(sqrtT, T, i, j); + compute1x2offDiagonalBlock(sqrtT, T, i, j); else if (!iBlockIs2x2 && !jBlockIs2x2) - compute1x1offDiagonalBlock(sqrtT, T, i, j); + compute1x1offDiagonalBlock(sqrtT, T, i, j); } } } @@ -174,7 +174,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value(); sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j)); @@ -184,7 +184,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix rhs = T.template block<1,2>(i,j); if (j-i > 1) @@ -198,7 +198,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix rhs = T.template block<2,1>(i,j); if (j-i > 2) @@ -212,7 +212,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix A = sqrtT.template block<2,2>(i,i); Matrix B = sqrtT.template block<2,2>(j,j); @@ -229,10 +229,10 @@ template template void MatrixSquareRootQuasiTriangular ::solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, - const SmallMatrixType& B, const SmallMatrixType& C) + const SmallMatrixType& B, const SmallMatrixType& C) { EIGEN_STATIC_ASSERT((internal::is_same >::value), - EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT); + EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT); Matrix coeffMatrix = Matrix::Zero(); coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h index 3de68ec3a..84f0df583 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h @@ -47,17 +47,17 @@ class StdStemFunctions Scalar res; switch (n % 4) { case 0: - res = std::cos(x); - break; + res = std::cos(x); + break; case 1: - res = -std::sin(x); - break; + res = -std::sin(x); + break; case 2: - res = -std::cos(x); - break; + res = -std::cos(x); + break; case 3: - res = std::sin(x); - break; + res = std::sin(x); + break; } return res; } @@ -68,17 +68,17 @@ class StdStemFunctions Scalar res; switch (n % 4) { case 0: - res = std::sin(x); - break; + res = std::sin(x); + break; case 1: - res = std::cos(x); - break; + res = std::cos(x); + break; case 2: - res = -std::sin(x); - break; + res = -std::sin(x); + break; case 3: - res = -std::cos(x); - break; + res = -std::cos(x); + break; } return res; } @@ -89,26 +89,26 @@ class StdStemFunctions Scalar res; switch (n % 2) { case 0: - res = std::cosh(x); - break; + res = std::cosh(x); + break; case 1: - res = std::sinh(x); - break; + res = std::sinh(x); + break; } return res; } - + /** \brief Hyperbolic sine (and its derivatives). */ static Scalar sinh(Scalar x, int n) { Scalar res; switch (n % 2) { case 0: - res = std::sinh(x); - break; + res = std::sinh(x); + break; case 1: - res = std::cosh(x); - break; + res = std::cosh(x); + break; } return res; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h index 9cfe1d9f5..adaf33a8e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h @@ -253,9 +253,9 @@ bool saveMarket(const SparseMatrixType& mat, const std::string& filename, int sy for(int j=0; j typename EigenSolver::EigenvalueType eivals = es.eigenvalues(); for (typename MatrixType::Index i = 0; i < size; ++i) { if (eivals(i).imag() == 0 && eivals(i).real() < 0) - eivals(i) = -eivals(i); + eivals(i) = -eivals(i); } result = (es.eigenvectors() * eivals.asDiagonal() * es.eigenvectors().inverse()).real(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp index 5c23544ef..44012de3b 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp @@ -1,60 +1,60 @@ /* - Multi-precision real number class. C++ interface fo MPFR library. - Project homepage: http://www.holoborodko.com/pavel/ - Contact e-mail: pavel@holoborodko.com + Multi-precision real number class. C++ interface fo MPFR library. + Project homepage: http://www.holoborodko.com/pavel/ + Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2011 Pavel Holoborodko + Copyright (c) 2008-2011 Pavel Holoborodko - Core Developers: - Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. + Core Developers: + Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. - Contributors: - Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, - Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, - Tsai Chia Cheng, Alexei Zubanov. + Contributors: + Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, + Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, + Tsai Chia Cheng, Alexei Zubanov. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + **************************************************************************** + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - **************************************************************************** - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: + **************************************************************************** + **************************************************************************** + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. - 3. The name of the author may be used to endorse or promote products - derived from this software without specific prior written permission. + 3. The name of the author may be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. */ #include #include "mpreal.h" @@ -72,9 +72,9 @@ using std::istream; namespace mpfr{ -mp_rnd_t mpreal::default_rnd = MPFR_RNDN; //(mpfr_get_default_rounding_mode)(); -mp_prec_t mpreal::default_prec = 64; //(mpfr_get_default_prec)(); -int mpreal::default_base = 10; +mp_rnd_t mpreal::default_rnd = MPFR_RNDN; //(mpfr_get_default_rounding_mode)(); +mp_prec_t mpreal::default_prec = 64; //(mpfr_get_default_prec)(); +int mpreal::default_base = 10; int mpreal::double_bits = -1; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) @@ -86,93 +86,93 @@ mpreal::mpreal() { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,default_prec); - mpfr_set_ui(mp,0,default_rnd); + mpfr_init2(mp,default_prec); + mpfr_set_ui(mp,0,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpreal& u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,mpfr_get_prec(u.mp)); - mpfr_set(mp,u.mp,default_rnd); + mpfr_init2(mp,mpfr_get_prec(u.mp)); + mpfr_set(mp,u.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpfr_t u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,mpfr_get_prec(u)); - mpfr_set(mp,u,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,mpfr_get_prec(u)); + mpfr_set(mp,u,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpf_t u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) - mpfr_set_f(mp,u,default_rnd); + mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) + mpfr_set_f(mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpz_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_z(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_z(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpq_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_q(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_q(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif if(double_bits == -1 || fits_in_bits(u, double_bits)) { - mpfr_init2(mp,prec); - mpfr_set_d(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_d(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } else throw conversion_overflow(); @@ -182,65 +182,65 @@ mpreal::mpreal(const long double u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif mpfr_init2(mp,prec); - mpfr_set_ld(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_set_ld(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const unsigned long int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_ui(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const unsigned int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_ui(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const long int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_si(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_si(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } #if defined (MPREAL_HAVE_INT64_SUPPORT) @@ -248,26 +248,26 @@ mpreal::mpreal(const uint64_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_uj(mp, u, mode); + mpfr_init2(mp,prec); + mpfr_set_uj(mp, u, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_sj(mp, u, mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_sj(mp, u, mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } #endif @@ -275,157 +275,157 @@ mpreal::mpreal(const char* s, mp_prec_t prec, int base, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_str(mp, s, base, mode); + mpfr_init2(mp,prec); + mpfr_set_str(mp, s, base, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const std::string& s, mp_prec_t prec, int base, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_str(mp, s.c_str(), base, mode); + mpfr_init2(mp,prec); + mpfr_set_str(mp, s.c_str(), base, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::~mpreal() { - mpfr_clear(mp); + mpfr_clear(mp); } // Operators - Assignment mpreal& mpreal::operator=(const char* s) { - mpfr_t t; + mpfr_t t; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - if(0==mpfr_init_set_str(t,s,default_base,default_rnd)) - { - // We will rewrite mp anyway, so flash it and resize - mpfr_set_prec(mp,mpfr_get_prec(t)); - mpfr_set(mp,t,mpreal::default_rnd); - mpfr_clear(t); + if(0==mpfr_init_set_str(t,s,default_base,default_rnd)) + { + // We will rewrite mp anyway, so flash it and resize + mpfr_set_prec(mp,mpfr_get_prec(t)); + mpfr_set(mp,t,mpreal::default_rnd); + mpfr_clear(t); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; - }else{ - mpfr_clear(t); - } + }else{ + mpfr_clear(t); + } - return *this; + return *this; } const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2, p3; + mpreal a; + mp_prec_t p1, p2, p3; - p1 = v1.get_prec(); - p2 = v2.get_prec(); - p3 = v3.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); + p3 = v3.get_prec(); - a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); + a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); - mpfr_fma(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); - return a; + mpfr_fma(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); + return a; } const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2, p3; + mpreal a; + mp_prec_t p1, p2, p3; - p1 = v1.get_prec(); - p2 = v2.get_prec(); - p3 = v3.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); + p3 = v3.get_prec(); - a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); + a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); - mpfr_fms(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); - return a; + mpfr_fms(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); + return a; } const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2; + mpreal a; + mp_prec_t p1, p2; - p1 = v1.get_prec(); - p2 = v2.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); - a.set_prec(p1>p2?p1:p2); + a.set_prec(p1>p2?p1:p2); - mpfr_agm(a.mp, v1.mp, v2.mp, rnd_mode); + mpfr_agm(a.mp, v1.mp, v2.mp, rnd_mode); - return a; + return a; } const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_ptr* t; - unsigned long int i; + mpreal x; + mpfr_ptr* t; + unsigned long int i; - t = new mpfr_ptr[n]; - for (i=0;ixp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); + mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); - return a; + return a; } template std::string toString(T t, std::ios_base & (*f)(std::ios_base&)) { - std::ostringstream oss; - oss << f << t; - return oss.str(); + std::ostringstream oss; + oss << f << t; + return oss.str(); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) std::string mpreal::toString(const std::string& format) const { - char *s = NULL; - string out; + char *s = NULL; + string out; - if( !format.empty() ) - { - if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) - { - out = std::string(s); + if( !format.empty() ) + { + if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) + { + out = std::string(s); - mpfr_free_str(s); - } - } + mpfr_free_str(s); + } + } - return out; + return out; } #endif @@ -436,116 +436,116 @@ std::string mpreal::toString(int n, int b, mp_rnd_t mode) const (void)mode; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - // Use MPFR native function for output - char format[128]; - int digits; + // Use MPFR native function for output + char format[128]; + int digits; - digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); + digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); - sprintf(format,"%%.%dRNg",digits); // Default format + sprintf(format,"%%.%dRNg",digits); // Default format - return toString(std::string(format)); + return toString(std::string(format)); #else - char *s, *ns = NULL; - size_t slen, nslen; - mp_exp_t exp; - string out; + char *s, *ns = NULL; + size_t slen, nslen; + mp_exp_t exp; + string out; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - if(mpfr_inf_p(mp)) - { - if(mpfr_sgn(mp)>0) return "+Inf"; - else return "-Inf"; - } + if(mpfr_inf_p(mp)) + { + if(mpfr_sgn(mp)>0) return "+Inf"; + else return "-Inf"; + } - if(mpfr_zero_p(mp)) return "0"; - if(mpfr_nan_p(mp)) return "NaN"; + if(mpfr_zero_p(mp)) return "0"; + if(mpfr_nan_p(mp)) return "NaN"; - s = mpfr_get_str(NULL,&exp,b,0,mp,mode); - ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); + s = mpfr_get_str(NULL,&exp,b,0,mp,mode); + ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); - if(s!=NULL && ns!=NULL) - { - slen = strlen(s); - nslen = strlen(ns); - if(nslen<=slen) - { - mpfr_free_str(s); - s = ns; - slen = nslen; - } - else { - mpfr_free_str(ns); - } + if(s!=NULL && ns!=NULL) + { + slen = strlen(s); + nslen = strlen(ns); + if(nslen<=slen) + { + mpfr_free_str(s); + s = ns; + slen = nslen; + } + else { + mpfr_free_str(ns); + } - // Make human eye-friendly formatting if possible - if (exp>0 && static_cast(exp)s+exp) ptr--; + // Make human eye-friendly formatting if possible + if (exp>0 && static_cast(exp)s+exp) ptr--; - if(ptr==s+exp) out = string(s,exp+1); - else out = string(s,exp+1)+'.'+string(s+exp+1,ptr-(s+exp+1)+1); + if(ptr==s+exp) out = string(s,exp+1); + else out = string(s,exp+1)+'.'+string(s+exp+1,ptr-(s+exp+1)+1); - //out = string(s,exp+1)+'.'+string(s+exp+1); - } - else - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s+exp-1) ptr--; + //out = string(s,exp+1)+'.'+string(s+exp+1); + } + else + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s+exp-1) ptr--; - if(ptr==s+exp-1) out = string(s,exp); - else out = string(s,exp)+'.'+string(s+exp,ptr-(s+exp)+1); + if(ptr==s+exp-1) out = string(s,exp); + else out = string(s,exp)+'.'+string(s+exp,ptr-(s+exp)+1); - //out = string(s,exp)+'.'+string(s+exp); - } + //out = string(s,exp)+'.'+string(s+exp); + } - }else{ // exp<0 || exp>slen - if(s[0]=='-') - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s+1) ptr--; + }else{ // exp<0 || exp>slen + if(s[0]=='-') + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s+1) ptr--; - if(ptr==s+1) out = string(s,2); - else out = string(s,2)+'.'+string(s+2,ptr-(s+2)+1); + if(ptr==s+1) out = string(s,2); + else out = string(s,2)+'.'+string(s+2,ptr-(s+2)+1); - //out = string(s,2)+'.'+string(s+2); - } - else - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s) ptr--; + //out = string(s,2)+'.'+string(s+2); + } + else + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s) ptr--; - if(ptr==s) out = string(s,1); - else out = string(s,1)+'.'+string(s+1,ptr-(s+1)+1); + if(ptr==s) out = string(s,1); + else out = string(s,1)+'.'+string(s+1,ptr-(s+1)+1); - //out = string(s,1)+'.'+string(s+1); - } + //out = string(s,1)+'.'+string(s+1); + } - // Make final string - if(--exp) - { - if(exp>0) out += "e+"+mpfr::toString(exp,std::dec); - else out += "e"+mpfr::toString(exp,std::dec); - } - } + // Make final string + if(--exp) + { + if(exp>0) out += "e+"+mpfr::toString(exp,std::dec); + else out += "e"+mpfr::toString(exp,std::dec); + } + } - mpfr_free_str(s); - return out; - }else{ - return "conversion error!"; - } + mpfr_free_str(s); + return out; + }else{ + return "conversion error!"; + } #endif } @@ -554,43 +554,43 @@ std::string mpreal::toString(int n, int b, mp_rnd_t mode) const // I/O ostream& operator<<(ostream& os, const mpreal& v) { - return os<(os.precision())); + return os<(os.precision())); } istream& operator>>(istream &is, mpreal& v) { - string tmp; - is >> tmp; - mpfr_set_str(v.mp, tmp.c_str(),mpreal::default_base,mpreal::default_rnd); - return is; + string tmp; + is >> tmp; + mpfr_set_str(v.mp, tmp.c_str(),mpreal::default_base,mpreal::default_rnd); + return is; } #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - // Optimized dynamic memory allocation/(re-)deallocation. - void * mpreal::mpreal_allocate(size_t alloc_size) - { - return(dlmalloc(alloc_size)); - } + // Optimized dynamic memory allocation/(re-)deallocation. + void * mpreal::mpreal_allocate(size_t alloc_size) + { + return(dlmalloc(alloc_size)); + } - void * mpreal::mpreal_reallocate(void *ptr, size_t old_size, size_t new_size) - { - return(dlrealloc(ptr,new_size)); - } + void * mpreal::mpreal_reallocate(void *ptr, size_t old_size, size_t new_size) + { + return(dlrealloc(ptr,new_size)); + } - void mpreal::mpreal_free(void *ptr, size_t size) - { - dlfree(ptr); - } + void mpreal::mpreal_free(void *ptr, size_t size) + { + dlfree(ptr); + } - inline void mpreal::set_custom_malloc(void) - { - if(!is_custom_malloc) - { - mp_set_memory_functions(mpreal_allocate,mpreal_reallocate,mpreal_free); - is_custom_malloc = true; - } - } + inline void mpreal::set_custom_malloc(void) + { + if(!is_custom_malloc) + { + mp_set_memory_functions(mpreal_allocate,mpreal_reallocate,mpreal_free); + is_custom_malloc = true; + } + } #endif } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index c640af947..9245beab0 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -1,59 +1,59 @@ /* - Multi-precision real number class. C++ interface for MPFR library. - Project homepage: http://www.holoborodko.com/pavel/ - Contact e-mail: pavel@holoborodko.com + Multi-precision real number class. C++ interface for MPFR library. + Project homepage: http://www.holoborodko.com/pavel/ + Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2012 Pavel Holoborodko + Copyright (c) 2008-2012 Pavel Holoborodko - Core Developers: - Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. + Core Developers: + Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. - Contributors: - Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, - Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, - Tsai Chia Cheng, Alexei Zubanov. + Contributors: + Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, + Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, + Tsai Chia Cheng, Alexei Zubanov. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + **************************************************************************** + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. The name of the author may be used to endorse or promote products - derived from this software without specific prior written permission. + **************************************************************************** + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. The name of the author may be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. */ #ifndef __MPREAL_H__ @@ -67,544 +67,544 @@ #include // Options -#define MPREAL_HAVE_INT64_SUPPORT // int64_t support: available only for MSVC 2010 & GCC -#define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer (valid only for MSVC in "Debug" builds) +#define MPREAL_HAVE_INT64_SUPPORT // int64_t support: available only for MSVC 2010 & GCC +#define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer (valid only for MSVC in "Debug" builds) // Detect compiler using signatures from http://predef.sourceforge.net/ #if defined(__GNUC__) && defined(__INTEL_COMPILER) - #define IsInf(x) isinf(x) // Intel ICC compiler on Linux + #define IsInf(x) isinf(x) // Intel ICC compiler on Linux -#elif defined(_MSC_VER) // Microsoft Visual C++ - #define IsInf(x) (!_finite(x)) +#elif defined(_MSC_VER) // Microsoft Visual C++ + #define IsInf(x) (!_finite(x)) #elif defined(__GNUC__) - #define IsInf(x) std::isinf(x) // GNU C/C++ + #define IsInf(x) std::isinf(x) // GNU C/C++ #else - #define IsInf(x) std::isinf(x) // Unknown compiler, just hope for C99 conformance + #define IsInf(x) std::isinf(x) // Unknown compiler, just hope for C99 conformance #endif #if defined(MPREAL_HAVE_INT64_SUPPORT) - - #define MPFR_USE_INTMAX_T // should be defined before mpfr.h + + #define MPFR_USE_INTMAX_T // should be defined before mpfr.h - #if defined(_MSC_VER) // is available only in msvc2010! - #if (_MSC_VER >= 1600) - #include - #else // MPFR relies on intmax_t which is available only in msvc2010 - #undef MPREAL_HAVE_INT64_SUPPORT // Besides, MPFR - MPIR have to be compiled with msvc2010 - #undef MPFR_USE_INTMAX_T // Since we cannot detect this, disable x64 by default - // Someone should change this manually if needed. - #endif - #endif - - #if defined (__MINGW32__) || defined(__MINGW64__) - #include // equivalent to msvc2010 - #elif defined (__GNUC__) - #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) - #undef MPREAL_HAVE_INT64_SUPPORT // remove all shaman dances for x64 builds since - #undef MPFR_USE_INTMAX_T // GCC already support x64 as of "long int" is 64-bit integer, nothing left to do - #else - #include // use int64_t, uint64_t otherwise. - #endif - #endif + #if defined(_MSC_VER) // is available only in msvc2010! + #if (_MSC_VER >= 1600) + #include + #else // MPFR relies on intmax_t which is available only in msvc2010 + #undef MPREAL_HAVE_INT64_SUPPORT // Besides, MPFR - MPIR have to be compiled with msvc2010 + #undef MPFR_USE_INTMAX_T // Since we cannot detect this, disable x64 by default + // Someone should change this manually if needed. + #endif + #endif + + #if defined (__MINGW32__) || defined(__MINGW64__) + #include // equivalent to msvc2010 + #elif defined (__GNUC__) + #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) + #undef MPREAL_HAVE_INT64_SUPPORT // remove all shaman dances for x64 builds since + #undef MPFR_USE_INTMAX_T // GCC already support x64 as of "long int" is 64-bit integer, nothing left to do + #else + #include // use int64_t, uint64_t otherwise. + #endif + #endif #endif #if defined(MPREAL_HAVE_MSVC_DEBUGVIEW) && defined(_MSC_VER) && defined(_DEBUG) -#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString() - #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView +#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString() + #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView #else - #define MPREAL_MSVC_DEBUGVIEW_CODE - #define MPREAL_MSVC_DEBUGVIEW_DATA + #define MPREAL_MSVC_DEBUGVIEW_CODE + #define MPREAL_MSVC_DEBUGVIEW_DATA #endif #include #if (MPFR_VERSION < MPFR_VERSION_NUM(3,0,0)) - #include // needed for random() + #include // needed for random() #endif namespace mpfr { class mpreal { private: - mpfr_t mp; + mpfr_t mp; public: - static mp_rnd_t default_rnd; - static mp_prec_t default_prec; - static int default_base; - static int double_bits; + static mp_rnd_t default_rnd; + static mp_prec_t default_prec; + static int default_base; + static int double_bits; public: - // Constructors && type conversion - mpreal(); - mpreal(const mpreal& u); - mpreal(const mpfr_t u); - mpreal(const mpf_t u); - mpreal(const mpz_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const mpq_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const long double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const unsigned long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const unsigned int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + // Constructors && type conversion + mpreal(); + mpreal(const mpreal& u); + mpreal(const mpfr_t u); + mpreal(const mpf_t u); + mpreal(const mpz_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const mpq_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const long double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const unsigned long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const unsigned int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); #if defined (MPREAL_HAVE_INT64_SUPPORT) - mpreal(const uint64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const int64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const uint64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const int64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); #endif - mpreal(const char* s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); - mpreal(const std::string& s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); + mpreal(const char* s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); + mpreal(const std::string& s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); - ~mpreal(); + ~mpreal(); - // Operations - // = - // +, -, *, /, ++, --, <<, >> - // *=, +=, -=, /=, - // <, >, ==, <=, >= + // Operations + // = + // +, -, *, /, ++, --, <<, >> + // *=, +=, -=, /=, + // <, >, ==, <=, >= - // = - mpreal& operator=(const mpreal& v); - mpreal& operator=(const mpf_t v); - mpreal& operator=(const mpz_t v); - mpreal& operator=(const mpq_t v); - mpreal& operator=(const long double v); - mpreal& operator=(const double v); - mpreal& operator=(const unsigned long int v); - mpreal& operator=(const unsigned int v); - mpreal& operator=(const long int v); - mpreal& operator=(const int v); - mpreal& operator=(const char* s); + // = + mpreal& operator=(const mpreal& v); + mpreal& operator=(const mpf_t v); + mpreal& operator=(const mpz_t v); + mpreal& operator=(const mpq_t v); + mpreal& operator=(const long double v); + mpreal& operator=(const double v); + mpreal& operator=(const unsigned long int v); + mpreal& operator=(const unsigned int v); + mpreal& operator=(const long int v); + mpreal& operator=(const int v); + mpreal& operator=(const char* s); - // + - mpreal& operator+=(const mpreal& v); - mpreal& operator+=(const mpf_t v); - mpreal& operator+=(const mpz_t v); - mpreal& operator+=(const mpq_t v); - mpreal& operator+=(const long double u); - mpreal& operator+=(const double u); - mpreal& operator+=(const unsigned long int u); - mpreal& operator+=(const unsigned int u); - mpreal& operator+=(const long int u); - mpreal& operator+=(const int u); + // + + mpreal& operator+=(const mpreal& v); + mpreal& operator+=(const mpf_t v); + mpreal& operator+=(const mpz_t v); + mpreal& operator+=(const mpq_t v); + mpreal& operator+=(const long double u); + mpreal& operator+=(const double u); + mpreal& operator+=(const unsigned long int u); + mpreal& operator+=(const unsigned int u); + mpreal& operator+=(const long int u); + mpreal& operator+=(const int u); #if defined (MPREAL_HAVE_INT64_SUPPORT) - mpreal& operator+=(const int64_t u); - mpreal& operator+=(const uint64_t u); - mpreal& operator-=(const int64_t u); - mpreal& operator-=(const uint64_t u); - mpreal& operator*=(const int64_t u); - mpreal& operator*=(const uint64_t u); - mpreal& operator/=(const int64_t u); - mpreal& operator/=(const uint64_t u); + mpreal& operator+=(const int64_t u); + mpreal& operator+=(const uint64_t u); + mpreal& operator-=(const int64_t u); + mpreal& operator-=(const uint64_t u); + mpreal& operator*=(const int64_t u); + mpreal& operator*=(const uint64_t u); + mpreal& operator/=(const int64_t u); + mpreal& operator/=(const uint64_t u); #endif - const mpreal operator+() const; - mpreal& operator++ (); - const mpreal operator++ (int); + const mpreal operator+() const; + mpreal& operator++ (); + const mpreal operator++ (int); - // - - mpreal& operator-=(const mpreal& v); - mpreal& operator-=(const mpz_t v); - mpreal& operator-=(const mpq_t v); - mpreal& operator-=(const long double u); - mpreal& operator-=(const double u); - mpreal& operator-=(const unsigned long int u); - mpreal& operator-=(const unsigned int u); - mpreal& operator-=(const long int u); - mpreal& operator-=(const int u); - const mpreal operator-() const; - friend const mpreal operator-(const unsigned long int b, const mpreal& a); - friend const mpreal operator-(const unsigned int b, const mpreal& a); - friend const mpreal operator-(const long int b, const mpreal& a); - friend const mpreal operator-(const int b, const mpreal& a); - friend const mpreal operator-(const double b, const mpreal& a); - mpreal& operator-- (); - const mpreal operator-- (int); + // - + mpreal& operator-=(const mpreal& v); + mpreal& operator-=(const mpz_t v); + mpreal& operator-=(const mpq_t v); + mpreal& operator-=(const long double u); + mpreal& operator-=(const double u); + mpreal& operator-=(const unsigned long int u); + mpreal& operator-=(const unsigned int u); + mpreal& operator-=(const long int u); + mpreal& operator-=(const int u); + const mpreal operator-() const; + friend const mpreal operator-(const unsigned long int b, const mpreal& a); + friend const mpreal operator-(const unsigned int b, const mpreal& a); + friend const mpreal operator-(const long int b, const mpreal& a); + friend const mpreal operator-(const int b, const mpreal& a); + friend const mpreal operator-(const double b, const mpreal& a); + mpreal& operator-- (); + const mpreal operator-- (int); - // * - mpreal& operator*=(const mpreal& v); - mpreal& operator*=(const mpz_t v); - mpreal& operator*=(const mpq_t v); - mpreal& operator*=(const long double v); - mpreal& operator*=(const double v); - mpreal& operator*=(const unsigned long int v); - mpreal& operator*=(const unsigned int v); - mpreal& operator*=(const long int v); - mpreal& operator*=(const int v); - - // / - mpreal& operator/=(const mpreal& v); - mpreal& operator/=(const mpz_t v); - mpreal& operator/=(const mpq_t v); - mpreal& operator/=(const long double v); - mpreal& operator/=(const double v); - mpreal& operator/=(const unsigned long int v); - mpreal& operator/=(const unsigned int v); - mpreal& operator/=(const long int v); - mpreal& operator/=(const int v); - friend const mpreal operator/(const unsigned long int b, const mpreal& a); - friend const mpreal operator/(const unsigned int b, const mpreal& a); - friend const mpreal operator/(const long int b, const mpreal& a); - friend const mpreal operator/(const int b, const mpreal& a); - friend const mpreal operator/(const double b, const mpreal& a); + // * + mpreal& operator*=(const mpreal& v); + mpreal& operator*=(const mpz_t v); + mpreal& operator*=(const mpq_t v); + mpreal& operator*=(const long double v); + mpreal& operator*=(const double v); + mpreal& operator*=(const unsigned long int v); + mpreal& operator*=(const unsigned int v); + mpreal& operator*=(const long int v); + mpreal& operator*=(const int v); + + // / + mpreal& operator/=(const mpreal& v); + mpreal& operator/=(const mpz_t v); + mpreal& operator/=(const mpq_t v); + mpreal& operator/=(const long double v); + mpreal& operator/=(const double v); + mpreal& operator/=(const unsigned long int v); + mpreal& operator/=(const unsigned int v); + mpreal& operator/=(const long int v); + mpreal& operator/=(const int v); + friend const mpreal operator/(const unsigned long int b, const mpreal& a); + friend const mpreal operator/(const unsigned int b, const mpreal& a); + friend const mpreal operator/(const long int b, const mpreal& a); + friend const mpreal operator/(const int b, const mpreal& a); + friend const mpreal operator/(const double b, const mpreal& a); - //<<= Fast Multiplication by 2^u - mpreal& operator<<=(const unsigned long int u); - mpreal& operator<<=(const unsigned int u); - mpreal& operator<<=(const long int u); - mpreal& operator<<=(const int u); + //<<= Fast Multiplication by 2^u + mpreal& operator<<=(const unsigned long int u); + mpreal& operator<<=(const unsigned int u); + mpreal& operator<<=(const long int u); + mpreal& operator<<=(const int u); - //>>= Fast Division by 2^u - mpreal& operator>>=(const unsigned long int u); - mpreal& operator>>=(const unsigned int u); - mpreal& operator>>=(const long int u); - mpreal& operator>>=(const int u); + //>>= Fast Division by 2^u + mpreal& operator>>=(const unsigned long int u); + mpreal& operator>>=(const unsigned int u); + mpreal& operator>>=(const long int u); + mpreal& operator>>=(const int u); - // Boolean Operators - friend bool operator > (const mpreal& a, const mpreal& b); - friend bool operator >= (const mpreal& a, const mpreal& b); - friend bool operator < (const mpreal& a, const mpreal& b); - friend bool operator <= (const mpreal& a, const mpreal& b); - friend bool operator == (const mpreal& a, const mpreal& b); - friend bool operator != (const mpreal& a, const mpreal& b); + // Boolean Operators + friend bool operator > (const mpreal& a, const mpreal& b); + friend bool operator >= (const mpreal& a, const mpreal& b); + friend bool operator < (const mpreal& a, const mpreal& b); + friend bool operator <= (const mpreal& a, const mpreal& b); + friend bool operator == (const mpreal& a, const mpreal& b); + friend bool operator != (const mpreal& a, const mpreal& b); - // Optimized specializations for boolean operators - friend bool operator == (const mpreal& a, const unsigned long int b); - friend bool operator == (const mpreal& a, const unsigned int b); - friend bool operator == (const mpreal& a, const long int b); - friend bool operator == (const mpreal& a, const int b); - friend bool operator == (const mpreal& a, const long double b); - friend bool operator == (const mpreal& a, const double b); + // Optimized specializations for boolean operators + friend bool operator == (const mpreal& a, const unsigned long int b); + friend bool operator == (const mpreal& a, const unsigned int b); + friend bool operator == (const mpreal& a, const long int b); + friend bool operator == (const mpreal& a, const int b); + friend bool operator == (const mpreal& a, const long double b); + friend bool operator == (const mpreal& a, const double b); - // Type Conversion operators - long toLong() const; - unsigned long toULong() const; - double toDouble() const; - long double toLDouble() const; + // Type Conversion operators + long toLong() const; + unsigned long toULong() const; + double toDouble() const; + long double toLDouble() const; #if defined (MPREAL_HAVE_INT64_SUPPORT) - int64_t toInt64() const; - uint64_t toUInt64() const; + int64_t toInt64() const; + uint64_t toUInt64() const; #endif - // Get raw pointers - ::mpfr_ptr mpfr_ptr(); - ::mpfr_srcptr mpfr_srcptr() const; + // Get raw pointers + ::mpfr_ptr mpfr_ptr(); + ::mpfr_srcptr mpfr_srcptr() const; - // Convert mpreal to string with n significant digits in base b - // n = 0 -> convert with the maximum available digits - std::string toString(int n = 0, int b = default_base, mp_rnd_t mode = default_rnd) const; + // Convert mpreal to string with n significant digits in base b + // n = 0 -> convert with the maximum available digits + std::string toString(int n = 0, int b = default_base, mp_rnd_t mode = default_rnd) const; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - std::string toString(const std::string& format) const; + std::string toString(const std::string& format) const; #endif - // Math Functions - friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + // Math Functions + friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int cmpabs(const mpreal& a,const mpreal& b); - - friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int cmpabs(const mpreal& a,const mpreal& b); + + friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int sgn(const mpreal& v); // -1 or +1 + friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sgn(const mpreal& v); // -1 or +1 // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - friend int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); #endif // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - friend const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal urandom (gmp_randstate_t& state,mp_rnd_t rnd_mode = mpreal::default_rnd); // use gmp_randinit_default() to init state, gmp_randclear() to clear - friend bool isregular(const mpreal& v); + friend const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal urandom (gmp_randstate_t& state,mp_rnd_t rnd_mode = mpreal::default_rnd); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend bool isregular(const mpreal& v); #endif - - // Uniformly distributed random number generation in [0,1] using - // Mersenne-Twister algorithm by default. - // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) - // Check urandom() for more precise control. - friend const mpreal random(unsigned int seed = 0); - - // Exponent and mantissa manipulation - friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); - friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); + + // Uniformly distributed random number generation in [0,1] using + // Mersenne-Twister algorithm by default. + // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) + // Check urandom() for more precise control. + friend const mpreal random(unsigned int seed = 0); + + // Exponent and mantissa manipulation + friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); + friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); - // Splits mpreal value into fractional and integer parts. - // Returns fractional part and stores integer part in n. - friend const mpreal modf(const mpreal& v, mpreal& n); + // Splits mpreal value into fractional and integer parts. + // Returns fractional part and stores integer part in n. + friend const mpreal modf(const mpreal& v, mpreal& n); - // Constants - // don't forget to call mpfr_free_cache() for every thread where you are using const-functions - friend const mpreal const_log2 (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_pi (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_euler (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_catalan (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - // returns +inf iff sign>=0 otherwise -inf - friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + // Constants + // don't forget to call mpfr_free_cache() for every thread where you are using const-functions + friend const mpreal const_log2 (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_pi (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_euler (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_catalan (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + // returns +inf iff sign>=0 otherwise -inf + friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - // Output/ Input - friend std::ostream& operator<<(std::ostream& os, const mpreal& v); + // Output/ Input + friend std::ostream& operator<<(std::ostream& os, const mpreal& v); friend std::istream& operator>>(std::istream& is, mpreal& v); - // Integer Related Functions - friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal ceil (const mpreal& v); - friend const mpreal floor(const mpreal& v); - friend const mpreal round(const mpreal& v); - friend const mpreal trunc(const mpreal& v); - friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - - // Miscellaneous Functions - friend const mpreal nexttoward (const mpreal& x, const mpreal& y); - friend const mpreal nextabove (const mpreal& x); - friend const mpreal nextbelow (const mpreal& x); + // Integer Related Functions + friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal ceil (const mpreal& v); + friend const mpreal floor(const mpreal& v); + friend const mpreal round(const mpreal& v); + friend const mpreal trunc(const mpreal& v); + friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + + // Miscellaneous Functions + friend const mpreal nexttoward (const mpreal& x, const mpreal& y); + friend const mpreal nextabove (const mpreal& x); + friend const mpreal nextbelow (const mpreal& x); - // use gmp_randinit_default() to init state, gmp_randclear() to clear - friend const mpreal urandomb (gmp_randstate_t& state); + // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal urandomb (gmp_randstate_t& state); // MPFR < 2.4.2 Specifics #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) - friend const mpreal random2 (mp_size_t size, mp_exp_t exp); + friend const mpreal random2 (mp_size_t size, mp_exp_t exp); #endif - // Instance Checkers - friend bool isnan (const mpreal& v); - friend bool isinf (const mpreal& v); - friend bool isfinite(const mpreal& v); + // Instance Checkers + friend bool isnan (const mpreal& v); + friend bool isinf (const mpreal& v); + friend bool isfinite(const mpreal& v); - friend bool isnum(const mpreal& v); - friend bool iszero(const mpreal& v); - friend bool isint(const mpreal& v); + friend bool isnum(const mpreal& v); + friend bool iszero(const mpreal& v); + friend bool isint(const mpreal& v); - // Set/Get instance properties - inline mp_prec_t get_prec() const; - inline void set_prec(mp_prec_t prec, mp_rnd_t rnd_mode = default_rnd); // Change precision with rounding mode + // Set/Get instance properties + inline mp_prec_t get_prec() const; + inline void set_prec(mp_prec_t prec, mp_rnd_t rnd_mode = default_rnd); // Change precision with rounding mode - // Aliases for get_prec(), set_prec() - needed for compatibility with std::complex interface - inline mpreal& setPrecision(int Precision, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); - inline int getPrecision() const; - - // Set mpreal to +/- inf, NaN, +/-0 - mpreal& setInf (int Sign = +1); - mpreal& setNan (); - mpreal& setZero (int Sign = +1); - mpreal& setSign (int Sign, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); + // Aliases for get_prec(), set_prec() - needed for compatibility with std::complex interface + inline mpreal& setPrecision(int Precision, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); + inline int getPrecision() const; + + // Set mpreal to +/- inf, NaN, +/-0 + mpreal& setInf (int Sign = +1); + mpreal& setNan (); + mpreal& setZero (int Sign = +1); + mpreal& setSign (int Sign, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); - //Exponent - mp_exp_t get_exp(); - int set_exp(mp_exp_t e); - int check_range (int t, mp_rnd_t rnd_mode = default_rnd); - int subnormalize (int t,mp_rnd_t rnd_mode = default_rnd); + //Exponent + mp_exp_t get_exp(); + int set_exp(mp_exp_t e); + int check_range (int t, mp_rnd_t rnd_mode = default_rnd); + int subnormalize (int t,mp_rnd_t rnd_mode = default_rnd); - // Inexact conversion from float - inline bool fits_in_bits(double x, int n); + // Inexact conversion from float + inline bool fits_in_bits(double x, int n); - // Set/Get global properties - static void set_default_prec(mp_prec_t prec); - static mp_prec_t get_default_prec(); - static void set_default_base(int base); - static int get_default_base(); - static void set_double_bits(int dbits); - static int get_double_bits(); - static void set_default_rnd(mp_rnd_t rnd_mode); - static mp_rnd_t get_default_rnd(); - static mp_exp_t get_emin (void); - static mp_exp_t get_emax (void); - static mp_exp_t get_emin_min (void); - static mp_exp_t get_emin_max (void); - static mp_exp_t get_emax_min (void); - static mp_exp_t get_emax_max (void); - static int set_emin (mp_exp_t exp); - static int set_emax (mp_exp_t exp); + // Set/Get global properties + static void set_default_prec(mp_prec_t prec); + static mp_prec_t get_default_prec(); + static void set_default_base(int base); + static int get_default_base(); + static void set_double_bits(int dbits); + static int get_double_bits(); + static void set_default_rnd(mp_rnd_t rnd_mode); + static mp_rnd_t get_default_rnd(); + static mp_exp_t get_emin (void); + static mp_exp_t get_emax (void); + static mp_exp_t get_emin_min (void); + static mp_exp_t get_emin_max (void); + static mp_exp_t get_emax_min (void); + static mp_exp_t get_emax_max (void); + static int set_emin (mp_exp_t exp); + static int set_emax (mp_exp_t exp); - // Efficient swapping of two mpreal values - friend void swap(mpreal& x, mpreal& y); - - //Min Max - macros is evil. Needed for systems which defines max and min globally as macros (e.g. Windows) - //Hope that globally defined macros use > < operations only - friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); - friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); + // Efficient swapping of two mpreal values + friend void swap(mpreal& x, mpreal& y); + + //Min Max - macros is evil. Needed for systems which defines max and min globally as macros (e.g. Windows) + //Hope that globally defined macros use > < operations only + friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); + friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) private: - // Optimized dynamic memory allocation/(re-)deallocation. - static bool is_custom_malloc; - static void *mpreal_allocate (size_t alloc_size); - static void *mpreal_reallocate (void *ptr, size_t old_size, size_t new_size); - static void mpreal_free (void *ptr, size_t size); - inline static void set_custom_malloc (void); + // Optimized dynamic memory allocation/(re-)deallocation. + static bool is_custom_malloc; + static void *mpreal_allocate (size_t alloc_size); + static void *mpreal_reallocate (void *ptr, size_t old_size, size_t new_size); + static void mpreal_free (void *ptr, size_t size); + inline static void set_custom_malloc (void); #endif private: - // Human friendly Debug Preview in Visual Studio. - // Put one of these lines: - // - // mpfr::mpreal= ; Show value only - // mpfr::mpreal=, bits ; Show value & precision - // - // at the beginning of - // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat - MPREAL_MSVC_DEBUGVIEW_DATA + // Human friendly Debug Preview in Visual Studio. + // Put one of these lines: + // + // mpfr::mpreal= ; Show value only + // mpfr::mpreal=, bits ; Show value & precision + // + // at the beginning of + // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat + MPREAL_MSVC_DEBUGVIEW_DATA }; ////////////////////////////////////////////////////////////////////////// // Exceptions class conversion_overflow : public std::exception { public: - std::string why() { return "inexact conversion from floating point"; } + std::string why() { return "inexact conversion from floating point"; } }; namespace internal{ - // Use SFINAE to restrict arithmetic operations instantiation only for numeric types - // This is needed for smooth integration with libraries based on expression templates - template struct result_type {}; - - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; + // Use SFINAE to restrict arithmetic operations instantiation only for numeric types + // This is needed for smooth integration with libraries based on expression templates + template struct result_type {}; + + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; #if defined (MPREAL_HAVE_INT64_SUPPORT) - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; #endif } // + Addition template inline const typename internal::result_type::type - operator+(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) += rhs; } + operator+(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) += rhs; } template inline const typename internal::result_type::type - operator+(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) += lhs; } + operator+(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) += lhs; } // - Subtraction template inline const typename internal::result_type::type - operator-(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) -= rhs; } + operator-(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) -= rhs; } template inline const typename internal::result_type::type - operator-(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) -= rhs; } + operator-(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) -= rhs; } // * Multiplication template inline const typename internal::result_type::type - operator*(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) *= rhs; } + operator*(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) *= rhs; } template inline const typename internal::result_type::type - operator*(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) *= lhs; } + operator*(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) *= lhs; } // / Division template inline const typename internal::result_type::type - operator/(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) /= rhs; } + operator/(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) /= rhs; } template inline const typename internal::result_type::type - operator/(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) /= rhs; } + operator/(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) /= rhs; } ////////////////////////////////////////////////////////////////////////// // sqrt @@ -654,13 +654,13 @@ const mpreal pow(const int a, const int b, mp_rnd_t rnd_mode = mpreal::default_r const mpreal pow(const int a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const int a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); -const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); +const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const int b, mp_rnd_t rnd_mode = mpreal::default_rnd); -const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); +const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); @@ -672,7 +672,7 @@ const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode = mpreal::defaul inline const mpreal machine_epsilon(mp_prec_t prec = mpreal::get_default_prec()); // Returns the positive distance from abs(x) to the next larger in magnitude floating point number of the same precision as x -inline const mpreal machine_epsilon(const mpreal& x); +inline const mpreal machine_epsilon(const mpreal& x); inline const mpreal mpreal_min(mp_prec_t prec = mpreal::get_default_prec()); inline const mpreal mpreal_max(mp_prec_t prec = mpreal::get_default_prec()); @@ -680,9 +680,9 @@ inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps); inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps); ////////////////////////////////////////////////////////////////////////// -// Bits - decimal digits relation -// bits = ceil(digits*log[2](10)) -// digits = floor(bits*log[10](2)) +// Bits - decimal digits relation +// bits = ceil(digits*log[2](10)) +// digits = floor(bits*log[10](2)) inline mp_prec_t digits2bits(int d); inline int bits2digits(mp_prec_t b); @@ -700,541 +700,541 @@ const mpreal (min)(const mpreal& x, const mpreal& y); // Operators - Assignment inline mpreal& mpreal::operator=(const mpreal& v) { - if (this != &v) - { - mpfr_clear(mp); - mpfr_init2(mp,mpfr_get_prec(v.mp)); - mpfr_set(mp,v.mp,default_rnd); + if (this != &v) + { + mpfr_clear(mp); + mpfr_init2(mp,mpfr_get_prec(v.mp)); + mpfr_set(mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - } - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + } + return *this; } inline mpreal& mpreal::operator=(const mpf_t v) { - mpfr_set_f(mp,v,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_f(mp,v,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const mpz_t v) { - mpfr_set_z(mp,v,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_z(mp,v,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const mpq_t v) { - mpfr_set_q(mp,v,default_rnd); + mpfr_set_q(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const long double v) -{ +inline mpreal& mpreal::operator=(const long double v) +{ mpfr_set_ld(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const double v) -{ +inline mpreal& mpreal::operator=(const double v) +{ if(double_bits == -1 || fits_in_bits(v, double_bits)) { - mpfr_set_d(mp,v,default_rnd); + mpfr_set_d(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } else throw conversion_overflow(); - return *this; + return *this; } -inline mpreal& mpreal::operator=(const unsigned long int v) -{ - mpfr_set_ui(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const unsigned long int v) +{ + mpfr_set_ui(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const unsigned int v) -{ - mpfr_set_ui(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const unsigned int v) +{ + mpfr_set_ui(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const long int v) -{ - mpfr_set_si(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const long int v) +{ + mpfr_set_si(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const int v) -{ - mpfr_set_si(mp,v,default_rnd); +{ + mpfr_set_si(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } ////////////////////////////////////////////////////////////////////////// // + Addition inline mpreal& mpreal::operator+=(const mpreal& v) { - mpfr_add(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpf_t u) { - *this += mpreal(u); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this += mpreal(u); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpz_t u) { - mpfr_add_z(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_z(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpq_t u) { - mpfr_add_q(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_q(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+= (const long double u) { - *this += mpreal(u); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this += mpreal(u); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+= (const double u) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_add_d(mp,mp,u,default_rnd); + mpfr_add_d(mp,mp,u,default_rnd); #else - *this += mpreal(u); + *this += mpreal(u); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const unsigned long int u) { - mpfr_add_ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const unsigned int u) { - mpfr_add_ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const long int u) { - mpfr_add_si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const int u) { - mpfr_add_si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline mpreal& mpreal::operator+=(const int64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator+=(const uint64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator-=(const int64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator-=(const uint64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator*=(const int64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator*=(const uint64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator/=(const int64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator/=(const uint64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator+=(const int64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator+=(const uint64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator-=(const int64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator-=(const uint64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator*=(const int64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator*=(const uint64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator/=(const int64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator/=(const uint64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } #endif -inline const mpreal mpreal::operator+()const { return mpreal(*this); } +inline const mpreal mpreal::operator+()const { return mpreal(*this); } inline const mpreal operator+(const mpreal& a, const mpreal& b) { - // prec(a+b) = max(prec(a),prec(b)) - if(a.get_prec()>b.get_prec()) return mpreal(a) += b; - else return mpreal(b) += a; + // prec(a+b) = max(prec(a),prec(b)) + if(a.get_prec()>b.get_prec()) return mpreal(a) += b; + else return mpreal(b) += a; } inline mpreal& mpreal::operator++() { - return *this += 1; + return *this += 1; } inline const mpreal mpreal::operator++ (int) { - mpreal x(*this); - *this += 1; - return x; + mpreal x(*this); + *this += 1; + return x; } inline mpreal& mpreal::operator--() { - return *this -= 1; + return *this -= 1; } inline const mpreal mpreal::operator-- (int) { - mpreal x(*this); - *this -= 1; - return x; + mpreal x(*this); + *this -= 1; + return x; } ////////////////////////////////////////////////////////////////////////// // - Subtraction inline mpreal& mpreal::operator-= (const mpreal& v) { - mpfr_sub(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const mpz_t v) { - mpfr_sub_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const mpq_t v) { - mpfr_sub_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const long double v) { - *this -= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this -= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_sub_d(mp,mp,v,default_rnd); + mpfr_sub_d(mp,mp,v,default_rnd); #else - *this -= mpreal(v); + *this -= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const unsigned long int v) { - mpfr_sub_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const unsigned int v) { - mpfr_sub_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const long int v) { - mpfr_sub_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const int v) { - mpfr_sub_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal mpreal::operator-()const { - mpreal u(*this); - mpfr_neg(u.mp,u.mp,default_rnd); - return u; + mpreal u(*this); + mpfr_neg(u.mp,u.mp,default_rnd); + return u; } inline const mpreal operator-(const mpreal& a, const mpreal& b) { - // prec(a-b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) - { - return mpreal(a) -= b; - }else{ - mpreal x(a); - x.setPrecision(b.getPrecision()); - return x -= b; - } + // prec(a-b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) + { + return mpreal(a) -= b; + }else{ + mpreal x(a); + x.setPrecision(b.getPrecision()); + return x -= b; + } } inline const mpreal operator-(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(a); - mpfr_d_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_d_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; #else - return mpreal(b) -= a; + return mpreal(b) -= a; #endif } inline const mpreal operator-(const unsigned long int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const unsigned int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const long int b, const mpreal& a) { - mpreal x(a); - mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const int b, const mpreal& a) { - mpreal x(a); - mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } ////////////////////////////////////////////////////////////////////////// // * Multiplication inline mpreal& mpreal::operator*= (const mpreal& v) { - mpfr_mul(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const mpz_t v) { - mpfr_mul_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const mpq_t v) { - mpfr_mul_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const long double v) { - *this *= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this *= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_mul_d(mp,mp,v,default_rnd); + mpfr_mul_d(mp,mp,v,default_rnd); #else - *this *= mpreal(v); + *this *= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const unsigned long int v) { - mpfr_mul_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const unsigned int v) { - mpfr_mul_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const long int v) { - mpfr_mul_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const int v) { - mpfr_mul_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator*(const mpreal& a, const mpreal& b) { - // prec(a*b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) return mpreal(a) *= b; - else return mpreal(b) *= a; + // prec(a*b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) return mpreal(a) *= b; + else return mpreal(b) *= a; } ////////////////////////////////////////////////////////////////////////// // / Division inline mpreal& mpreal::operator/=(const mpreal& v) { - mpfr_div(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const mpz_t v) { - mpfr_div_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const mpq_t v) { - mpfr_div_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const long double v) { - *this /= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this /= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_div_d(mp,mp,v,default_rnd); + mpfr_div_d(mp,mp,v,default_rnd); #else - *this /= mpreal(v); + *this /= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const unsigned long int v) { - mpfr_div_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const unsigned int v) { - mpfr_div_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const long int v) { - mpfr_div_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const int v) { - mpfr_div_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator/(const mpreal& a, const mpreal& b) { - // prec(a/b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) - { - return mpreal(a) /= b; - }else{ + // prec(a/b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) + { + return mpreal(a) /= b; + }else{ - mpreal x(a); - x.setPrecision(b.getPrecision()); - return x /= b; - } + mpreal x(a); + x.setPrecision(b.getPrecision()); + return x /= b; + } } inline const mpreal operator/(const unsigned long int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const unsigned int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const long int b, const mpreal& a) { - mpreal x(a); - mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const int b, const mpreal& a) { - mpreal x(a); - mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(a); - mpfr_d_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_d_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; #else - return mpreal(b) /= a; + return mpreal(b) /= a; #endif } @@ -1242,314 +1242,314 @@ inline const mpreal operator/(const double b, const mpreal& a) // Shifts operators - Multiplication/Division by power of 2 inline mpreal& mpreal::operator<<=(const unsigned long int u) { - mpfr_mul_2ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const unsigned int u) { - mpfr_mul_2ui(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2ui(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const long int u) { - mpfr_mul_2si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const int u) { - mpfr_mul_2si(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2si(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const unsigned long int u) { - mpfr_div_2ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const unsigned int u) { - mpfr_div_2ui(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2ui(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const long int u) { - mpfr_div_2si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const int u) { - mpfr_div_2si(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2si(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator<<(const mpreal& v, const unsigned long int k) { - return mul_2ui(v,k); + return mul_2ui(v,k); } inline const mpreal operator<<(const mpreal& v, const unsigned int k) { - return mul_2ui(v,static_cast(k)); + return mul_2ui(v,static_cast(k)); } inline const mpreal operator<<(const mpreal& v, const long int k) { - return mul_2si(v,k); + return mul_2si(v,k); } inline const mpreal operator<<(const mpreal& v, const int k) { - return mul_2si(v,static_cast(k)); + return mul_2si(v,static_cast(k)); } inline const mpreal operator>>(const mpreal& v, const unsigned long int k) { - return div_2ui(v,k); + return div_2ui(v,k); } inline const mpreal operator>>(const mpreal& v, const long int k) { - return div_2si(v,k); + return div_2si(v,k); } inline const mpreal operator>>(const mpreal& v, const unsigned int k) { - return div_2ui(v,static_cast(k)); + return div_2ui(v,static_cast(k)); } inline const mpreal operator>>(const mpreal& v, const int k) { - return div_2si(v,static_cast(k)); + return div_2si(v,static_cast(k)); } // mul_2ui inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); + return x; } // mul_2si inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); + return x; } inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); + return x; } inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_div_2si(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_div_2si(x.mp,v.mp,k,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// //Boolean operators -inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } -inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } -inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } -inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } -inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } +inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } +inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } +inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } +inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } +inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } +inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } -inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } -inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } -inline bool isfinite(const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } -inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } -inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } +inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } +inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } +inline bool isfinite(const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } +inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } +inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} +inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} #endif ////////////////////////////////////////////////////////////////////////// // Type Converters -inline long mpreal::toLong() const { return mpfr_get_si(mp,GMP_RNDZ); } -inline unsigned long mpreal::toULong() const { return mpfr_get_ui(mp,GMP_RNDZ); } -inline double mpreal::toDouble() const { return mpfr_get_d(mp,default_rnd); } -inline long double mpreal::toLDouble() const { return mpfr_get_ld(mp,default_rnd); } +inline long mpreal::toLong() const { return mpfr_get_si(mp,GMP_RNDZ); } +inline unsigned long mpreal::toULong() const { return mpfr_get_ui(mp,GMP_RNDZ); } +inline double mpreal::toDouble() const { return mpfr_get_d(mp,default_rnd); } +inline long double mpreal::toLDouble() const { return mpfr_get_ld(mp,default_rnd); } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline int64_t mpreal::toInt64() const{ return mpfr_get_sj(mp,GMP_RNDZ); } -inline uint64_t mpreal::toUInt64() const{ return mpfr_get_uj(mp,GMP_RNDZ); } +inline int64_t mpreal::toInt64() const{ return mpfr_get_sj(mp,GMP_RNDZ); } +inline uint64_t mpreal::toUInt64() const{ return mpfr_get_uj(mp,GMP_RNDZ); } #endif -inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } -inline ::mpfr_srcptr mpreal::mpfr_srcptr() const { return const_cast< ::mpfr_srcptr >(mp); } +inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } +inline ::mpfr_srcptr mpreal::mpfr_srcptr() const { return const_cast< ::mpfr_srcptr >(mp); } ////////////////////////////////////////////////////////////////////////// -// Bits - decimal digits relation -// bits = ceil(digits*log[2](10)) -// digits = floor(bits*log[10](2)) +// Bits - decimal digits relation +// bits = ceil(digits*log[2](10)) +// digits = floor(bits*log[10](2)) inline mp_prec_t digits2bits(int d) { - const double LOG2_10 = 3.3219280948873624; + const double LOG2_10 = 3.3219280948873624; - d = 10>d?10:d; + d = 10>d?10:d; - return (mp_prec_t)std::ceil((d)*LOG2_10); + return (mp_prec_t)std::ceil((d)*LOG2_10); } inline int bits2digits(mp_prec_t b) { - const double LOG10_2 = 0.30102999566398119; + const double LOG10_2 = 0.30102999566398119; - b = 34>b?34:b; + b = 34>b?34:b; - return (int)std::floor((b)*LOG10_2); + return (int)std::floor((b)*LOG10_2); } ////////////////////////////////////////////////////////////////////////// // Set/Get number properties inline int sgn(const mpreal& v) { - int r = mpfr_signbit(v.mp); - return (r>0?-1:1); + int r = mpfr_signbit(v.mp); + return (r>0?-1:1); } inline mpreal& mpreal::setSign(int sign, mp_rnd_t RoundingMode) { - mpfr_setsign(mp,mp,(sign<0?1:0),RoundingMode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_setsign(mp,mp,(sign<0?1:0),RoundingMode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline int mpreal::getPrecision() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mp); } inline mpreal& mpreal::setPrecision(int Precision, mp_rnd_t RoundingMode) { - mpfr_prec_round(mp,Precision, RoundingMode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_prec_round(mp,Precision, RoundingMode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::setInf(int sign) { - mpfr_set_inf(mp,sign); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; -} + mpfr_set_inf(mp,sign); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; +} inline mpreal& mpreal::setNan() { - mpfr_set_nan(mp); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_nan(mp); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::setZero(int sign) +inline mpreal& mpreal::setZero(int sign) { - mpfr_set_zero(mp,sign); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_zero(mp,sign); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mp_prec_t mpreal::get_prec() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mp); } inline void mpreal::set_prec(mp_prec_t prec, mp_rnd_t rnd_mode) { - mpfr_prec_round(mp,prec,rnd_mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_prec_round(mp,prec,rnd_mode); + MPREAL_MSVC_DEBUGVIEW_CODE; } inline mp_exp_t mpreal::get_exp () { - return mpfr_get_exp(mp); + return mpfr_get_exp(mp); } inline int mpreal::set_exp (mp_exp_t e) { - int x = mpfr_set_exp(mp, e); - MPREAL_MSVC_DEBUGVIEW_CODE; - return x; + int x = mpfr_set_exp(mp, e); + MPREAL_MSVC_DEBUGVIEW_CODE; + return x; } inline const mpreal frexp(const mpreal& v, mp_exp_t* exp) { - mpreal x(v); - *exp = x.get_exp(); - x.set_exp(0); - return x; + mpreal x(v); + *exp = x.get_exp(); + x.set_exp(0); + return x; } inline const mpreal ldexp(const mpreal& v, mp_exp_t exp) { - mpreal x(v); + mpreal x(v); - // rounding is not important since we just increasing the exponent - mpfr_mul_2si(x.mp,x.mp,exp,mpreal::default_rnd); - return x; + // rounding is not important since we just increasing the exponent + mpfr_mul_2si(x.mp,x.mp,exp,mpreal::default_rnd); + return x; } inline const mpreal machine_epsilon(mp_prec_t prec) { - // the smallest eps such that 1.0+eps != 1.0 - // depends (of cause) on the precision - return machine_epsilon(mpreal(1,prec)); + // the smallest eps such that 1.0+eps != 1.0 + // depends (of cause) on the precision + return machine_epsilon(mpreal(1,prec)); } inline const mpreal machine_epsilon(const mpreal& x) -{ - if( x < 0) - { - return nextabove(-x)+x; - }else{ - return nextabove(x)-x; - } +{ + if( x < 0) + { + return nextabove(-x)+x; + }else{ + return nextabove(x)-x; + } } inline const mpreal mpreal_min(mp_prec_t prec) { - // min = 1/2*2^emin = 2^(emin-1) + // min = 1/2*2^emin = 2^(emin-1) - return mpreal(1,prec) << mpreal::get_emin()-1; + return mpreal(1,prec) << mpreal::get_emin()-1; } inline const mpreal mpreal_max(mp_prec_t prec) { - // max = (1-eps)*2^emax, assume eps = 0?, - // and use emax-1 to prevent value to be +inf - // max = 2^(emax-1) + // max = (1-eps)*2^emax, assume eps = 0?, + // and use emax-1 to prevent value to be +inf + // max = 2^(emax-1) - return mpreal(1,prec) << mpreal::get_emax()-1; + return mpreal(1,prec) << mpreal::get_emax()-1; } inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) @@ -1562,74 +1562,74 @@ inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps) { - return abs(a - b) <= (min)(abs(a), abs(b)) * eps; + return abs(a - b) <= (min)(abs(a), abs(b)) * eps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b) { - return isEqualFuzzy(a,b,machine_epsilon((std::min)(abs(a), abs(b)))); + return isEqualFuzzy(a,b,machine_epsilon((std::min)(abs(a), abs(b)))); } inline const mpreal modf(const mpreal& v, mpreal& n) { - mpreal frac(v); + mpreal frac(v); - // rounding is not important since we are using the same number - mpfr_frac(frac.mp,frac.mp,mpreal::default_rnd); - mpfr_trunc(n.mp,v.mp); - return frac; + // rounding is not important since we are using the same number + mpfr_frac(frac.mp,frac.mp,mpreal::default_rnd); + mpfr_trunc(n.mp,v.mp); + return frac; } inline int mpreal::check_range (int t, mp_rnd_t rnd_mode) { - return mpfr_check_range(mp,t,rnd_mode); + return mpfr_check_range(mp,t,rnd_mode); } inline int mpreal::subnormalize (int t,mp_rnd_t rnd_mode) { - int r = mpfr_subnormalize(mp,t,rnd_mode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return r; + int r = mpfr_subnormalize(mp,t,rnd_mode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return r; } inline mp_exp_t mpreal::get_emin (void) { - return mpfr_get_emin(); + return mpfr_get_emin(); } inline int mpreal::set_emin (mp_exp_t exp) { - return mpfr_set_emin(exp); + return mpfr_set_emin(exp); } inline mp_exp_t mpreal::get_emax (void) { - return mpfr_get_emax(); + return mpfr_get_emax(); } inline int mpreal::set_emax (mp_exp_t exp) { - return mpfr_set_emax(exp); + return mpfr_set_emax(exp); } inline mp_exp_t mpreal::get_emin_min (void) { - return mpfr_get_emin_min(); + return mpfr_get_emin_min(); } inline mp_exp_t mpreal::get_emin_max (void) { - return mpfr_get_emin_max(); + return mpfr_get_emin_max(); } inline mp_exp_t mpreal::get_emax_min (void) { - return mpfr_get_emax_min(); + return mpfr_get_emax_min(); } inline mp_exp_t mpreal::get_emax_max (void) { - return mpfr_get_emax_max(); + return mpfr_get_emax_max(); } ////////////////////////////////////////////////////////////////////////// @@ -1637,460 +1637,460 @@ inline mp_exp_t mpreal::get_emax_max (void) ////////////////////////////////////////////////////////////////////////// inline const mpreal sqr(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sqr(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sqr(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sqrt(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sqrt(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_sqrt_ui(x.mp,v,rnd_mode); - return x; + mpreal x; + mpfr_sqrt_ui(x.mp,v,rnd_mode); + return x; } inline const mpreal sqrt(const unsigned int v, mp_rnd_t rnd_mode) { - return sqrt(static_cast(v),rnd_mode); + return sqrt(static_cast(v),rnd_mode); } inline const mpreal sqrt(const long int v, mp_rnd_t rnd_mode) { - if (v>=0) return sqrt(static_cast(v),rnd_mode); - else return mpreal().setNan(); // NaN + if (v>=0) return sqrt(static_cast(v),rnd_mode); + else return mpreal().setNan(); // NaN } inline const mpreal sqrt(const int v, mp_rnd_t rnd_mode) { - if (v>=0) return sqrt(static_cast(v),rnd_mode); - else return mpreal().setNan(); // NaN + if (v>=0) return sqrt(static_cast(v),rnd_mode); + else return mpreal().setNan(); // NaN } inline const mpreal sqrt(const long double v, mp_rnd_t rnd_mode) { - return sqrt(mpreal(v),rnd_mode); + return sqrt(mpreal(v),rnd_mode); } inline const mpreal sqrt(const double v, mp_rnd_t rnd_mode) { - return sqrt(mpreal(v),rnd_mode); + return sqrt(mpreal(v),rnd_mode); } inline const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cbrt(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cbrt(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_root(x.mp,x.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_root(x.mp,x.mp,k,rnd_mode); + return x; } inline const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_abs(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_abs(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_abs(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_abs(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_dim(x.mp,a.mp,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_dim(x.mp,a.mp,b.mp,rnd_mode); + return x; } inline int cmpabs(const mpreal& a,const mpreal& b) { - return mpfr_cmpabs(a.mp,b.mp); + return mpfr_cmpabs(a.mp,b.mp); } inline const mpreal log (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal log2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log10(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log10(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp10(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp10(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cos(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cos(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sin(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sin(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_tan(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_tan(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sec(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sec(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_csc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_csc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cot(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cot(x.mp,v.mp,rnd_mode); + return x; } inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) { - return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); } inline const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_acos(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_acos(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_asin(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_asin(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_atan(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_atan(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode) { - return atan(1/v, rnd_mode); + return atan(1/v, rnd_mode); } inline const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode) { - return acos(1/v, rnd_mode); + return acos(1/v, rnd_mode); } inline const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode) { - return asin(1/v, rnd_mode); + return asin(1/v, rnd_mode); } inline const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode) { - return atanh(1/v, rnd_mode); + return atanh(1/v, rnd_mode); } inline const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode) { - return acosh(1/v, rnd_mode); + return acosh(1/v, rnd_mode); } inline const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode) { - return asinh(1/v, rnd_mode); + return asinh(1/v, rnd_mode); } inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); + mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); - return a; + return a; } inline const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cosh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cosh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sinh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sinh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_tanh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_tanh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sech(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sech(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_csch(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_csch(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_coth(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_coth(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_acosh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_acosh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_asinh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_asinh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_atanh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_atanh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); + mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) -{ - mpreal a; - mp_prec_t yp, xp; +{ + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); + mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x(0,prec); - mpfr_fac_ui(x.mp,v,rnd_mode); - return x; + mpreal x(0,prec); + mpfr_fac_ui(x.mp,v,rnd_mode); + return x; } inline const mpreal log1p (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log1p(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log1p(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal expm1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_expm1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_expm1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_eint(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_eint(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal gamma (const mpreal& x, mp_rnd_t rnd_mode) { - mpreal FunctionValue(x); + mpreal FunctionValue(x); - // x < 0: gamma(-x) = -pi/(x * gamma(x) * sin(pi*x)) + // x < 0: gamma(-x) = -pi/(x * gamma(x) * sin(pi*x)) - mpfr_gamma(FunctionValue.mp, x.mp, rnd_mode); + mpfr_gamma(FunctionValue.mp, x.mp, rnd_mode); - return FunctionValue; + return FunctionValue; } inline const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_lngamma(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_lngamma(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode) { - mpreal x(v); - int tsignp; + mpreal x(v); + int tsignp; - if(signp) - mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); - else - mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); + if(signp) + mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); + else + mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); - return x; + return x; } inline const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_zeta(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_zeta(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_erf(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_erf(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_erfc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_erfc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_j0(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_j0(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_j1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_j1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_jn(x.mp,n,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_jn(x.mp,n,v.mp,rnd_mode); + return x; } inline const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_y0(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_y0(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_y1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_y1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_yn(x.mp,n,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_yn(x.mp,n,v.mp,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// @@ -2099,36 +2099,36 @@ inline const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode) inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) { - return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); } inline const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_li2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_li2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_fmod(a.mp, x.mp, y.mp, rnd_mode); + mpfr_fmod(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); + return x; } #endif // MPFR 2.4.0 Specifics @@ -2138,16 +2138,16 @@ inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) inline const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_digamma(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_digamma(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_ai(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_ai(x.mp,v.mp,rnd_mode); + return x; } #endif // MPFR 3.0.0 Specifics @@ -2156,192 +2156,192 @@ inline const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode) // Constants inline const mpreal const_log2 (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_log2(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_log2(x.mp,rnd_mode); + return x; } inline const mpreal const_pi (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_pi(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_pi(x.mp,rnd_mode); + return x; } inline const mpreal const_euler (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_euler(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_euler(x.mp,rnd_mode); + return x; } inline const mpreal const_catalan (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_catalan(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_catalan(x.mp,rnd_mode); + return x; } inline const mpreal const_infinity (int sign, mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec,rnd_mode); - mpfr_set_inf(x.mp, sign); - return x; + mpreal x; + x.set_prec(prec,rnd_mode); + mpfr_set_inf(x.mp, sign); + return x; } ////////////////////////////////////////////////////////////////////////// // Integer Related Functions inline const mpreal rint(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal ceil(const mpreal& v) { - mpreal x(v); - mpfr_ceil(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_ceil(x.mp,v.mp); + return x; } inline const mpreal floor(const mpreal& v) { - mpreal x(v); - mpfr_floor(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_floor(x.mp,v.mp); + return x; } inline const mpreal round(const mpreal& v) { - mpreal x(v); - mpfr_round(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_round(x.mp,v.mp); + return x; } inline const mpreal trunc(const mpreal& v) { - mpreal x(v); - mpfr_trunc(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_trunc(x.mp,v.mp); + return x; } inline const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_ceil(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_ceil(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_floor(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_floor(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_round(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_round(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_trunc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_trunc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_frac(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_frac(x.mp,v.mp,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// // Miscellaneous Functions inline void swap(mpreal& a, mpreal& b) { - mpfr_swap(a.mp,b.mp); + mpfr_swap(a.mp,b.mp); } inline const mpreal (max)(const mpreal& x, const mpreal& y) { - return (x>y?x:y); + return (x>y?x:y); } inline const mpreal (min)(const mpreal& x, const mpreal& y) { - return (x= MPFR_VERSION_NUM(3,0,0)) // use gmp_randinit_default() to init state, gmp_randclear() to clear inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_urandom(x.mp,state,rnd_mode); - return x; + mpreal x; + mpfr_urandom(x.mp,state,rnd_mode); + return x; } #endif #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) inline const mpreal random2 (mp_size_t size, mp_exp_t exp) { - mpreal x; - mpfr_random2(x.mp,size,exp); - return x; + mpreal x; + mpfr_random2(x.mp,size,exp); + return x; } #endif @@ -2353,22 +2353,22 @@ inline const mpreal random(unsigned int seed) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - static gmp_randstate_t state; - static bool isFirstTime = true; + static gmp_randstate_t state; + static bool isFirstTime = true; - if(isFirstTime) - { - gmp_randinit_default(state); - gmp_randseed_ui(state,0); - isFirstTime = false; - } + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,0); + isFirstTime = false; + } - if(seed != 0) gmp_randseed_ui(state,seed); + if(seed != 0) gmp_randseed_ui(state,seed); - return mpfr::urandom(state); + return mpfr::urandom(state); #else - if(seed != 0) std::srand(seed); - return mpfr::mpreal(std::rand()/(double)RAND_MAX); + if(seed != 0) std::srand(seed); + return mpfr::mpreal(std::rand()/(double)RAND_MAX); #endif } @@ -2377,346 +2377,346 @@ inline const mpreal random(unsigned int seed) // Set/Get global properties inline void mpreal::set_default_prec(mp_prec_t prec) { - default_prec = prec; - mpfr_set_default_prec(prec); + default_prec = prec; + mpfr_set_default_prec(prec); } inline mp_prec_t mpreal::get_default_prec() { - return (mpfr_get_default_prec)(); + return (mpfr_get_default_prec)(); } inline void mpreal::set_default_base(int base) { - default_base = base; + default_base = base; } inline int mpreal::get_default_base() { - return default_base; + return default_base; } inline void mpreal::set_default_rnd(mp_rnd_t rnd_mode) { - default_rnd = rnd_mode; - mpfr_set_default_rounding_mode(rnd_mode); + default_rnd = rnd_mode; + mpfr_set_default_rounding_mode(rnd_mode); } inline mp_rnd_t mpreal::get_default_rnd() { - return static_cast((mpfr_get_default_rounding_mode)()); + return static_cast((mpfr_get_default_rounding_mode)()); } inline void mpreal::set_double_bits(int dbits) { - double_bits = dbits; + double_bits = dbits; } inline int mpreal::get_double_bits() { - return double_bits; + return double_bits; } inline bool mpreal::fits_in_bits(double x, int n) { - int i; - double t; - return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); + int i; + double t; + return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); } inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_z(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_z(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); + return pow(a,static_cast(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_si(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_si(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); + return pow(a,static_cast(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const long double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); + return pow(a,mpreal(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); + return pow(a,mpreal(b),rnd_mode); } inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); + return x; } inline const mpreal pow(const unsigned int a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),b,rnd_mode); + return pow(static_cast(a),b,rnd_mode); } inline const mpreal pow(const long int a, const mpreal& b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + if (a>=0) return pow(static_cast(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const int a, const mpreal& b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + if (a>=0) return pow(static_cast(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const long double a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); + return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const double a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); + return pow(mpreal(a),b,rnd_mode); } // pow unsigned long int inline const mpreal pow(const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_ui_pow_ui(x.mp,a,b,rnd_mode); - return x; + mpreal x(a); + mpfr_ui_pow_ui(x.mp,a,b,rnd_mode); + return x; } inline const mpreal pow(const unsigned long int a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned long int a, const long int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const long double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } // pow unsigned int inline const mpreal pow(const unsigned int a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned int a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned int a, const long int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const long double b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const double b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } // pow long int inline const mpreal pow(const long int a, const unsigned long int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long int a, const unsigned int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long int a, const long int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const long int a, const int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const long int a, const long double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } inline const mpreal pow(const long int a, const double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } // pow int inline const mpreal pow(const int a, const unsigned long int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const int a, const unsigned int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const int a, const long int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const int a, const int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const int a, const long double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } inline const mpreal pow(const int a, const double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } // pow long double inline const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),mpreal(b),rnd_mode); + return pow(mpreal(a),mpreal(b),rnd_mode); } inline const mpreal pow(const long double a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long double a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long double a, const long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si } inline const mpreal pow(const long double a, const int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si } inline const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),mpreal(b),rnd_mode); + return pow(mpreal(a),mpreal(b),rnd_mode); } inline const mpreal pow(const double a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_ui + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_ui } inline const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_ui + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_ui } inline const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si } inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si } } // End of mpfr namespace @@ -2725,11 +2725,11 @@ inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) // Non-throwing swap C++ idiom: http://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Non-throwing_swap namespace std { - template <> - inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) - { - return mpfr::swap(x, y); - } + template <> + inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) + { + return mpfr::swap(x, y); + } } #endif /* __MPREAL_H__ */ diff --git a/gtsam/3rdparty/UFconfig/UFconfig.h b/gtsam/3rdparty/UFconfig/UFconfig.h index 464867b33..795f5668c 100644 --- a/gtsam/3rdparty/UFconfig/UFconfig.h +++ b/gtsam/3rdparty/UFconfig/UFconfig.h @@ -75,10 +75,10 @@ extern "C" { typedef struct UFconfig_struct { - void *(*malloc_memory) (size_t) ; /* pointer to malloc */ + void *(*malloc_memory) (size_t) ; /* pointer to malloc */ void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ - void (*free_memory) (void *) ; /* pointer to free */ - void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ + void (*free_memory) (void *) ; /* pointer to free */ + void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ } UFconfig ; @@ -111,31 +111,31 @@ void *UFfree /* always returns NULL */ * * SuiteSparse Version 3.6.1 contains the following packages: * - * AMD version 2.2.2 - * BTF version 1.1.2 - * CAMD version 2.2.2 - * CCOLAMD version 2.7.3 - * CHOLMOD version 1.7.3 - * COLAMD version 2.7.3 - * CSparse version 2.2.5 - * CSparse3 version 3.0.1 - * CXSparse version 2.2.5 - * KLU version 1.1.2 - * LDL version 2.0.3 - * RBio version 2.0.1 + * AMD version 2.2.2 + * BTF version 1.1.2 + * CAMD version 2.2.2 + * CCOLAMD version 2.7.3 + * CHOLMOD version 1.7.3 + * COLAMD version 2.7.3 + * CSparse version 2.2.5 + * CSparse3 version 3.0.1 + * CXSparse version 2.2.5 + * KLU version 1.1.2 + * LDL version 2.0.3 + * RBio version 2.0.1 * SPQR version 1.2.2 (also called SuiteSparseQR) * UFcollection version 1.5.0 - * UFconfig version number is the same as SuiteSparse - * UMFPACK version 5.5.1 + * UFconfig version number is the same as SuiteSparse + * UMFPACK version 5.5.1 * LINFACTOR version 1.1.0 * MESHND version 1.1.1 * SSMULT version 2.0.2 * MATLAB_Tools no specific version number * * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) */ #define SUITESPARSE_DATE "May 10, 2011" diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 6d79dcb53..2bf18916d 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -26,71 +26,71 @@ namespace gtsam { /* ************************************************************************* */ DSFVector::DSFVector (const size_t numNodes) { - v_ = boost::make_shared(numNodes); - int index = 0; - keys_.reserve(numNodes); - for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) { - *it = index; - keys_.push_back(index); - } + v_ = boost::make_shared(numNodes); + int index = 0; + keys_.reserve(numNodes); + for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) { + *it = index; + keys_.push_back(index); + } } /* ************************************************************************* */ DSFVector::DSFVector(const boost::shared_ptr& v_in, const std::vector& keys) : keys_(keys) { - v_ = v_in; - BOOST_FOREACH(const size_t key, keys) - (*v_)[key] = key; + v_ = v_in; + BOOST_FOREACH(const size_t key, keys) + (*v_)[key] = key; } /* ************************************************************************* */ bool DSFVector::isSingleton(const Label& label) const { - bool result = false; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); ++it) { - if(findSet(*it) == label) { - if (!result) // find the first occurrence - result = true; - else - return false; - } - } - return result; + bool result = false; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); ++it) { + if(findSet(*it) == label) { + if (!result) // find the first occurrence + result = true; + else + return false; + } + } + return result; } /* ************************************************************************* */ std::set DSFVector::set(const Label& label) const { - std::set set; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - if (findSet(*it) == label) - set.insert(*it); - } - return set; + std::set set; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + if (findSet(*it) == label) + set.insert(*it); + } + return set; } /* ************************************************************************* */ std::map > DSFVector::sets() const { - std::map > sets; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - sets[findSet(*it)].insert(*it); - } - return sets; + std::map > sets; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + sets[findSet(*it)].insert(*it); + } + return sets; } /* ************************************************************************* */ std::map > DSFVector::arrays() const { - std::map > arrays; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - arrays[findSet(*it)].push_back(*it); - } - return arrays; + std::map > arrays; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + arrays[findSet(*it)].push_back(*it); + } + return arrays; } /* ************************************************************************* */ void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) { - (*v_)[findSet(i2)] = findSet(i1); + (*v_)[findSet(i2)] = findSet(i1); } } // namespace gtsam diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index 3940723c4..80b26c3ef 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -25,55 +25,55 @@ namespace gtsam { - /** - * A fast implementation of disjoint set forests that uses vector as underly data structure. - * @addtogroup base - */ - class DSFVector { + /** + * A fast implementation of disjoint set forests that uses vector as underly data structure. + * @addtogroup base + */ + class DSFVector { - public: - typedef std::vector V; ///< Vector of ints - typedef size_t Label; ///< Label type - typedef V::const_iterator const_iterator; ///< const iterator over V - typedef V::iterator iterator;///< iterator over V + public: + typedef std::vector V; ///< Vector of ints + typedef size_t Label; ///< Label type + typedef V::const_iterator const_iterator; ///< const iterator over V + typedef V::iterator iterator;///< iterator over V - private: - // TODO could use existing memory to improve the efficiency - boost::shared_ptr v_; - std::vector keys_; + private: + // TODO could use existing memory to improve the efficiency + boost::shared_ptr v_; + std::vector keys_; - public: - /// constructor that allocate a new memory - DSFVector(const size_t numNodes); + public: + /// constructor that allocate a new memory + DSFVector(const size_t numNodes); - /// constructor that uses the existing memory - DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); + /// constructor that uses the existing memory + DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); - /// find the label of the set in which {key} lives - inline Label findSet(size_t key) const { - size_t parent = (*v_)[key]; - while (parent != key) { - key = parent; - parent = (*v_)[key]; - } - return parent; - } + /// find the label of the set in which {key} lives + inline Label findSet(size_t key) const { + size_t parent = (*v_)[key]; + while (parent != key) { + key = parent; + parent = (*v_)[key]; + } + return parent; + } - /// find whether there is one and only one occurrence for the given {label} - bool isSingleton(const Label& label) const; + /// find whether there is one and only one occurrence for the given {label} + bool isSingleton(const Label& label) const; - /// get the nodes in the tree with the given label - std::set set(const Label& label) const; + /// get the nodes in the tree with the given label + std::set set(const Label& label) const; - /// return all sets, i.e. a partition of all elements - std::map > sets() const; + /// return all sets, i.e. a partition of all elements + std::map > sets() const; - /// return all sets, i.e. a partition of all elements - std::map > arrays() const; + /// return all sets, i.e. a partition of all elements + std::map > arrays() const; - /// the in-place version of makeUnion - void makeUnionInPlace(const size_t& i1, const size_t& i2); + /// the in-place version of makeUnion + void makeUnionInPlace(const size_t& i1, const size_t& i2); - }; + }; } diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 2dc6f3d67..9c61667ff 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -26,21 +26,21 @@ template class DerivedValue : public Value { protected: - DerivedValue() {} + DerivedValue() {} public: - virtual ~DerivedValue() {} + virtual ~DerivedValue() {} /** * Create a duplicate object returned as a pointer to the generic Value interface. * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. */ virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - DERIVED* ptr = new(place) DERIVED(static_cast(*this)); - return ptr; + void *place = boost::singleton_pool::malloc(); + DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + return ptr; } /** @@ -48,15 +48,15 @@ public: */ virtual void deallocate_() const { this->~DerivedValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*)this); // Release memory from pool + boost::singleton_pool::free((void*)this); // Release memory from pool } - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(static_cast(*this)); - } + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(static_cast(*this)); + } /// equals implementing generic Value interface virtual bool equals_(const Value& p, double tol = 1e-9) const { @@ -68,7 +68,7 @@ public: } /// Generic Value interface version of retract - virtual Value* retract_(const Vector& delta) const { + virtual Value* retract_(const Vector& delta) const { // Call retract on the derived class const DERIVED retractResult = (static_cast(this))->retract(delta); @@ -78,30 +78,30 @@ public: // Return the pointer to the Value base class return resultAsValue; - } + } - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { // Cast the base class Value pointer to a derived class pointer const DERIVED& derivedValue2 = dynamic_cast(value2); // Return the result of calling localCoordinates on the derived class return (static_cast(this))->localCoordinates(derivedValue2); - } + } - /// Assignment operator - virtual Value& operator=(const Value& rhs) { + /// Assignment operator + virtual Value& operator=(const Value& rhs) { // Cast the base class Value pointer to a derived class pointer const DERIVED& derivedRhs = dynamic_cast(rhs); // Do the assignment and return the result return (static_cast(this))->operator=(derivedRhs); - } + } - /// Conversion to the derived class - operator const DERIVED& () const { - return static_cast(*this); - } + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } /// Conversion to the derived class operator DERIVED& () { @@ -109,16 +109,16 @@ public: } protected: - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { - // Nothing to do, do not call base class assignment operator - return *this; - } + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + DerivedValue& operator=(const DerivedValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } private: - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { }; + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; }; diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index aa1ab24e8..5c80799ce 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -31,7 +31,7 @@ namespace gtsam { * convenience to avoid having lengthy types in the code. Through timing, * we've seen that the fast_pool_allocator can lead to speedups of several * percent. - * @addtogroup base + * @addtogroup base */ template class FastList: public std::list > { diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index 33bc5cb7c..f725cacc0 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -88,10 +88,10 @@ public: Base::assign(x.begin(), x.end()); } - /** Conversion to a standard STL container */ - operator std::vector() const { - return std::vector(this->begin(), this->end()); - } + /** Conversion to a standard STL container */ + operator std::vector() const { + return std::vector(this->begin(), this->end()); + } private: /** Serialization function */ diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3997b6c30..55762a108 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -28,21 +28,21 @@ namespace gtsam { template class GroupConcept { private: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + static T concept_check(const T& t) { + /** assignment */ + T t2 = t; - /** identity */ - T identity = T::identity(); + /** identity */ + T identity = T::identity(); - /** compose with another object */ - T compose_ret = identity.compose(t2); + /** compose with another object */ + T compose_ret = identity.compose(t2); - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); + /** invert the object and yield a new one */ + T inverse_ret = compose_ret.inverse(); - return inverse_ret; - } + return inverse_ret; + } }; } // \namespace gtsam diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index a770d9975..7a9d32249 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -60,15 +60,15 @@ inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap * as x2 = x1.compose(Expmap(v)). * * Expmap around identity - * static T Expmap(const Vector& v); + * static T Expmap(const Vector& v); * * Logmap around identity - * static Vector Logmap(const T& p); + * static Vector Logmap(const T& p); * * Compute l0 s.t. l2=l1*l0, where (*this) is l1 * A default implementation of between(*this, lp) is available: * between_default() - * T between(const T& l2) const; + * T between(const T& l2) const; * * By convention, we use capital letters to designate a static function * @@ -77,28 +77,28 @@ inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap template class LieConcept { private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { + /** concept checking function - implement the functions this demands */ + static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = expmap_identity_ret.between(t2); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = expmap_identity_ret.between(t2); - return between_ret; - } + return between_ret; + } }; @@ -112,10 +112,10 @@ private: /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { - static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; - T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); } /** @@ -132,8 +132,8 @@ template Matrix wedge(const Vector& x); */ template T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return T(expm(xhat,K)); + Matrix xhat = wedge(x); + return T(expm(xhat,K)); } } // namespace gtsam @@ -147,11 +147,11 @@ T expm(const Vector& x, int K=7) { * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 5844e8201..eb52011a1 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -31,109 +31,109 @@ namespace gtsam { */ struct LieMatrix : public Matrix, public DerivedValue { - /** default constructor - should be unnecessary */ - LieMatrix() {} + /** default constructor - should be unnecessary */ + LieMatrix() {} - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : Matrix(Eigen::Map(data, m, n)) {} - /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ - LieMatrix(size_t m, size_t n, ...); + /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ + LieMatrix(size_t m, size_t n, ...); - /** get the underlying vector */ - inline Matrix matrix() const { - return static_cast(*this); - } + /** get the underlying vector */ + inline Matrix matrix() const { + return static_cast(*this); + } - /** print @param s optional string naming the object */ - inline void print(const std::string& name="") const { - gtsam::print(matrix(), name); - } + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(matrix(), name); + } - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->size(); } - /** Update the LieMatrix with a tangent space update. The elements of the + /** Update the LieMatrix with a tangent space update. The elements of the * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ - inline LieMatrix retract(const Vector& v) const { + inline LieMatrix retract(const Vector& v) const { if(v.size() != this->rows() * this->cols()) throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); return LieMatrix(*this + Eigen::Map(&v(0), this->cols(), this->rows()).transpose()); } - /** @return the local coordinates of another object. The elements of the + /** @return the local coordinates of another object. The elements of the * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ - inline Vector localCoordinates(const LieMatrix& t2) const { + inline Vector localCoordinates(const LieMatrix& t2) const { Vector result(this->rows() * this->cols()); Eigen::Map(&result(0), this->cols(), this->rows()).transpose() = (t2 - *this); return result; } - // Group requirements + // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieMatrix compose(const LieMatrix& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); + // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments + // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class + // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector + // as the other geometry objects (Point3, Rot3, etc.) have this problem + /** compose with another object */ + inline LieMatrix compose(const LieMatrix& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); - return LieMatrix(*this + p); - } + return LieMatrix(*this + p); + } - /** between operation */ - inline LieMatrix between(const LieMatrix& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieMatrix(l2 - *this); - } + /** between operation */ + inline LieMatrix between(const LieMatrix& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); + return LieMatrix(l2 - *this); + } - /** invert the object and yield a new one */ - inline LieMatrix inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); + /** invert the object and yield a new one */ + inline LieMatrix inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); - return LieMatrix(-(*this)); - } + return LieMatrix(-(*this)); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieMatrix Expmap(const Vector& v) { + /** Expmap around identity */ + static inline LieMatrix Expmap(const Vector& v) { throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); return LieMatrix(v); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieMatrix& p) { + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } private: diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 3721492ec..dc8f29645 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -22,80 +22,80 @@ namespace gtsam { - /** - * LieScalar is a wrapper around double to allow it to be a Lie type - */ - struct LieScalar : public DerivedValue { + /** + * LieScalar is a wrapper around double to allow it to be a Lie type + */ + struct LieScalar : public DerivedValue { - /** default constructor */ - LieScalar() : d_(0.0) {} + /** default constructor */ + LieScalar() : d_(0.0) {} - /** wrap a double */ - explicit LieScalar(double d) : d_(d) {} + /** wrap a double */ + explicit LieScalar(double d) : d_(d) {} - /** access the underlying value */ - double value() const { return d_; } + /** access the underlying value */ + double value() const { return d_; } - /** Automatic conversion to underlying value */ - operator double() const { return d_; } + /** Automatic conversion to underlying value */ + operator double() const { return d_; } - /** print @param name optional string naming the object */ - inline void print(const std::string& name="") const { - std::cout << name << ": " << d_ << std::endl; - } + /** print @param name optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } - /** equality up to tolerance */ - inline bool equals(const LieScalar& expected, double tol=1e-5) const { - return fabs(expected.d_ - d_) <= tol; - } + /** equality up to tolerance */ + inline bool equals(const LieScalar& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return 1; } - inline static size_t Dim() { return 1; } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } - /** Update the LieScalar with a tangent space update */ - inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } + /** Update the LieScalar with a tangent space update */ + inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } - // Group requirements + // Group requirements - /** identity */ - inline static LieScalar identity() { - return LieScalar(); - } + /** identity */ + inline static LieScalar identity() { + return LieScalar(); + } - /** compose with another object */ - inline LieScalar compose(const LieScalar& p) const { - return LieScalar(d_ + p.d_); - } + /** compose with another object */ + inline LieScalar compose(const LieScalar& p) const { + return LieScalar(d_ + p.d_); + } - /** between operation */ - inline LieScalar between(const LieScalar& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(1); - if(H2) *H2 = eye(1); - return LieScalar(l2.value() - value()); - } + /** between operation */ + inline LieScalar between(const LieScalar& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(1); + if(H2) *H2 = eye(1); + return LieScalar(l2.value() - value()); + } - /** invert the object and yield a new one */ - inline LieScalar inverse() const { - return LieScalar(-1.0 * value()); - } + /** invert the object and yield a new one */ + inline LieScalar inverse() const { + return LieScalar(-1.0 * value()); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } + /** Expmap around identity */ + static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } - private: - double d_; - }; + private: + double d_; + }; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 916730fd3..20c705a23 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -28,96 +28,96 @@ namespace gtsam { */ struct LieVector : public Vector, public DerivedValue { - /** default constructor - should be unnecessary */ - LieVector() {} + /** default constructor - should be unnecessary */ + LieVector() {} - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} + /** initialize from a fixed size normal vector */ + template + LieVector(const Eigen::Matrix& v) : Vector(v) {} - /** wrap a double */ - LieVector(double d) : Vector(Vector_(1, d)) {} + /** wrap a double */ + LieVector(double d) : Vector(Vector_(1, d)) {} - /** constructor with size and initial data, row order ! */ - LieVector(size_t m, const double* const data); + /** constructor with size and initial data, row order ! */ + LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - LieVector(size_t m, ...); + /** Specify arguments directly, as in Vector_() - always force these to be doubles */ + LieVector(size_t m, ...); - /** get the underlying vector */ - inline Vector vector() const { - return static_cast(*this); - } + /** get the underlying vector */ + inline Vector vector() const { + return static_cast(*this); + } - /** print @param name optional string naming the object */ - inline void print(const std::string& name="") const { - gtsam::print(vector(), name); - } + /** print @param name optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(vector(), name); + } - /** equality up to tolerance */ - inline bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } + /** equality up to tolerance */ + inline bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->size(); } - /** Update the LieVector with a tangent space update */ - inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } + /** Update the LieVector with a tangent space update */ + inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - // Group requirements + // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); + } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieVector compose(const LieVector& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); + // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments + // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class + // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector + // as the other geometry objects (Point3, Rot3, etc.) have this problem + /** compose with another object */ + inline LieVector compose(const LieVector& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); - return LieVector(vector() + p); - } + return LieVector(vector() + p); + } - /** between operation */ - inline LieVector between(const LieVector& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieVector(l2.vector() - vector()); - } + /** between operation */ + inline LieVector between(const LieVector& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); + return LieVector(l2.vector() - vector()); + } - /** invert the object and yield a new one */ - inline LieVector inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); + /** invert the object and yield a new one */ + inline LieVector inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); - return LieVector(-1.0 * vector()); - } + return LieVector(-1.0 * vector()); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieVector Expmap(const Vector& v) { return LieVector(v); } + /** Expmap around identity */ + static inline LieVector Expmap(const Vector& v) { return LieVector(v); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieVector& p) { return p; } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieVector& p) { return p; } private: diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index b84a6b525..ceebf6bad 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -49,14 +49,14 @@ namespace gtsam { * * Returns dimensionality of the tangent space, which may be smaller than the number * of nonlinear parameters. - * size_t dim() const; + * size_t dim() const; * * Returns a new T that is a result of updating *this with the delta v after pulling * the updated value back to the manifold T. - * T retract(const Vector& v) const; + * T retract(const Vector& v) const; * * Returns the linear coordinates of lp in the tangent space centered around *this. - * Vector localCoordinates(const T& lp) const; + * Vector localCoordinates(const T& lp) const; * * By convention, we use capital letters to designate a static function * @tparam T is a Lie type, like Point2, Pose3, etc. @@ -64,29 +64,29 @@ namespace gtsam { template class ManifoldConcept { private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { + /** concept checking function - implement the functions this demands */ + static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); - return retract_ret; - } + return retract_ret; + } }; } // namespace gtsam diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1369f13b8..40731591f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -40,200 +40,200 @@ namespace gtsam { /* ************************************************************************* */ Matrix Matrix_( size_t m, size_t n, const double* const data) { - MatrixRowMajor A(m,n); - copy(data, data+m*n, A.data()); - return Matrix(A); + MatrixRowMajor A(m,n); + copy(data, data+m*n, A.data()); + return Matrix(A); } /* ************************************************************************* */ Matrix Matrix_( size_t m, size_t n, const Vector& v) { - Matrix A(m,n); - // column-wise copy - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - A(i,j) = v(k); - return A; + Matrix A(m,n); + // column-wise copy + for( size_t j = 0, k=0 ; j < n ; j++) + for( size_t i = 0; i < m ; i++,k++) + A(i,j) = v(k); + return A; } /* ************************************************************************* */ Matrix Matrix_(size_t m, size_t n, ...) { - Matrix A(m,n); - va_list ap; - va_start(ap, n); - for( size_t i = 0 ; i < m ; i++) - for( size_t j = 0 ; j < n ; j++) { - double value = va_arg(ap, double); - A(i,j) = value; - } - va_end(ap); - return A; + Matrix A(m,n); + va_list ap; + va_start(ap, n); + for( size_t i = 0 ; i < m ; i++) + for( size_t j = 0 ; j < n ; j++) { + double value = va_arg(ap, double); + A(i,j) = value; + } + va_end(ap); + return A; } /* ************************************************************************* */ Matrix zeros( size_t m, size_t n ) { - return Matrix::Zero(m,n); + return Matrix::Zero(m,n); } /* ************************************************************************* */ Matrix eye( size_t m, size_t n) { - return Matrix::Identity(m, n); + return Matrix::Identity(m, n); } /* ************************************************************************* */ Matrix diag(const Vector& v) { - return v.asDiagonal(); + return v.asDiagonal(); } /* ************************************************************************* */ bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; + if (equal_with_abs_tol(expected,actual,tol)) return true; - size_t n1 = expected.cols(), m1 = expected.rows(); - size_t n2 = actual.cols(), m2 = actual.rows(); + size_t n1 = expected.cols(), m1 = expected.rows(); + size_t n2 = actual.cols(), m2 = actual.rows(); - cout << "not equal:" << endl; - print(expected,"expected = "); - print(actual,"actual = "); - if(m1!=m2 || n1!=n2) - cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl; - else { - Matrix diff = actual-expected; - print(diff, "actual - expected = "); - } - return false; + cout << "not equal:" << endl; + print(expected,"expected = "); + print(actual,"actual = "); + if(m1!=m2 || n1!=n2) + cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl; + else { + Matrix diff = actual-expected; + print(diff, "actual - expected = "); + } + return false; } /* ************************************************************************* */ bool assert_equal(const std::list& As, const std::list& Bs, double tol) { - if (As.size() != Bs.size()) return false; + if (As.size() != Bs.size()) return false; - list::const_iterator itA, itB; - itA = As.begin(); itB = Bs.begin(); - for (; itA != As.end(); itA++, itB++) - if (!assert_equal(*itB, *itA, tol)) - return false; + list::const_iterator itA, itB; + itA = As.begin(); itB = Bs.begin(); + for (; itA != As.end(); itA++, itB++) + if (!assert_equal(*itB, *itA, tol)) + return false; - return true; + return true; } /* ************************************************************************* */ static bool is_linear_dependent(const Matrix& A, const Matrix& B, double tol) { - // This local static function is used by linear_independent and - // linear_dependent just below. - size_t n1 = A.cols(), m1 = A.rows(); - size_t n2 = B.cols(), m2 = B.rows(); + // This local static function is used by linear_independent and + // linear_dependent just below. + size_t n1 = A.cols(), m1 = A.rows(); + size_t n2 = B.cols(), m2 = B.rows(); - bool dependent = true; - if(m1!=m2 || n1!=n2) dependent = false; + bool dependent = true; + if(m1!=m2 || n1!=n2) dependent = false; - for(size_t i=0; dependent && i>(istream& inputStream, Matrix& destinationMatrix) { /* ************************************************************************* */ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) { - fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; + fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; } /* ************************************************************************* */ void insertColumn(Matrix& A, const Vector& col, size_t j) { - A.col(j) = col; + A.col(j) = col; } /* ************************************************************************* */ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { - A.col(j).segment(i, col.size()) = col; + A.col(j).segment(i, col.size()) = col; } /* ************************************************************************* */ Vector columnNormSquare(const Matrix &A) { - Vector v (A.cols()) ; - for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { - v[i] = A.col(i).dot(A.col(i)); - } - return v ; + Vector v (A.cols()) ; + for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { + v[i] = A.col(i).dot(A.col(i)); + } + return v ; } /* ************************************************************************* */ void solve(Matrix& A, Matrix& B) { - // Eigen version - untested - B = A.fullPivLu().solve(B); + // Eigen version - untested + B = A.fullPivLu().solve(B); } /* ************************************************************************* */ Matrix inverse(const Matrix& A) { - return A.inverse(); + return A.inverse(); } /* ************************************************************************* */ /** Householder QR factorization, Golub & Van Loan p 224, explicit version */ /* ************************************************************************* */ pair qr(const Matrix& A) { - const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); + const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); - Matrix Q=eye(m,m),R(A); - Vector v(m); + Matrix Q=eye(m,m),R(A); + Vector v(m); - // loop over the kprime first columns - for(size_t j=0; j < kprime; j++){ + // loop over the kprime first columns + for(size_t j=0; j < kprime; j++){ - // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end) - const size_t mm=m-j; + // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end) + const size_t mm=m-j; - // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j) - Vector xjm(mm); - for(size_t k = 0 ; k < mm; k++) - xjm(k) = R(j+k, j); + // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j) + Vector xjm(mm); + for(size_t k = 0 ; k < mm; k++) + xjm(k) = R(j+k, j); - // calculate the Householder vector v - double beta; Vector vjm; - boost::tie(beta,vjm) = house(xjm); + // calculate the Householder vector v + double beta; Vector vjm; + boost::tie(beta,vjm) = house(xjm); - // pad with zeros to get m-dimensional vector v - for(size_t k = 0 ; k < m; k++) - v(k) = k > weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { - size_t m = A.rows(), n = A.cols(); // get size(A) - size_t maxRank = min(m,n); + size_t m = A.rows(), n = A.cols(); // get size(A) + size_t maxRank = min(m,n); - // create list - list > results; + // create list + list > results; - Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once + Vector pseudo(m); // allocate storage for pseudo-inverse + Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once - // We loop over all columns, because the columns that can be eliminated - // are not necessarily contiguous. For each one, estimate the corresponding - // scalar variable x as d-rS, with S the separator (remaining columns). - // Then update A and b by substituting x with d-rS, zero-ing out x's column. - for (size_t j=0; j=maxRank) break; + // exit after rank exhausted + if (results.size()>=maxRank) break; - // update A, b, expensive, using outer product - // A' \define A_{S}-a*r and b'\define b-d*a - A -= a * r.transpose(); - b -= d * a; - } + // update A, b, expensive, using outer product + // A' \define A_{S}-a*r and b'\define b-d*a + A -= a * r.transpose(); + b -= d * a; + } - return results; + return results; } /* ************************************************************************* */ @@ -400,188 +400,188 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { /* ************************************************************************* */ void householder_(Matrix& A, size_t k, bool copy_vectors) { - const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); - // loop over the kprime first columns - for(size_t j=0; j < kprime; j++) { - // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j) - Vector vjm = A.col(j).segment(j, m-j); + const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); + // loop over the kprime first columns + for(size_t j=0; j < kprime; j++) { + // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j) + Vector vjm = A.col(j).segment(j, m-j); - // calculate the Householder vector, in place - double beta = houseInPlace(vjm); + // calculate the Householder vector, in place + double beta = houseInPlace(vjm); - // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w' - tic(1, "householder_update"); // bottleneck for system - // don't touch old columns - Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm; - A.block(j,j,m-j,n-j) -= vjm * w.transpose(); - toc(1, "householder_update"); + // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w' + tic(1, "householder_update"); // bottleneck for system + // don't touch old columns + Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm; + A.block(j,j,m-j,n-j) -= vjm * w.transpose(); + toc(1, "householder_update"); - // the Householder vector is copied in the zeroed out part - if (copy_vectors) { - tic(2, "householder_vector_copy"); - A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1)); - toc(2, "householder_vector_copy"); - } - } // column j + // the Householder vector is copied in the zeroed out part + if (copy_vectors) { + tic(2, "householder_vector_copy"); + A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1)); + toc(2, "householder_vector_copy"); + } + } // column j } /* ************************************************************************* */ void householder(Matrix& A, size_t k) { - // version with zeros below diagonal - tic(1, "householder_"); - householder_(A,k,false); - toc(1, "householder_"); -// tic(2, "householder_zero_fill"); -// const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); -// for(size_t j=0; j < kprime; j++) -// A.col(j).segment(j+1, m-(j+1)).setZero(); -// toc(2, "householder_zero_fill"); + // version with zeros below diagonal + tic(1, "householder_"); + householder_(A,k,false); + toc(1, "householder_"); +// tic(2, "householder_zero_fill"); +// const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); +// for(size_t j=0; j < kprime; j++) +// A.col(j).segment(j+1, m-(j+1)).setZero(); +// toc(2, "householder_zero_fill"); } /* ************************************************************************* */ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) { - // @return the solution x of L*x=b - assert(L.rows() == L.cols()); - if (unit) - return L.triangularView().solve(b); - else - return L.triangularView().solve(b); + // @return the solution x of L*x=b + assert(L.rows() == L.cols()); + if (unit) + return L.triangularView().solve(b); + else + return L.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) { - // @return the solution x of U*x=b - assert(U.rows() == U.cols()); - if (unit) - return U.triangularView().solve(b); - else - return U.triangularView().solve(b); + // @return the solution x of U*x=b + assert(U.rows() == U.cols()); + if (unit) + return U.triangularView().solve(b); + else + return U.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) { - // @return the solution x of x'*U=b' - assert(U.rows() == U.cols()); - if (unit) - return U.triangularView().transpose().solve(b); - else - return U.triangularView().transpose().solve(b); + // @return the solution x of x'*U=b' + assert(U.rows() == U.cols()); + if (unit) + return U.triangularView().transpose().solve(b); + else + return U.triangularView().transpose().solve(b); } /* ************************************************************************* */ Matrix stack(size_t nrMatrices, ...) { - size_t dimA1 = 0; - size_t dimA2 = 0; - va_list ap; - va_start(ap, nrMatrices); - for(size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - dimA1 += M->rows(); - dimA2 = M->cols(); // TODO: should check if all the same ! - } - va_end(ap); - va_start(ap, nrMatrices); - Matrix A(dimA1, dimA2); - size_t vindex = 0; - for( size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++) - for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++) - A(vindex+d1, d2) = (*M)(d1, d2); - vindex += M->rows(); - } + size_t dimA1 = 0; + size_t dimA2 = 0; + va_list ap; + va_start(ap, nrMatrices); + for(size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + dimA1 += M->rows(); + dimA2 = M->cols(); // TODO: should check if all the same ! + } + va_end(ap); + va_start(ap, nrMatrices); + Matrix A(dimA1, dimA2); + size_t vindex = 0; + for( size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++) + for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++) + A(vindex+d1, d2) = (*M)(d1, d2); + vindex += M->rows(); + } - return A; + return A; } /* ************************************************************************* */ Matrix collect(const std::vector& matrices, size_t m, size_t n) { - // if we have known and constant dimensions, use them - size_t dimA1 = m; - size_t dimA2 = n*matrices.size(); - if (!m && !n) { - BOOST_FOREACH(const Matrix* M, matrices) { - dimA1 = M->rows(); // TODO: should check if all the same ! - dimA2 += M->cols(); - } - } + // if we have known and constant dimensions, use them + size_t dimA1 = m; + size_t dimA2 = n*matrices.size(); + if (!m && !n) { + BOOST_FOREACH(const Matrix* M, matrices) { + dimA1 = M->rows(); // TODO: should check if all the same ! + dimA2 += M->cols(); + } + } - // stl::copy version - Matrix A(dimA1, dimA2); - size_t hindex = 0; - BOOST_FOREACH(const Matrix* M, matrices) { - size_t row_len = M->cols(); - A.block(0, hindex, dimA1, row_len) = *M; - hindex += row_len; - } + // stl::copy version + Matrix A(dimA1, dimA2); + size_t hindex = 0; + BOOST_FOREACH(const Matrix* M, matrices) { + size_t row_len = M->cols(); + A.block(0, hindex, dimA1, row_len) = *M; + hindex += row_len; + } - return A; + return A; } /* ************************************************************************* */ Matrix collect(size_t nrMatrices, ...) { - vector matrices; - va_list ap; - va_start(ap, nrMatrices); - for( size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - matrices.push_back(M); - } - return collect(matrices); + vector matrices; + va_list ap; + va_start(ap, nrMatrices); + for( size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + matrices.push_back(M); + } + return collect(matrices); } /* ************************************************************************* */ // row scaling, in-place void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { - const size_t m = A.rows(); - if (inf_mask) { - for (size_t i=0; i llt; - llt.compute(A); - return llt.matrixL(); + Matrix L = zeros(A.rows(), A.rows()); + Eigen::LLT llt; + llt.compute(A); + return llt.matrixL(); } Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + return LLt(A).transpose(); } /* @@ -627,32 +627,32 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); + // FIXME: replace with real algorithm + return A.inverse(); -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); +// Matrix L = LLt(A); +// Matrix inv(eye(A.rows())); +// inplace_solve (L, inv, BNU::lower_tag ()); +// return BNU::prod(trans(inv), inv); } #if 0 /* ************************************************************************* */ // TODO, would be faster with Cholesky Matrix inverse_square_root(const Matrix& A) { - size_t m = A.cols(), n = A.rows(); - if (m!=n) - throw invalid_argument("inverse_square_root: A must be square"); + size_t m = A.cols(), n = A.rows(); + if (m!=n) + throw invalid_argument("inverse_square_root: A must be square"); - // Perform SVD, TODO: symmetric SVD? - Matrix U,V; - Vector S; - svd(A,U,S,V); + // Perform SVD, TODO: symmetric SVD? + Matrix U,V; + Vector S; + svd(A,U,S,V); - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i().solveInPlace(inv); - return inv.transpose(); + Matrix R = RtR(A); + Matrix inv = eye(A.rows()); + R.triangularView().solveInPlace(inv); + return inv.transpose(); } /* ************************************************************************* */ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) { - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - U = svd.matrixU(); - S = svd.singularValues(); - V = svd.matrixV(); + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + U = svd.matrixU(); + S = svd.singularValues(); + V = svd.matrixV(); } /* ************************************************************************* */ boost::tuple DLT(const Matrix& A, double rank_tol) { - // Check size of A - int n = A.rows(), p = A.cols(), m = min(n,p); + // Check size of A + int n = A.rows(), p = A.cols(), m = min(n,p); - // Do SVD on A + // Do SVD on A Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); Vector s = svd.singularValues(); Matrix V = svd.matrixV(); - // Find rank - int rank = 0; - for (int j = 0; j < m; j++) - if (s(j) > rank_tol) rank++; + // Find rank + int rank = 0; + for (int j = 0; j < m; j++) + if (s(j) > rank_tol) rank++; - // Return rank, error, and corresponding column of V - double error = m

(rank, error, Vector(column(V, p-1))); + // Return rank, error, and corresponding column of V + double error = m

(rank, error, Vector(column(V, p-1))); } /* ************************************************************************* */ Matrix expm(const Matrix& A, size_t K) { - Matrix E = eye(A.rows()), A_k = eye(A.rows()); - for(size_t k=1;k<=K;k++) { - A_k = A_k*A/k; - E = E + A_k; - } - return E; + Matrix E = eye(A.rows()), A_k = eye(A.rows()); + for(size_t k=1;k<=K;k++) { + A_k = A_k*A/k; + E = E + A_k; + } + return E; } /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - int n = A.cols(); - assert(A.rows() == n); + int n = A.cols(); + assert(A.rows() == n); - // original -// const Matrix I = eye(n); -// return (I-A)*inverse(I+A); + // original +// const Matrix I = eye(n); +// return (I-A)*inverse(I+A); - // inlined to let Eigen do more optimization - return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); + // inlined to let Eigen do more optimization + return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index e6a40f727..a20a2357f 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -100,19 +100,19 @@ Matrix diag(const Vector& v); template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& B, double tol = 1e-9) { - const size_t n1 = A.cols(), m1 = A.rows(); - const size_t n2 = B.cols(), m2 = B.rows(); + const size_t n1 = A.cols(), m1 = A.rows(); + const size_t n2 = B.cols(), m2 = B.rows(); - if(m1!=m2 || n1!=n2) return false; + if(m1!=m2 || n1!=n2) return false; - for(size_t i=0; i tol) - return false; - } - return true; + for(size_t i=0; i tol) + return false; + } + return true; } /** @@ -183,8 +183,8 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec /** products using old-style format to improve compatibility */ template inline MATRIX prod(const MATRIX& A, const MATRIX&B) { - MATRIX result = A * B; - return result; + MATRIX result = A * B; + return result; } /** @@ -220,8 +220,8 @@ std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix); */ template Eigen::Block sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) { - size_t m=i2-i1, n=j2-j1; - return A.block(i1,j1,m,n); + size_t m=i2-i1, n=j2-j1; + return A.block(i1,j1,m,n); } /** @@ -242,7 +242,7 @@ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j); */ template const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) { - return A.col(j); + return A.col(j); } /** @@ -253,7 +253,7 @@ const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) { */ template const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { - return A.row(j); + return A.row(j); } /** @@ -276,10 +276,10 @@ Vector columnNormSquare(const Matrix &A); */ template void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { - const size_t m = A.rows(), n = A.cols(); - const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n); - for (size_t j=0; j qr(const Matrix& A); */ template void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); + size_t rows = A.rows(); + size_t cols = A.cols(); + size_t size = std::min(rows,cols); - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + HCoeffsType hCoeffs(size); + RowVectorType temp(cols); - Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); + Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); - zeroBelowDiagonal(A); + zeroBelowDiagonal(A); } /** @@ -478,8 +478,8 @@ Matrix Cayley(const Matrix& A); /// Eigen do more optimization template Matrix Cayley(const Eigen::Matrix& A) { - typedef Eigen::Matrix FMat; - return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse()); + typedef Eigen::Matrix FMat; + return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse()); } } // namespace gtsam @@ -488,31 +488,31 @@ Matrix Cayley(const Eigen::Matrix& A) { #include namespace boost { - namespace serialization { + namespace serialization { // split version - sends sizes ahead - template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { - const int rows = m.rows(), cols = m.cols(), elements = rows * cols; - std::vector raw_data(elements); - std::copy(m.data(), m.data() + elements, raw_data.begin()); - ar << make_nvp("rows", rows); - ar << make_nvp("cols", cols); - ar << make_nvp("data", raw_data); - } + template + void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { + const int rows = m.rows(), cols = m.cols(), elements = rows * cols; + std::vector raw_data(elements); + std::copy(m.data(), m.data() + elements, raw_data.begin()); + ar << make_nvp("rows", rows); + ar << make_nvp("cols", cols); + ar << make_nvp("data", raw_data); + } - template - void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { - size_t rows, cols; - std::vector raw_data; - ar >> make_nvp("rows", rows); - ar >> make_nvp("cols", cols); - ar >> make_nvp("data", raw_data); - m = gtsam::Matrix(rows, cols); - std::copy(raw_data.begin(), raw_data.end(), m.data()); - } + template + void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { + size_t rows, cols; + std::vector raw_data; + ar >> make_nvp("rows", rows); + ar >> make_nvp("cols", cols); + ar >> make_nvp("data", raw_data); + m = gtsam::Matrix(rows, cols); + std::copy(raw_data.begin(), raw_data.end(), m.data()); + } - } // namespace serialization + } // namespace serialization } // namespace boost BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index b6ae1b3ef..a308c50a1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -20,12 +20,12 @@ * the function exists in derived class and throw compile-time errors. * * print with optional string naming the object - * void print(const std::string& name) const = 0; + * void print(const std::string& name) const = 0; * * equality up to tolerance * tricky to implement, see NoiseModelFactor1 for an example * equals is not supposed to print out *anything*, just return true|false - * bool equals(const Derived& expected, double tol) const = 0; + * bool equals(const Derived& expected, double tol) const = 0; * */ @@ -41,84 +41,84 @@ namespace gtsam { - /** - * A testable concept check that should be placed in applicable unit - * tests and in generic algorithms. - * - * See macros for details on using this structure - * @addtogroup base - * @tparam T is the type this constrains to be testable - assumes print() and equals() - */ - template - class TestableConcept { - static bool checkTestableConcept(const T& d) { - // check print function, with optional string - d.print(std::string()); - d.print(); + /** + * A testable concept check that should be placed in applicable unit + * tests and in generic algorithms. + * + * See macros for details on using this structure + * @addtogroup base + * @tparam T is the type this constrains to be testable - assumes print() and equals() + */ + template + class TestableConcept { + static bool checkTestableConcept(const T& d) { + // check print function, with optional string + d.print(std::string()); + d.print(); - // check print, with optional threshold - double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; - } - }; + // check print, with optional threshold + double tol = 1.0; + bool r1 = d.equals(d, tol); + bool r2 = d.equals(d); + return r1 && r2; + } + }; - /** Call print on the object */ - template - inline void print(const T& object, const std::string& s = "") { - object.print(s); - } + /** Call print on the object */ + template + inline void print(const T& object, const std::string& s = "") { + object.print(s); + } - /** Call equal on the object */ - template - inline bool equal(const T& obj1, const T& obj2, double tol) { - return obj1.equals(obj2, tol); - } + /** Call equal on the object */ + template + inline bool equal(const T& obj1, const T& obj2, double tol) { + return obj1.equals(obj2, tol); + } - /** Call equal on the object without tolerance (use default tolerance) */ - template - inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); - } + /** Call equal on the object without tolerance (use default tolerance) */ + template + inline bool equal(const T& obj1, const T& obj2) { + return obj1.equals(obj2); + } - /** - * This template works for any type with equals - */ - template - bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) - return true; - printf("Not equal:\n"); - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } + /** + * This template works for any type with equals + */ + template + bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { + if (actual.equals(expected, tol)) + return true; + printf("Not equal:\n"); + expected.print("expected:\n"); + actual.print("actual:\n"); + return false; + } - /** - * Template to create a binary predicate - */ - template - struct equals : public std::binary_function { - double tol_; - equals(double tol = 1e-9) : tol_(tol) {} - bool operator()(const V& expected, const V& actual) { - return (actual.equals(expected, tol_)); - } - }; + /** + * Template to create a binary predicate + */ + template + struct equals : public std::binary_function { + double tol_; + equals(double tol = 1e-9) : tol_(tol) {} + bool operator()(const V& expected, const V& actual) { + return (actual.equals(expected, tol_)); + } + }; - /** - * Binary predicate on shared pointers - */ - template - struct equals_star : public std::binary_function&, const boost::shared_ptr&, bool> { - double tol_; - equals_star(double tol = 1e-9) : tol_(tol) {} - bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { - if (!actual || !expected) return false; - return (actual->equals(*expected, tol_)); - } - }; + /** + * Binary predicate on shared pointers + */ + template + struct equals_star : public std::binary_function&, const boost::shared_ptr&, bool> { + double tol_; + equals_star(double tol = 1e-9) : tol_(tol) {} + bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + if (!actual || !expected) return false; + return (actual->equals(*expected, tol_)); + } + }; } // \namespace gtsam diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 00ab79d30..7308b89a7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -47,36 +47,36 @@ inline bool assert_equal(const Index& expected, const Index& actual, double tol */ template bool assert_equal(const boost::optional& expected, - const boost::optional& actual, double tol = 1e-9) { - if (!expected && actual) { - std::cout << "expected is boost::none, while actual is not" << std::endl; - return false; - } - if (expected && !actual) { - std::cout << "actual is boost::none, while expected is not" << std::endl; - return false; - } - if (!expected && !actual) - return true; - return assert_equal(*expected, *actual, tol); + const boost::optional& actual, double tol = 1e-9) { + if (!expected && actual) { + std::cout << "expected is boost::none, while actual is not" << std::endl; + return false; + } + if (expected && !actual) { + std::cout << "actual is boost::none, while expected is not" << std::endl; + return false; + } + if (!expected && !actual) + return true; + return assert_equal(*expected, *actual, tol); } template bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } template bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } /** @@ -97,15 +97,15 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, } } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; } + std::cout << std::endl; + return false; + } + return true; } /** @@ -114,37 +114,37 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, */ template bool assert_container_equal(const std::map& expected, const std::map& actual, double tol = 1e-9) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(itExp->first, itAct->first, tol) || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(itExp->first, itAct->first, tol) || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - a.first.print("key"); - a.second.print(" value"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - a.first.print("key"); - a.second.print(" value"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + a.first.print("key"); + a.second.print(" value"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + a.first.print("key"); + a.second.print(" value"); + } + std::cout << std::endl; + return false; + } + return true; } /** @@ -152,37 +152,37 @@ bool assert_container_equal(const std::map& expected, const std::map bool assert_container_equal(const std::map& expected, const std::map& actual, double tol = 1e-9) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (itExp->first != itAct->first || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - std::cout << "Key: " << a.first << std::endl; - a.second.print(" value"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - std::cout << "Key: " << a.first << std::endl; - a.second.print(" value"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + a.second.print(" value"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + a.second.print(" value"); + } + std::cout << std::endl; + return false; + } + return true; } /** @@ -190,38 +190,38 @@ bool assert_container_equal(const std::map& expected, const std::map< */ template bool assert_container_equal(const std::vector >& expected, - const std::vector >& actual, double tol = 1e-9) { - typedef typename std::vector > VectorPair; + const std::vector >& actual, double tol = 1e-9) { + typedef typename std::vector > VectorPair; bool match = true; if (expected.size() != actual.size()) match = false; typename VectorPair::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(itExp->first, itAct->first, tol) || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(itExp->first, itAct->first, tol) || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, expected) { - a.first.print( " first "); - a.second.print(" second"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, actual) { - a.first.print( " first "); - a.second.print(" second"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename VectorPair::value_type& a, expected) { + a.first.print( " first "); + a.second.print(" second"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename VectorPair::value_type& a, actual) { + a.first.print( " first "); + a.second.print(" second"); + } + std::cout << std::endl; + return false; + } + return true; } @@ -234,25 +234,25 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- if (expected.size() != actual.size()) match = false; typename V::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(*itExp, *itAct, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(*itExp, *itAct, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); } + std::cout << std::endl; + return false; + } + return true; } /** @@ -261,36 +261,36 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- */ template bool assert_container_equality(const std::map& expected, const std::map& actual) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (itExp->first != itAct->first || itExp->second != itAct->second) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || itExp->second != itAct->second) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - std::cout << "Key: " << a.first << std::endl; - std::cout << "Value: " << a.second << std::endl; - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - std::cout << "Key: " << a.first << std::endl; - std::cout << "Value: " << a.second << std::endl; - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << std::endl; + return false; + } + return true; } @@ -303,37 +303,37 @@ bool assert_container_equality(const V& expected, const V& actual) { if (expected.size() != actual.size()) match = false; typename V::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (*itExp != *itAct) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (*itExp != *itAct) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; } + std::cout << std::endl; + return false; + } + return true; } /** * Compare strings for unit tests */ inline bool assert_equal(const std::string& expected, const std::string& actual) { - if (expected == actual) - return true; - printf("Not equal:\n"); - std::cout << "expected: [" << expected << "]\n"; - std::cout << "actual: [" << actual << "]" << std::endl; - return false; + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; } /** @@ -341,12 +341,12 @@ inline bool assert_equal(const std::string& expected, const std::string& actual) */ template bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) { - if (!actual.equals(expected, tol)) - return true; - printf("Erroneously equal:\n"); - expected.print("expected"); - actual.print("actual"); - return false; + if (!actual.equals(expected, tol)) + return true; + printf("Erroneously equal:\n"); + expected.print("expected"); + actual.print("actual"); + return false; } } // \namespace gtsam diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 8b19a6532..53c374497 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -107,8 +107,8 @@ namespace gtsam { /** Deallocate a raw pointer of this value */ virtual void deallocate_() const = 0; - /** Clone this value (normal clone on the heap, delete with 'delete' operator) */ - virtual boost::shared_ptr clone() const = 0; + /** Clone this value (normal clone on the heap, delete with 'delete' operator) */ + virtual boost::shared_ptr clone() const = 0; /** Compare this Value with another for equality. */ virtual bool equals_(const Value& other, double tol = 1e-9) const = 0; @@ -146,39 +146,39 @@ namespace gtsam { virtual ~Value() {} private: - /** Empty serialization function. - * - * There are two important things that users need to do to serialize derived objects in Values successfully: - * (Those derived objects are stored in Values as pointer to this abstract base class Value) - * - * 1. All DERIVED classes derived from Value must put the following line in their serialization function: - * \code - ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object(*this)); - \endcode - * or, alternatively - * \code - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - \endcode - * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting - * - * 2. The source module that includes archive class headers to serialize objects of derived classes - * (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either - * BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros: - \code - BOOST_CLASS_EXPORT(DERIVED_CLASS_1) - BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING") - \endcode - * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers - * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export - * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\ - * http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export - * http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export - * The last two links explain why these export lines have to be in the same source module that includes - * any of the archive class headers. - * */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) {} + /** Empty serialization function. + * + * There are two important things that users need to do to serialize derived objects in Values successfully: + * (Those derived objects are stored in Values as pointer to this abstract base class Value) + * + * 1. All DERIVED classes derived from Value must put the following line in their serialization function: + * \code + ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object(*this)); + \endcode + * or, alternatively + * \code + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting + * + * 2. The source module that includes archive class headers to serialize objects of derived classes + * (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either + * BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros: + \code + BOOST_CLASS_EXPORT(DERIVED_CLASS_1) + BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING") + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\ + * http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export + * The last two links explain why these export lines have to be in the same source module that includes + * any of the archive class headers. + * */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) {} }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index a7e8f2769..82d503837 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -41,42 +41,42 @@ namespace gtsam { /* ************************************************************************* */ void odprintf_(const char *format, ostream& stream, ...) { - char buf[4096], *p = buf; + char buf[4096], *p = buf; - va_list args; - va_start(args, stream); + va_list args; + va_start(args, stream); #ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif - va_end(args); + va_end(args); //#ifdef WIN32 -// OutputDebugString(buf); +// OutputDebugString(buf); //#else - stream << buf; + stream << buf; //#endif } /* ************************************************************************* */ void odprintf(const char *format, ...) { - char buf[4096], *p = buf; + char buf[4096], *p = buf; - va_list args; - va_start(args, format); + va_list args; + va_start(args, format); #ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif - va_end(args); + va_end(args); //#ifdef WIN32 -// OutputDebugString(buf); +// OutputDebugString(buf); //#else - cout << buf; + cout << buf; //#endif } @@ -89,343 +89,343 @@ Vector Vector_( size_t m, const double* const data) { /* ************************************************************************* */ Vector Vector_(size_t m, ...) { - Vector v(m); - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - v(i) = value; - } - va_end(ap); - return v; + Vector v(m); + va_list ap; + va_start(ap, m); + for( size_t i = 0 ; i < m ; i++) { + double value = va_arg(ap, double); + v(i) = value; + } + va_end(ap); + return v; } /* ************************************************************************* */ Vector Vector_(const std::vector& d) { - Vector v(d.size()); - copy(d.begin(), d.end(), v.data()); - return v; + Vector v(d.size()); + copy(d.begin(), d.end(), v.data()); + return v; } /* ************************************************************************* */ bool zero(const Vector& v) { - bool result = true; - size_t n = v.size(); - for( size_t j = 0 ; j < n ; j++) - result = result && (v(j) == 0.0); - return result; + bool result = true; + size_t n = v.size(); + for( size_t j = 0 ; j < n ; j++) + result = result && (v(j) == 0.0); + return result; } /* ************************************************************************* */ Vector repeat(size_t n, double value) { - return Vector::Constant(n, value); + return Vector::Constant(n, value); } /* ************************************************************************* */ Vector delta(size_t n, size_t i, double value) { - return Vector::Unit(n, i) * value; + return Vector::Unit(n, i) * value; } /* ************************************************************************* */ void print(const Vector& v, const string& s, ostream& stream) { - size_t n = v.size(); - odprintf_("%s [", stream, s.c_str()); - for(size_t i=0; i= vec2[i])) - return false; - return true; + size_t m = vec1.size(); + for(size_t i=0; i= vec2[i])) + return false; + return true; } /* ************************************************************************* */ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - size_t m = vec1.size(); - for(size_t i=0; i tol) - return false; - } - return true; + if (vec1.size()!=vec2.size()) return false; + size_t m = vec1.size(); + for(size_t i=0; i tol) + return false; + } + return true; } /* ************************************************************************* */ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - size_t m = vec1.size(); - for(size_t i=0; i tol) - return false; - } - return true; + if (vec1.size()!=vec2.size()) return false; + size_t m = vec1.size(); + for(size_t i=0; i tol) + return false; + } + return true; } /* ************************************************************************* */ bool assert_equal(const Vector& expected, const Vector& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; - cout << "not equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_inequal(const Vector& expected, const Vector& actual, double tol) { - if (!equal_with_abs_tol(expected,actual,tol)) return true; - cout << "Erroneously equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (!equal_with_abs_tol(expected,actual,tol)) return true; + cout << "Erroneously equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; - cout << "not equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, double tol) { - if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; - cout << "not equal:" << endl; - print(Vector(expected), "expected"); - print(Vector(actual), "actual"); - return false; + if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; + cout << "not equal:" << endl; + print(Vector(expected), "expected"); + print(Vector(actual), "actual"); + return false; } /* ************************************************************************* */ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - bool flag = false; double scale = 1.0; - size_t m = vec1.size(); - for(size_t i=0; itol&&fabs(vec2[i])tol)) - return false; - if(vec1[i] == 0 && vec2[i] == 0) continue; - if (!flag) { - scale = vec1[i] / vec2[i]; - flag = true ; - } - else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; - } - return flag; + if (vec1.size()!=vec2.size()) return false; + bool flag = false; double scale = 1.0; + size_t m = vec1.size(); + for(size_t i=0; itol&&fabs(vec2[i])tol)) + return false; + if(vec1[i] == 0 && vec2[i] == 0) continue; + if (!flag) { + scale = vec1[i] / vec2[i]; + flag = true ; + } + else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; + } + return flag; } /* ************************************************************************* */ ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { - return v.segment(i1,i2-i1); + return v.segment(i1,i2-i1); } /* ************************************************************************* */ void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { - fullVector.segment(i, subVector.size()) = subVector; + fullVector.segment(i, subVector.size()) = subVector; } /* ************************************************************************* */ Vector emul(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseProduct(b); + assert (b.size()==a.size()); + return a.cwiseProduct(b); } /* ************************************************************************* */ Vector ediv(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseQuotient(b); + assert (b.size()==a.size()); + return a.cwiseQuotient(b); } /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { - size_t n = a.size(); - assert (b.size()==a.size()); - Vector c(n); - for( size_t i = 0; i < n; i++ ) { - const double &ai = a(i), &bi = b(i); - c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi; - } - return c; + size_t n = a.size(); + assert (b.size()==a.size()); + Vector c(n); + for( size_t i = 0; i < n; i++ ) { + const double &ai = a(i), &bi = b(i); + c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi; + } + return c; } /* ************************************************************************* */ double sum(const Vector &a) { - return a.sum(); + return a.sum(); } /* ************************************************************************* */ double norm_2(const Vector& v) { - return v.norm(); + return v.norm(); } /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + size_t n = a.size(); + Vector b(n); + for( size_t i = 0; i < n; i++ ) + b(i) = 1.0/a(i); + return b; } /* ************************************************************************* */ Vector esqrt(const Vector& v) { - return v.cwiseSqrt(); + return v.cwiseSqrt(); } /* ************************************************************************* */ Vector abs(const Vector& v) { - return v.cwiseAbs(); + return v.cwiseAbs(); } /* ************************************************************************* */ double max(const Vector &a) { - return a.maxCoeff(); + return a.maxCoeff(); } /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { - const double x0 = v(0); - const double x02 = x0*x0; + const double x0 = v(0); + const double x02 = x0*x0; - // old code - GSL verison was actually a bit slower - const double sigma = v.squaredNorm() - x02; + // old code - GSL verison was actually a bit slower + const double sigma = v.squaredNorm() - x02; - v(0) = 1.0; + v(0) = 1.0; - if( sigma == 0.0 ) - return 0.0; - else { - double mu = sqrt(x02 + sigma); - if( x0 <= 0.0 ) - v(0) = x0 - mu; - else - v(0) = -sigma / (x0 + mu); + if( sigma == 0.0 ) + return 0.0; + else { + double mu = sqrt(x02 + sigma); + if( x0 <= 0.0 ) + v(0) = x0 - mu; + else + v(0) = -sigma / (x0 + mu); - const double v02 = v(0)*v(0); - v = v / v(0); - return 2.0 * v02 / (sigma + v02); - } + const double v02 = v(0)*v(0); + v = v / v(0); + return 2.0 * v02 / (sigma + v02); + } } /* ************************************************************************* */ pair house(const Vector &x) { - Vector v(x); - double beta = houseInPlace(v); - return make_pair(beta, v); + Vector v(x); + double beta = houseInPlace(v); + return make_pair(beta, v); } /* ************************************************************************* */ // Fast version *no error checking* ! // Pass in initialized vector of size m or will crash ! double weightedPseudoinverse(const Vector& a, const Vector& weights, - Vector& pseudo) { + Vector& pseudo) { - size_t m = weights.size(); - static const double inf = std::numeric_limits::infinity(); + size_t m = weights.size(); + static const double inf = std::numeric_limits::infinity(); - // Check once for zero entries of a. TODO: really needed ? - vector isZero; - for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); + // Check once for zero entries of a. TODO: really needed ? + vector isZero; + for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); - // If there is a valid (a!=0) constraint (sigma==0) return the first one - for (size_t i = 0; i < m; ++i) - if (weights[i] == inf && !isZero[i]) { - pseudo = delta(m, i, 1 / a[i]); - return inf; - } + // If there is a valid (a!=0) constraint (sigma==0) return the first one + for (size_t i = 0; i < m; ++i) + if (weights[i] == inf && !isZero[i]) { + pseudo = delta(m, i, 1 / a[i]); + return inf; + } - // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) - // For diagonal Sigma, inv(Sigma) = diag(precisions) - double precision = 0; - for (size_t i = 0; i < m; i++) { - double ai = a[i]; - if (!isZero[i]) // also catches remaining sigma==0 rows - precision += weights[i] * (ai * ai); - } + // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) + // For diagonal Sigma, inv(Sigma) = diag(precisions) + double precision = 0; + for (size_t i = 0; i < m; i++) { + double ai = a[i]; + if (!isZero[i]) // also catches remaining sigma==0 rows + precision += weights[i] * (ai * ai); + } - // precision = a'inv(Sigma)a - if (precision < 1e-9) - for (size_t i = 0; i < m; i++) - pseudo[i] = 0; - else { - // emul(precisions,a)/precision - double variance = 1.0 / precision; - for (size_t i = 0; i < m; i++) - pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i]; - } - return precision; // sum of weights + // precision = a'inv(Sigma)a + if (precision < 1e-9) + for (size_t i = 0; i < m; i++) + pseudo[i] = 0; + else { + // emul(precisions,a)/precision + double variance = 1.0 / precision; + for (size_t i = 0; i < m; i++) + pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i]; + } + return precision; // sum of weights } /* ************************************************************************* */ // Slow version with error checking pair weightedPseudoinverse(const Vector& a, const Vector& weights) { - int m = weights.size(); - if (a.size() != m) - throw invalid_argument("a and weights have different sizes!"); - Vector pseudo(m); - double precision = weightedPseudoinverse(a, weights, pseudo); - return make_pair(pseudo, precision); + int m = weights.size(); + if (a.size() != m) + throw invalid_argument("a and weights have different sizes!"); + Vector pseudo(m); + double precision = weightedPseudoinverse(a, weights, pseudo); + return make_pair(pseudo, precision); } /* ************************************************************************* */ Vector concatVectors(const std::list& vs) { - size_t dim = 0; - BOOST_FOREACH(Vector v, vs) - dim += v.size(); + size_t dim = 0; + BOOST_FOREACH(Vector v, vs) + dim += v.size(); - Vector A(dim); - size_t index = 0; - BOOST_FOREACH(Vector v, vs) { - for(int d = 0; d < v.size(); d++) - A(d+index) = v(d); - index += v.size(); - } + Vector A(dim); + size_t index = 0; + BOOST_FOREACH(Vector v, vs) { + for(int d = 0; d < v.size(); d++) + A(d+index) = v(d); + index += v.size(); + } - return A; + return A; } /* ************************************************************************* */ Vector concatVectors(size_t nrVectors, ...) { - va_list ap; - list vs; - va_start(ap, nrVectors); - for( size_t i = 0 ; i < nrVectors ; i++) { - Vector* V = va_arg(ap, Vector*); - vs.push_back(*V); - } - va_end(ap); - return concatVectors(vs); + va_list ap; + list vs; + va_start(ap, nrVectors); + for( size_t i = 0 ; i < nrVectors ; i++) { + Vector* V = va_arg(ap, Vector*); + vs.push_back(*V); + } + va_end(ap); + return concatVectors(vs); } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 0d2d11f98..a52b90f08 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -98,7 +98,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);} * @param n size */ inline Vector ones(size_t n) { return Vector::Ones(n); } - + /** * check if all zero */ @@ -277,15 +277,15 @@ double max(const Vector &a); */ template inline double dot(const V1 &a, const V2& b) { - assert (b.size()==a.size()); - return a.dot(b); + assert (b.size()==a.size()); + return a.dot(b); } /** compatibility version for ublas' inner_prod() */ template inline double inner_prod(const V1 &a, const V2& b) { - assert (b.size()==a.size()); - return a.dot(b); + assert (b.size()==a.size()); + return a.dot(b); } /** @@ -300,12 +300,12 @@ inline void scal(double alpha, Vector& x) { x *= alpha; } */ template inline void axpy(double alpha, const V1& x, V2& y) { - assert (y.size()==x.size()); - y += alpha * x; + assert (y.size()==x.size()); + y += alpha * x; } inline void axpy(double alpha, const Vector& x, SubVector y) { - assert (y.size()==x.size()); - y += alpha * x; + assert (y.size()==x.size()); + y += alpha * x; } /** @@ -360,17 +360,17 @@ namespace serialization { template void save(Archive & ar, const gtsam::Vector & v, unsigned int version) { - const size_t n = v.size(); - std::vector raw_data(n); - copy(v.data(), v.data()+n, raw_data.begin()); - ar << make_nvp("data", raw_data); + const size_t n = v.size(); + std::vector raw_data(n); + copy(v.data(), v.data()+n, raw_data.begin()); + ar << make_nvp("data", raw_data); } template void load(Archive & ar, gtsam::Vector & v, unsigned int version) { - std::vector raw_data; - ar >> make_nvp("data", raw_data); - v = gtsam::Vector_(raw_data); + std::vector raw_data; + ar >> make_nvp("data", raw_data); + v = gtsam::Vector_(raw_data); } } // namespace serialization diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h index c0bc95a50..a23f71e43 100644 --- a/gtsam/base/blockMatrices.h +++ b/gtsam/base/blockMatrices.h @@ -307,11 +307,11 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(rowStart_); - ar & BOOST_SERIALIZATION_NVP(rowEnd_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); } }; @@ -377,9 +377,9 @@ public: blockStart_ = 0; fillOffsets(firstBlockDim, lastBlockDim); if (preserve) - matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); else - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); } /** Row size @@ -415,9 +415,9 @@ public: assert(i_actualEndBlock < variableColOffsets_.size()); assert(j_actualEndBlock < variableColOffsets_.size()); return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); } constBlock range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) const { @@ -431,9 +431,9 @@ public: assert(i_actualEndBlock < variableColOffsets_.size()); assert(j_actualEndBlock < variableColOffsets_.size()); return ((const FullMatrix&)matrix_).block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); } Block full() { @@ -448,19 +448,19 @@ public: const FullMatrix& fullMatrix() const { return matrix_; } Column column(size_t i_block, size_t j_block, size_t columnOffset) { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); + assertInvariants(); + size_t i_actualBlock = i_block + blockStart_; + size_t j_actualBlock = j_block + blockStart_; + checkBlock(i_actualBlock); + checkBlock(j_actualBlock); + assert(i_actualBlock < variableColOffsets_.size()); + assert(j_actualBlock < variableColOffsets_.size()); + assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - return matrix_.col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); + return matrix_.col( + variableColOffsets_[j_actualBlock] + columnOffset).segment( + variableColOffsets_[i_actualBlock], + variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); } constColumn column(size_t i_block, size_t j_block, size_t columnOffset) const { @@ -496,9 +496,9 @@ public: assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); return matrix_.col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); + variableColOffsets_[j_actualStartBlock] + columnOffset).segment( + variableColOffsets_[i_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); } constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const { @@ -513,9 +513,9 @@ public: assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); + variableColOffsets_[j_actualStartBlock] + columnOffset).segment( + variableColOffsets_[i_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); } size_t offset(size_t block) const { @@ -615,9 +615,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); } }; diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index e941082ae..f73b6de78 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -33,22 +33,22 @@ namespace gtsam { static const double negativePivotThreshold = -1e-1; static const double zeroPivotThreshold = 1e-6; static const double underconstrainedPrior = 1e-5; - static const int underconstrainedExponentDifference = 12; + static const int underconstrainedExponentDifference = 12; /* ************************************************************************* */ static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { - const bool debug = ISDEBUG("choleskyCareful"); + const bool debug = ISDEBUG("choleskyCareful"); // Get pivot value double alpha = ATA(k,k); // Correct negative pivots from round-off error if(alpha < negativePivotThreshold) { - if(debug) { - cout << "pivot = " << alpha << endl; - print(ATA, "Partially-factorized matrix: "); - } + if(debug) { + cout << "pivot = " << alpha << endl; + print(ATA, "Partially-factorized matrix: "); + } return -1; } else if(alpha < 0.0) alpha = 0.0; @@ -63,23 +63,23 @@ static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { if(k < (order-1)) { // Update A(k,k+1:end) <- A(k,k+1:end) / beta - typedef Matrix::RowXpr::SegmentReturnType BlockRow; - BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); - V *= betainv; + typedef Matrix::RowXpr::SegmentReturnType BlockRow; + BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); + V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha - ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; -// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() -// .rankUpdate(V.adjoint(), -1); + ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; +// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() +// .rankUpdate(V.adjoint(), -1); } - return 1; + return 1; } else { // For zero pivots, add the underconstrained variable prior ATA(k,k) = underconstrainedPrior; for(size_t j=k+1; j choleskyCareful(Matrix& ATA, int order) { // The index of the row after the last non-zero row of the square-root factor size_t maxrank = 0; - bool success = true; + bool success = true; // Factor row-by-row for(size_t k = 0; k < size_t(order); ++k) { int stepResult = choleskyStep(ATA, k, size_t(order)); - if(stepResult == 1) { + if(stepResult == 1) { if(debug) cout << "choleskyCareful: Factored through " << k << endl; if(debug) print(ATA, "ATA: "); maxrank = k+1; } else if(stepResult == -1) { success = false; - break; + break; } /* else if(stepResult == 0) Found zero pivot */ } @@ -132,7 +132,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { // Compute Cholesky factorization of A, overwrites A. tic(1, "lld"); - Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); + Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); ABC.block(0,0,nFrontal,nFrontal).triangularView() = llt.matrixU(); toc(1, "lld"); @@ -156,26 +156,26 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; toc(3, "compute L"); - // Check last diagonal element - Eigen does not check it - bool ok; - if(llt.info() == Eigen::Success) { - if(nFrontal >= 2) { - int exp2, exp1; - (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); - (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); - ok = (exp2 - exp1 < underconstrainedExponentDifference); - } else if(nFrontal == 1) { - int exp1; - (void)frexp(ABC(0,0), &exp1); - ok = (exp1 > -underconstrainedExponentDifference); - } else { - ok = true; - } - } else { - ok = false; - } + // Check last diagonal element - Eigen does not check it + bool ok; + if(llt.info() == Eigen::Success) { + if(nFrontal >= 2) { + int exp2, exp1; + (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); + (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); + ok = (exp2 - exp1 < underconstrainedExponentDifference); + } else if(nFrontal == 1) { + int exp1; + (void)frexp(ABC(0,0), &exp1); + ok = (exp1 > -underconstrainedExponentDifference); + } else { + ok = true; + } + } else { + ok = false; + } - return ok; + return ok; } } diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index b1992f741..302ac32ea 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -29,23 +29,23 @@ namespace gtsam { namespace testing { - /** binary functions */ - template - T between(const T& t1, const T& t2) { return t1.between(t2); } + /** binary functions */ + template + T between(const T& t1, const T& t2) { return t1.between(t2); } - template - T compose(const T& t1, const T& t2) { return t1.compose(t2); } + template + T compose(const T& t1, const T& t2) { return t1.compose(t2); } - /** unary functions */ - template - T inverse(const T& t) { return t.inverse(); } + /** unary functions */ + template + T inverse(const T& t) { return t.inverse(); } - /** rotation functions */ - template - P rotate(const T& r, const P& pt) { return r.rotate(pt); } + /** rotation functions */ + template + P rotate(const T& r, const P& pt) { return r.rotate(pt); } - template - P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } + template + P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } } // \namespace testing } // \namespace gtsam diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 86abf6067..6b6a377de 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -32,431 +32,431 @@ namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) - * - * For additional details, see the documentation: - * http://www.boost.org/doc/libs/release/libs/bind/bind.html - */ + /* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * Use boost bind as follows to create a function pointer that uses the member function: + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * + * For additional details, see the documentation: + * http://www.boost.org/doc/libs/release/libs/bind/bind.html + */ - /** global functions for converting to a LieVector for use with numericalDerivative */ + /** global functions for converting to a LieVector for use with numericalDerivative */ inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j + Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { + double factor = 1.0/(2.0*delta); + const size_t n = x.dim(); + Vector d = zero(n), g = zero(n); + for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } + template + Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { + return numericalGradient(boost::bind(h, _1), x, delta); + } - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + Y hx = h(x); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } + /** use a raw C++ function pointer */ + template + Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); + } - /** remapping for double valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } + /** remapping for double valued functions */ + template + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); + } - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } + template + Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); + } - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } + /** remapping for vector valued functions */ + template + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); + } - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } + template + Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); + } - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + Y hx = h(x1,x2); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x1.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } + /** use a raw C++ function pointer */ + template + inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + Matrix numericalDerivative21(double (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative22 - (boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative22 + (boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + Y hx = h(x1,x2); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x2.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } + /** use a raw C++ function pointer */ + template + inline Matrix numericalDerivative22 + (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** - * Compute numerical derivative in argument 1 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative31 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 1 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative31 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x1.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative31 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** - * Compute numerical derivative in argument 2 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative32 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative32 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x2.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative32 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** - * Compute numerical derivative in argument 3 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative33 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative33 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative33 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x3.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative33 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative33(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative33(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar @@ -466,17 +466,17 @@ namespace gtsam { * @param delta The numerical derivative step size * @return n*n Hessian matrix computed via central differencing */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } + template + inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::function(boost::bind( + static_cast,const X&, double)>(&numericalGradient), + f, _1, delta)), x, delta); + } - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(f), x, delta); - } + template + inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { + return numericalHessian(boost::function(f), x, delta); + } /** Helper class that computes the derivative of f w.r.t. x1, centered about diff --git a/gtsam/base/tests/testBlockMatrices.cpp b/gtsam/base/tests/testBlockMatrices.cpp index 06b3f3002..3950d561a 100644 --- a/gtsam/base/tests/testBlockMatrices.cpp +++ b/gtsam/base/tests/testBlockMatrices.cpp @@ -23,15 +23,15 @@ using namespace gtsam; /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor1) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; + typedef Matrix AbMatrix; + typedef VerticalBlockView BlockAb; - AbMatrix matrix; // actual matrix - empty to start with + AbMatrix matrix; // actual matrix - empty to start with // from JacobianFactor::Constructor - one variable Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); + 1., 2., 3., + 4., 5., 6.); Vector b = Vector_(2, 7., 8.); size_t dims[] = { A1.cols(), 1}; @@ -50,26 +50,26 @@ TEST(testBlockMatrices, jacobian_factor1) { // examine matrix contents EXPECT_LONGS_EQUAL(2, Ab.nBlocks()); Matrix expFull = Matrix_(2, 4, - 1., 2., 3., 7., - 4., 5., 6., 8.); + 1., 2., 3., 7., + 4., 5., 6., 8.); Matrix actFull = Ab.full(); EXPECT(assert_equal(expFull, actFull)); } /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor2) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; + typedef Matrix AbMatrix; + typedef VerticalBlockView BlockAb; - AbMatrix matrix; // actual matrix - empty to start with + AbMatrix matrix; // actual matrix - empty to start with // from JacobianFactor::Constructor - two variables Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); + 1., 2., 3., + 4., 5., 6.); Matrix A2 = Matrix_(2,1, - 10., - 11.); + 10., + 11.); Vector b = Vector_(2, 7., 8.); size_t dims[] = { A1.cols(), A2.cols(), 1}; @@ -91,8 +91,8 @@ TEST(testBlockMatrices, jacobian_factor2) { // examine matrix contents EXPECT_LONGS_EQUAL(3, Ab.nBlocks()); Matrix expFull = Matrix_(2, 5, - 1., 2., 3., 10., 7., - 4., 5., 6., 11., 8.); + 1., 2., 3., 10., 7., + 4., 5., 6., 11., 8.); Matrix actFull = Ab.full(); EXPECT(assert_equal(expFull, actFull)); } @@ -102,29 +102,29 @@ TEST(testBlockMatrices, hessian_factor1) { typedef Matrix InfoMatrix; typedef SymmetricBlockView BlockInfo; - Matrix expected_full = Matrix_(3, 3, - 3.0, 5.0, -8.0, - 0.0, 6.0, -9.0, - 0.0, 0.0, 10.0); + Matrix expected_full = Matrix_(3, 3, + 3.0, 5.0, -8.0, + 0.0, 6.0, -9.0, + 0.0, 0.0, 10.0); Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); Vector g = Vector_(2, -8.0, -9.0); double f = 10.0; - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); - BlockInfo infoView(fullMatrix, dims, dims+2); - infoView(0,0) = G; - infoView.column(0,1,0) = g; - infoView(1,1)(0,0) = f; + size_t dims[] = { G.rows(), 1 }; + InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); + BlockInfo infoView(fullMatrix, dims, dims+2); + infoView(0,0) = G; + infoView.column(0,1,0) = g; + infoView(1,1)(0,0) = f; - EXPECT_LONGS_EQUAL(0, infoView.blockStart()); - EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); - EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); - EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) - EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); + EXPECT_LONGS_EQUAL(0, infoView.blockStart()); + EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); + EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); + EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) + EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); - EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); + EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); EXPECT(assert_equal(g, Vector(((const BlockInfo)infoView).rangeColumn(0, 1, 1, 0)))); } diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 8e8da7a70..93e8ce2a2 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -86,42 +86,42 @@ TEST(cholesky, BadScalingSVD) { /* ************************************************************************* */ TEST(cholesky, underconstrained) { - Matrix L(6,6); L << - 1, 0, 0, 0, 0, 0, - 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, - 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, - 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, - 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, - 0.1197220685104, 1, 0, 0, 0, 0; - Matrix D1(6,6); D1 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 1.34342584865901, 0, - 0, 0, 0, 0, 0, 1e-12; - Matrix D2(6,6); D2 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0; - Matrix D3(6,6); D3 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, -0.5, 0, - 0, 0, 0, 0, 0, -0.6; + Matrix L(6,6); L << + 1, 0, 0, 0, 0, 0, + 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, + 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, + 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, + 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, + 0.1197220685104, 1, 0, 0, 0, 0; + Matrix D1(6,6); D1 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 1.34342584865901, 0, + 0, 0, 0, 0, 0, 1e-12; + Matrix D2(6,6); D2 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0; + Matrix D3(6,6); D3 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, -0.5, 0, + 0, 0, 0, 0, 0, -0.6; - Matrix A1 = L * D1 * L.transpose(); - Matrix A2 = L * D2 * L.transpose(); - Matrix A3 = L * D3 * L.transpose(); + Matrix A1 = L * D1 * L.transpose(); + Matrix A2 = L * D2 * L.transpose(); + Matrix A3 = L * D3 * L.transpose(); - LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6))); - LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6))); - LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index c0b72f1a0..cad2f6ded 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -31,139 +31,139 @@ using namespace gtsam; /* ************************************************************************* */ TEST(DSFVectorVector, findSet) { - DSFVector dsf(3); - CHECK(dsf.findSet(0) != dsf.findSet(2)); + DSFVector dsf(3); + CHECK(dsf.findSet(0) != dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVectorVector, makeUnionInPlace) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,2); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVectorVector, makeUnionInPlace2) { - boost::shared_ptr v = boost::make_shared(5); - std::vector keys; keys += 1, 3; - DSFVector dsf(v, keys); - dsf.makeUnionInPlace(1,3); - CHECK(dsf.findSet(1) == dsf.findSet(3)); + boost::shared_ptr v = boost::make_shared(5); + std::vector keys; keys += 1, 3; + DSFVector dsf(v, keys); + dsf.makeUnionInPlace(1,3); + CHECK(dsf.findSet(1) == dsf.findSet(3)); } /* ************************************************************************* */ TEST(DSFVector, makeUnion2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(2,0); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(2,0); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVector, makeUnion3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVector, sets) { - DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); + DSFVector dsf(2); + dsf.makeUnionInPlace(0,1); + map > sets = dsf.sets(); + LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays) { - DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); + DSFVector dsf(2); + dsf.makeUnionInPlace(0,1); + map > arrays = dsf.arrays(); + LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + map > sets = dsf.sets(); + LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1, 2; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1, 2; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - map > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + map > arrays = dsf.arrays(); + LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1, 2; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1, 2; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > sets = dsf.sets(); - LONGS_EQUAL(2, sets.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + map > sets = dsf.sets(); + LONGS_EQUAL(2, sets.size()); - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(2, arrays.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + map > arrays = dsf.arrays(); + LONGS_EQUAL(2, arrays.size()); - vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, set) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - set set = dsf.set(0); - LONGS_EQUAL(2, set.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + set set = dsf.set(0); + LONGS_EQUAL(2, set.size()); - std::set expected; expected += 0, 1; - CHECK(expected == set); + std::set expected; expected += 0, 1; + CHECK(expected == set); } /* ************************************************************************* */ TEST(DSFVector, set2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - set set = dsf.set(0); - LONGS_EQUAL(3, set.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + set set = dsf.set(0); + LONGS_EQUAL(3, set.size()); - std::set expected; expected += 0, 1, 2; - CHECK(expected == set); + std::set expected; expected += 0, 1, 2; + CHECK(expected == set); } /* ************************************************************************* */ TEST(DSFVector, isSingleton) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - CHECK(!dsf.isSingleton(0)); - CHECK(!dsf.isSingleton(1)); - CHECK( dsf.isSingleton(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + CHECK(!dsf.isSingleton(0)); + CHECK(!dsf.isSingleton(1)); + CHECK( dsf.isSingleton(2)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index b3e065921..464624bd4 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -21,12 +21,12 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; - init_vector += 2, 3, 4, 5; + FastVector init_vector; + init_vector += 2, 3, 4, 5; - FastSet actSet(init_vector); - FastSet expSet; expSet += 2, 3, 4, 5; - EXPECT(actSet == expSet); + FastSet actSet(init_vector); + FastSet expSet; expSet += 2, 3, 4, 5; + EXPECT(actSet == expSet); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index aad28563c..5ef867e85 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -27,24 +27,24 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix) /* ************************************************************************* */ TEST( LieMatrix, construction ) { - Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); - LieMatrix lie1(m), lie2(m); + Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); + LieMatrix lie1(m), lie2(m); - EXPECT(lie1.dim() == 4); - EXPECT(assert_equal(m, lie1.matrix())); - EXPECT(assert_equal(lie1, lie2)); + EXPECT(lie1.dim() == 4); + EXPECT(assert_equal(m, lie1.matrix())); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); - LieMatrix exp(init); - LieMatrix a(2,2,10.0,20.0,30.0,40.0); - double data[] = {10,30,20,40}; - LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, a)); - EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); + Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); + LieMatrix exp(init); + LieMatrix a(2,2,10.0,20.0,30.0,40.0); + double data[] = {10,30,20,40}; + LieMatrix b(2,2,data); + EXPECT(assert_equal(exp, a)); + EXPECT(assert_equal(exp, b)); + EXPECT(assert_equal(b, a)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 229737b8d..11157a701 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -29,20 +29,20 @@ const double tol=1e-9; /* ************************************************************************* */ TEST( testLieScalar, construction ) { - double d = 2.; - LieScalar lie1(d), lie2(d); + double d = 2.; + LieScalar lie1(d), lie2(d); - EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); - EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(lie1.dim() == 1); - EXPECT(assert_equal(lie1, lie2)); + EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); + EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); + EXPECT(lie1.dim() == 1); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( testLieScalar, localCoordinates ) { - LieScalar lie1(1.), lie2(3.); + LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); + EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index c4c88dc9b..74be6628f 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -27,26 +27,26 @@ GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { - Vector v = Vector_(3, 1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); + Vector v = Vector_(3, 1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); - EXPECT(lie1.dim() == 3); - EXPECT(assert_equal(v, lie1.vector())); - EXPECT(assert_equal(lie1, lie2)); + EXPECT(lie1.dim() == 3); + EXPECT(assert_equal(v, lie1.vector())); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( testLieVector, other_constructors ) { - Vector init = Vector_(2, 10.0, 20.0); - LieVector exp(init); - LieVector a(2,10.0,20.0); - double data[] = {10,20}; - LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); - EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); + Vector init = Vector_(2, 10.0, 20.0); + LieVector exp(init); + LieVector a(2,10.0,20.0); + double data[] = {10,20}; + LieVector b(2,data); + LieVector c(2.3), c_exp(LieVector(1, 2.3)); + EXPECT(assert_equal(exp, a)); + EXPECT(assert_equal(exp, b)); + EXPECT(assert_equal(b, a)); + EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 56be42c02..4f57a6b6b 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -32,234 +32,234 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); + double data[] = { -5, 3, 0, -5 }; + Matrix A = Matrix_(2, 2, data); - Matrix B(2, 2); - B(0, 0) = -5; - B(0, 1) = 3; - B(1, 0) = 0; - B(1, 1) = -5; + Matrix B(2, 2); + B(0, 0) = -5; + B(0, 1) = 3; + B(1, 0) = 0; + B(1, 1) = -5; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, constructor_vector ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); - Vector v(4); - copy(data, data + 4, v.data()); - Matrix B = Matrix_(2, 2, v); // this one is column order ! - EQUALITY(A,trans(B)); + double data[] = { -5, 3, 0, -5 }; + Matrix A = Matrix_(2, 2, data); + Vector v(4); + copy(data, data + 4, v.data()); + Matrix B = Matrix_(2, 2, v); // this one is column order ! + EQUALITY(A,trans(B)); } /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B(2, 2); - B(0, 0) = -5; - B(0, 1) = 3; - B(1, 0) = 0; - B(1, 1) = -5; + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B(2, 2); + B(0, 0) = -5; + B(0, 1) = 3; + B(1, 0) = 0; + B(1, 1) = -5; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - const double * const a = &A(0, 0); - EXPECT_DOUBLES_EQUAL(1, a[0], tol); - EXPECT_DOUBLES_EQUAL(3, a[1], tol); - EXPECT_DOUBLES_EQUAL(2, a[2], tol); - EXPECT_DOUBLES_EQUAL(4, a[3], tol); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + const double * const a = &A(0, 0); + EXPECT_DOUBLES_EQUAL(1, a[0], tol); + EXPECT_DOUBLES_EQUAL(3, a[1], tol); + EXPECT_DOUBLES_EQUAL(2, a[2], tol); + EXPECT_DOUBLES_EQUAL(4, a[3], tol); } /* ************************************************************************* */ TEST( matrix, collect1 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - Matrix AB = collect(2, &A, &B); - Matrix C(2, 5); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 3; j++) - C(i, j + 2) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix AB = collect(2, &A, &B); + Matrix C(2, 5); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 3; j++) + C(i, j + 2) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, collect2 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - vector matrices; - matrices.push_back(&A); - matrices.push_back(&B); - Matrix AB = collect(matrices); - Matrix C(2, 5); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 3; j++) - C(i, j + 2) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + vector matrices; + matrices.push_back(&A); + matrices.push_back(&B); + Matrix AB = collect(matrices); + Matrix C(2, 5); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 3; j++) + C(i, j + 2) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, collect3 ) { - Matrix A, B; - A = eye(2, 3); - B = eye(2, 3); - vector matrices; - matrices.push_back(&A); - matrices.push_back(&B); - Matrix AB = collect(matrices, 2, 3); - Matrix exp = Matrix_(2, 6, - 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); + Matrix A, B; + A = eye(2, 3); + B = eye(2, 3); + vector matrices; + matrices.push_back(&A); + matrices.push_back(&B); + Matrix AB = collect(matrices, 2, 3); + Matrix exp = Matrix_(2, 6, + 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); - EQUALITY(exp,AB); + EQUALITY(exp,AB); } /* ************************************************************************* */ TEST( matrix, stack ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - Matrix AB = stack(2, &A, &B); - Matrix C(5, 2); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 2; j++) - C(i + 2, j) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix AB = stack(2, &A, &B); + Matrix C(5, 2); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 2; j++) + C(i + 2, j) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, column ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., - 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., - -0.1); - Vector a1 = column(A, 0); - Vector exp1 = Vector_(4, -1., 0., 1., 0.); - EXPECT(assert_equal(a1, exp1)); + Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., + -0.1); + Vector a1 = column(A, 0); + Vector exp1 = Vector_(4, -1., 0., 1., 0.); + EXPECT(assert_equal(a1, exp1)); - Vector a2 = column(A, 3); - Vector exp2 = Vector_(4, 0., 1., 0., 0.); - EXPECT(assert_equal(a2, exp2)); + Vector a2 = column(A, 3); + Vector exp2 = Vector_(4, 0., 1., 0., 0.); + EXPECT(assert_equal(a2, exp2)); - Vector a3 = column(A, 6); - Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); - EXPECT(assert_equal(a3, exp3)); + Vector a3 = column(A, 6); + Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); + EXPECT(assert_equal(a3, exp3)); } /* ************************************************************************* */ TEST( matrix, insert_column ) { - Matrix big = zeros(5, 6); - Vector col = ones(5); - size_t j = 3; + Matrix big = zeros(5, 6); + Vector col = ones(5); + size_t j = 3; - insertColumn(big, col, j); + insertColumn(big, col, j); - Matrix expected = Matrix_(5, 6, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); + Matrix expected = Matrix_(5, 6, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ TEST( matrix, insert_subcolumn ) { - Matrix big = zeros(5, 6); - Vector col1 = ones(2); - size_t i = 1; - size_t j = 3; + Matrix big = zeros(5, 6); + Vector col1 = ones(2); + size_t i = 1; + size_t j = 3; - insertColumn(big, col1, i, j); // check 1 + insertColumn(big, col1, i, j); // check 1 - Vector col2 = ones(1); - insertColumn(big, col2, 4, 5); // check 2 + Vector col2 = ones(1); + insertColumn(big, col2, 4, 5); // check 2 - Matrix expected = Matrix_(5, 6, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + Matrix expected = Matrix_(5, 6, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ TEST( matrix, row ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., - 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., - -0.1); - Vector a1 = row(A, 0); - Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); - EXPECT(assert_equal(a1, exp1)); + Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., + -0.1); + Vector a1 = row(A, 0); + Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); + EXPECT(assert_equal(a1, exp1)); - Vector a2 = row(A, 2); - Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); - EXPECT(assert_equal(a2, exp2)); + Vector a2 = row(A, 2); + Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); + EXPECT(assert_equal(a2, exp2)); - Vector a3 = row(A, 3); - Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); - EXPECT(assert_equal(a3, exp3)); + Vector a3 = row(A, 3); + Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); + EXPECT(assert_equal(a3, exp3)); } /* ************************************************************************* */ TEST( matrix, zeros ) { - Matrix A(2, 3); - A(0, 0) = 0; - A(0, 1) = 0; - A(0, 2) = 0; - A(1, 0) = 0; - A(1, 1) = 0; - A(1, 2) = 0; + Matrix A(2, 3); + A(0, 0) = 0; + A(0, 1) = 0; + A(0, 2) = 0; + A(1, 0) = 0; + A(1, 1) = 0; + A(1, 2) = 0; - Matrix zero = zeros(2, 3); + Matrix zero = zeros(2, 3); - EQUALITY(A , zero); + EQUALITY(A , zero); } /* ************************************************************************* */ TEST( matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0); + Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0); - insertSub(big, small, 1, 2); + insertSub(big, small, 1, 2); - Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ @@ -285,537 +285,537 @@ TEST( matrix, stream_read ) { /* ************************************************************************* */ TEST( matrix, scale_columns ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(4, 2., 3., 4., 5.); + Vector v = Vector_(4, 2., 3., 4., 5.); - Matrix actual = vector_scale(A, v); + Matrix actual = vector_scale(A, v); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 3.; - expected(0, 2) = 4.; - expected(0, 3) = 5.; - expected(1, 0) = 2.; - expected(1, 1) = 3.; - expected(1, 2) = 4.; - expected(1, 3) = 5.; - expected(2, 0) = 2.; - expected(2, 1) = 3.; - expected(2, 2) = 4.; - expected(2, 3) = 5.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 3.; + expected(0, 2) = 4.; + expected(0, 3) = 5.; + expected(1, 0) = 2.; + expected(1, 1) = 3.; + expected(1, 2) = 4.; + expected(1, 3) = 5.; + expected(2, 0) = 2.; + expected(2, 1) = 3.; + expected(2, 2) = 4.; + expected(2, 3) = 5.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, scale_rows ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(3, 2., 3., 4.); + Vector v = Vector_(3, 2., 3., 4.); - Matrix actual = vector_scale(v, A); + Matrix actual = vector_scale(v, A); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 2.; - expected(0, 2) = 2.; - expected(0, 3) = 2.; - expected(1, 0) = 3.; - expected(1, 1) = 3.; - expected(1, 2) = 3.; - expected(1, 3) = 3.; - expected(2, 0) = 4.; - expected(2, 1) = 4.; - expected(2, 2) = 4.; - expected(2, 3) = 4.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 2.; + expected(0, 2) = 2.; + expected(0, 3) = 2.; + expected(1, 0) = 3.; + expected(1, 1) = 3.; + expected(1, 2) = 3.; + expected(1, 3) = 3.; + expected(2, 0) = 4.; + expected(2, 1) = 4.; + expected(2, 2) = 4.; + expected(2, 3) = 4.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, scale_rows_mask ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(3, 2., std::numeric_limits::infinity(), 4.); + Vector v = Vector_(3, 2., std::numeric_limits::infinity(), 4.); - Matrix actual = vector_scale(v, A, true); + Matrix actual = vector_scale(v, A, true); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 2.; - expected(0, 2) = 2.; - expected(0, 3) = 2.; - expected(1, 0) = 1.; - expected(1, 1) = 1.; - expected(1, 2) = 1.; - expected(1, 3) = 1.; - expected(2, 0) = 4.; - expected(2, 1) = 4.; - expected(2, 2) = 4.; - expected(2, 3) = 4.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 2.; + expected(0, 2) = 2.; + expected(0, 3) = 2.; + expected(1, 0) = 1.; + expected(1, 1) = 1.; + expected(1, 2) = 1.; + expected(1, 3) = 1.; + expected(2, 0) = 4.; + expected(2, 1) = 4.; + expected(2, 2) = 4.; + expected(2, 3) = 4.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, equal ) { - Matrix A(4, 4); - A(0, 0) = -1; - A(0, 1) = 1; - A(0, 2) = 2; - A(0, 3) = 3; - A(1, 0) = 1; - A(1, 1) = -3; - A(1, 2) = 1; - A(1, 3) = 3; - A(2, 0) = 1; - A(2, 1) = 2; - A(2, 2) = -1; - A(2, 3) = 4; - A(3, 0) = 2; - A(3, 1) = 1; - A(3, 2) = 2; - A(3, 3) = -2; + Matrix A(4, 4); + A(0, 0) = -1; + A(0, 1) = 1; + A(0, 2) = 2; + A(0, 3) = 3; + A(1, 0) = 1; + A(1, 1) = -3; + A(1, 2) = 1; + A(1, 3) = 3; + A(2, 0) = 1; + A(2, 1) = 2; + A(2, 2) = -1; + A(2, 3) = 4; + A(3, 0) = 2; + A(3, 1) = 1; + A(3, 2) = 2; + A(3, 3) = -2; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3, 3) = -2.1; + Matrix A3(A); + A3(3, 3) = -2.1; - EXPECT(A==A2); - EXPECT(A!=A3); + EXPECT(A==A2); + EXPECT(A!=A3); } /* ************************************************************************* */ TEST( matrix, equal_nan ) { - Matrix A(4, 4); - A(0, 0) = -1; - A(0, 1) = 1; - A(0, 2) = 2; - A(0, 3) = 3; - A(1, 0) = 1; - A(1, 1) = -3; - A(1, 2) = 1; - A(1, 3) = 3; - A(2, 0) = 1; - A(2, 1) = 2; - A(2, 2) = -1; - A(2, 3) = 4; - A(3, 0) = 2; - A(3, 1) = 1; - A(3, 2) = 2; - A(3, 3) = -2; + Matrix A(4, 4); + A(0, 0) = -1; + A(0, 1) = 1; + A(0, 2) = 2; + A(0, 3) = 3; + A(1, 0) = 1; + A(1, 1) = -3; + A(1, 2) = 1; + A(1, 3) = 3; + A(2, 0) = 1; + A(2, 1) = 2; + A(2, 2) = -1; + A(2, 3) = 4; + A(3, 0) = 2; + A(3, 1) = 1; + A(3, 2) = 2; + A(3, 3) = -2; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3, 3) = inf; + Matrix A3(A); + A3(3, 3) = inf; - EXPECT(A!=A3); + EXPECT(A!=A3); } /* ************************************************************************* */ TEST( matrix, addition ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); - EQUALITY(A+B,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + EQUALITY(A+B,C); } /* ************************************************************************* */ TEST( matrix, addition_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); - A += B; - EQUALITY(A,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + A += B; + EQUALITY(A,C); } /* ************************************************************************* */ TEST( matrix, subtraction ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); - EQUALITY(A-B,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + EQUALITY(A-B,C); } /* ************************************************************************* */ TEST( matrix, subtraction_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); - A -= B; - EQUALITY(A,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + A -= B; + EQUALITY(A,C); } /* ************************************************************************* */ TEST( matrix, multiplication ) { - Matrix A(2, 2); - A(0, 0) = -1; - A(1, 0) = 1; - A(0, 1) = 1; - A(1, 1) = -3; + Matrix A(2, 2); + A(0, 0) = -1; + A(1, 0) = 1; + A(0, 1) = 1; + A(1, 1) = -3; - Matrix B(2, 1); - B(0, 0) = 1.2; - B(1, 0) = 3.4; + Matrix B(2, 1); + B(0, 0) = 1.2; + B(1, 0) = 3.4; - Matrix AB(2, 1); - AB(0, 0) = 2.2; - AB(1, 0) = -9.; + Matrix AB(2, 1); + AB(0, 0) = 2.2; + AB(1, 0) = -9.; - EQUALITY(A*B,AB); + EQUALITY(A*B,AB); } /* ************************************************************************* */ TEST( matrix, scalar_matrix_multiplication ) { - Vector result(2); + Vector result(2); - Matrix A(2, 2); - A(0, 0) = -1; - A(1, 0) = 1; - A(0, 1) = 1; - A(1, 1) = -3; + Matrix A(2, 2); + A(0, 0) = -1; + A(1, 0) = 1; + A(0, 1) = 1; + A(1, 1) = -3; - Matrix B(2, 2); - B(0, 0) = -10; - B(1, 0) = 10; - B(0, 1) = 10; - B(1, 1) = -30; + Matrix B(2, 2); + B(0, 0) = -10; + B(1, 0) = 10; + B(0, 1) = 10; + B(1, 1) = -30; - EQUALITY((10*A),B); + EQUALITY((10*A),B); } /* ************************************************************************* */ TEST( matrix, matrix_vector_multiplication ) { - Vector result(2); + Vector result(2); - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Vector v = Vector_(3, 1., 2., 3.); - Vector Av = Vector_(2, 14., 32.); - Vector AtAv = Vector_(3, 142., 188., 234.); + Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Vector v = Vector_(3, 1., 2., 3.); + Vector Av = Vector_(2, 14., 32.); + Vector AtAv = Vector_(3, 142., 188., 234.); - EQUALITY(A*v,Av); - EQUALITY(A^Av,AtAv); + EQUALITY(A*v,Av); + EQUALITY(A^Av,AtAv); } /* ************************************************************************* */ TEST( matrix, nrRowsAndnrCols ) { - Matrix A(3, 6); - LONGS_EQUAL( A.rows() , 3 ); - LONGS_EQUAL( A.cols() , 6 ); + Matrix A(3, 6); + LONGS_EQUAL( A.rows() , 3 ); + LONGS_EQUAL( A.cols() , 6 ); } /* ************************************************************************* */ TEST( matrix, scalar_divide ) { - Matrix A(2, 2); - A(0, 0) = 10; - A(1, 0) = 30; - A(0, 1) = 20; - A(1, 1) = 40; + Matrix A(2, 2); + A(0, 0) = 10; + A(1, 0) = 30; + A(0, 1) = 20; + A(1, 1) = 40; - Matrix B(2, 2); - B(0, 0) = 1; - B(1, 0) = 3; - B(0, 1) = 2; - B(1, 1) = 4; + Matrix B(2, 2); + B(0, 0) = 1; + B(1, 0) = 3; + B(0, 1) = 2; + B(1, 1) = 4; - EQUALITY(B,A/10); + EQUALITY(B,A/10); } /* ************************************************************************* */ TEST( matrix, zero_below_diagonal ) { - Matrix A1 = Matrix_(3, 4, - 1.0, 2.0, 3.0, 4.0, - 1.0, 2.0, 3.0, 4.0, - 1.0, 2.0, 3.0, 4.0); + Matrix A1 = Matrix_(3, 4, + 1.0, 2.0, 3.0, 4.0, + 1.0, 2.0, 3.0, 4.0, + 1.0, 2.0, 3.0, 4.0); - Matrix expected1 = Matrix_(3, 4, - 1.0, 2.0, 3.0, 4.0, - 0.0, 2.0, 3.0, 4.0, - 0.0, 0.0, 3.0, 4.0); - Matrix actual1r = A1; - zeroBelowDiagonal(actual1r); - EXPECT(assert_equal(expected1, actual1r, 1e-10)); + Matrix expected1 = Matrix_(3, 4, + 1.0, 2.0, 3.0, 4.0, + 0.0, 2.0, 3.0, 4.0, + 0.0, 0.0, 3.0, 4.0); + Matrix actual1r = A1; + zeroBelowDiagonal(actual1r); + EXPECT(assert_equal(expected1, actual1r, 1e-10)); - Matrix actual1c = A1; - zeroBelowDiagonal(actual1c); - EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); + Matrix actual1c = A1; + zeroBelowDiagonal(actual1c); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - actual1c = A1; - zeroBelowDiagonal(actual1c, 4); - EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); + actual1c = A1; + zeroBelowDiagonal(actual1c, 4); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - Matrix A2 = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0); - Matrix expected2 = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 0.0, 3.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); + Matrix A2 = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0); + Matrix expected2 = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 0.0, 3.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0); - Matrix actual2r = A2; - zeroBelowDiagonal(actual2r); - EXPECT(assert_equal(expected2, actual2r, 1e-10)); + Matrix actual2r = A2; + zeroBelowDiagonal(actual2r); + EXPECT(assert_equal(expected2, actual2r, 1e-10)); - Matrix actual2c = A2; - zeroBelowDiagonal(actual2c); - EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); + Matrix actual2c = A2; + zeroBelowDiagonal(actual2c); + EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); - Matrix expected2_partial = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0); - actual2c = A2; - zeroBelowDiagonal(actual2c, 1); - EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); + Matrix expected2_partial = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0); + actual2c = A2; + zeroBelowDiagonal(actual2c, 1); + EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); } /* ************************************************************************* */ TEST( matrix, inverse ) { - Matrix A(3, 3); - A(0, 0) = 1; - A(0, 1) = 2; - A(0, 2) = 3; - A(1, 0) = 0; - A(1, 1) = 4; - A(1, 2) = 5; - A(2, 0) = 1; - A(2, 1) = 0; - A(2, 2) = 6; + Matrix A(3, 3); + A(0, 0) = 1; + A(0, 1) = 2; + A(0, 2) = 3; + A(1, 0) = 0; + A(1, 1) = 4; + A(1, 2) = 5; + A(2, 0) = 1; + A(2, 1) = 0; + A(2, 2) = 6; - Matrix Ainv = inverse(A); - EXPECT(assert_equal(eye(3), A*Ainv)); - EXPECT(assert_equal(eye(3), Ainv*A)); + Matrix Ainv = inverse(A); + EXPECT(assert_equal(eye(3), A*Ainv)); + EXPECT(assert_equal(eye(3), Ainv*A)); - Matrix expected(3, 3); - expected(0, 0) = 1.0909; - expected(0, 1) = -0.5454; - expected(0, 2) = -0.0909; - expected(1, 0) = 0.2272; - expected(1, 1) = 0.1363; - expected(1, 2) = -0.2272; - expected(2, 0) = -0.1818; - expected(2, 1) = 0.0909; - expected(2, 2) = 0.1818; + Matrix expected(3, 3); + expected(0, 0) = 1.0909; + expected(0, 1) = -0.5454; + expected(0, 2) = -0.0909; + expected(1, 0) = 0.2272; + expected(1, 1) = 0.1363; + expected(1, 2) = -0.2272; + expected(2, 0) = -0.1818; + expected(2, 1) = 0.0909; + expected(2, 2) = 0.1818; - EXPECT(assert_equal(expected, Ainv, 1e-4)); + EXPECT(assert_equal(expected, Ainv, 1e-4)); - // These two matrices failed before version 2003 because we called LU incorrectly - Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, - 0.0, -1.0, 1.0, - 1.0, 0.0, 2.0, - 0.0, 0.0, 1.0), - inverse(lMg))); - Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, - 0.0, 1.0,-2.0, - -1.0, 0.0, 1.0, - 0.0, 0.0, 1.0), - inverse(gMl))); + // These two matrices failed before version 2003 because we called LU incorrectly + Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal(Matrix_(3,3, + 0.0, -1.0, 1.0, + 1.0, 0.0, 2.0, + 0.0, 0.0, 1.0), + inverse(lMg))); + Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0), + inverse(gMl))); } /* ************************************************************************* */ TEST( matrix, inverse2 ) { - Matrix A(3, 3); - A(0, 0) = 0; - A(0, 1) = -1; - A(0, 2) = 1; - A(1, 0) = 1; - A(1, 1) = 0; - A(1, 2) = 2; - A(2, 0) = 0; - A(2, 1) = 0; - A(2, 2) = 1; + Matrix A(3, 3); + A(0, 0) = 0; + A(0, 1) = -1; + A(0, 2) = 1; + A(1, 0) = 1; + A(1, 1) = 0; + A(1, 2) = 2; + A(2, 0) = 0; + A(2, 1) = 0; + A(2, 2) = 1; - Matrix Ainv = inverse(A); + Matrix Ainv = inverse(A); - Matrix expected(3, 3); - expected(0, 0) = 0; - expected(0, 1) = 1; - expected(0, 2) = -2; - expected(1, 0) = -1; - expected(1, 1) = 0; - expected(1, 2) = 1; - expected(2, 0) = 0; - expected(2, 1) = 0; - expected(2, 2) = 1; + Matrix expected(3, 3); + expected(0, 0) = 0; + expected(0, 1) = 1; + expected(0, 2) = -2; + expected(1, 0) = -1; + expected(1, 1) = 0; + expected(1, 2) = 1; + expected(2, 0) = 0; + expected(2, 1) = 0; + expected(2, 2) = 1; - EXPECT(assert_equal(expected, Ainv, 1e-4)); + EXPECT(assert_equal(expected, Ainv, 1e-4)); } /* ************************************************************************* */ TEST( matrix, backsubtitution ) { - // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = Vector_(2, 3.6250, -0.75); - Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); - Vector b1 = U22 * expected1; - EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); + // TEST ONE 2x2 matrix U1*x=b1 + Vector expected1 = Vector_(2, 3.6250, -0.75); + Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); + Vector b1 = U22 * expected1; + EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); - // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = Vector_(3, 5.5, -8.5, 5.); - Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); - Vector b2 = U33 * expected2; - EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); + // TEST TWO 3x3 matrix U2*x=b2 + Vector expected2 = Vector_(3, 5.5, -8.5, 5.); + Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Vector b2 = U33 * expected2; + EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); - // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = Vector_(3, 1., 1., 1.); - Matrix L3 = trans(U33); - Vector b3 = L3 * expected3; - EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); + // TEST THREE Lower triangular 3x3 matrix L3*x=b3 + Vector expected3 = Vector_(3, 1., 1., 1.); + Matrix L3 = trans(U33); + Vector b3 = L3 * expected3; + EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); - // TEST FOUR Try the above with transpose backSubstituteUpper - EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001)); + // TEST FOUR Try the above with transpose backSubstituteUpper + EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001)); } /* ************************************************************************* */ TEST( matrix, householder ) { - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + double data[] = { -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, - -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1 = Matrix_(4, 7, data1); - Matrix A1 = Matrix_(4, 7, data); - householder_(A1, 3); - EXPECT(assert_equal(expected1, A1, 1e-3)); + // check in-place householder, with v vectors below diagonal + double data1[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1 = Matrix_(4, 7, data1); + Matrix A1 = Matrix_(4, 7, data); + householder_(A1, 3); + EXPECT(assert_equal(expected1, A1, 1e-3)); - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected = Matrix_(4, 7, data2); - Matrix A2 = Matrix_(4, 7, data); - householder(A2, 3); - EXPECT(assert_equal(expected, A2, 1e-3)); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected = Matrix_(4, 7, data2); + Matrix A2 = Matrix_(4, 7, data); + householder(A2, 3); + EXPECT(assert_equal(expected, A2, 1e-3)); } /* ************************************************************************* */ TEST( matrix, householder_colMajor ) { - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + double data[] = { + -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, - -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1(Matrix_(4, 7, data1)); - Matrix A1(Matrix_(4, 7, data)); - householder_(A1, 3); - EXPECT(assert_equal(expected1, A1, 1e-3)); + // check in-place householder, with v vectors below diagonal + double data1[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1(Matrix_(4, 7, data1)); + Matrix A1(Matrix_(4, 7, data)); + householder_(A1, 3); + EXPECT(assert_equal(expected1, A1, 1e-3)); - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A2(Matrix_(4, 7, data)); - householder(A2, 3); - EXPECT(assert_equal(expected, A2, 1e-3)); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected(Matrix_(4, 7, data2)); + Matrix A2(Matrix_(4, 7, data)); + householder(A2, 3); + EXPECT(assert_equal(expected, A2, 1e-3)); } /* ************************************************************************* */ TEST( matrix, eigen_QR ) { - // use standard Eigen function to yield a non-in-place QR factorization - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + // use standard Eigen function to yield a non-in-place QR factorization + double data[] = { + -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A(Matrix_(4, 7, data)); - Matrix actual = A.householderQr().matrixQR(); - zeroBelowDiagonal(actual); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected(Matrix_(4, 7, data2)); + Matrix A(Matrix_(4, 7, data)); + Matrix actual = A.householderQr().matrixQR(); + zeroBelowDiagonal(actual); - EXPECT(assert_equal(expected, actual, 1e-3)); + EXPECT(assert_equal(expected, actual, 1e-3)); - // use shiny new in place QR inside gtsam - A = Matrix(Matrix_(4, 7, data)); - inplace_QR(A); - EXPECT(assert_equal(expected, A, 1e-3)); + // use shiny new in place QR inside gtsam + A = Matrix(Matrix_(4, 7, data)); + inplace_QR(A); + EXPECT(assert_equal(expected, A, 1e-3)); } /* ************************************************************************* */ @@ -824,125 +824,125 @@ TEST( matrix, eigen_QR ) /* ************************************************************************* */ TEST( matrix, qr ) { - double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, - 0, 0, -10, 10, 0, -10, 0 }; - Matrix A = Matrix_(6, 4, data); + double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, + 0, 0, -10, 10, 0, -10, 0 }; + Matrix A = Matrix_(6, 4, data); - double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, - 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, - 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, - 0, -0.5963, 0, 0, -0.4472, }; - Matrix expectedQ = Matrix_(6, 6, dataQ); + double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, + 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, + 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, + 0, -0.5963, 0, 0, -0.4472, }; + Matrix expectedQ = Matrix_(6, 6, dataQ); - double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, - 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, }; - Matrix expectedR = Matrix_(6, 4, dataR); + double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, + 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, }; + Matrix expectedR = Matrix_(6, 4, dataR); - Matrix Q, R; - boost::tie(Q, R) = qr(A); - EXPECT(assert_equal(expectedQ, Q, 1e-4)); - EXPECT(assert_equal(expectedR, R, 1e-4)); - EXPECT(assert_equal(A, Q*R, 1e-14)); + Matrix Q, R; + boost::tie(Q, R) = qr(A); + EXPECT(assert_equal(expectedQ, Q, 1e-4)); + EXPECT(assert_equal(expectedR, R, 1e-4)); + EXPECT(assert_equal(A, Q*R, 1e-14)); } /* ************************************************************************* */ TEST( matrix, sub ) { - double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, - 0, 00, 10, 0, 0, 0, -10 }; - Matrix A = Matrix_(4, 6, data1); - Matrix actual = sub(A, 1, 3, 1, 5); + double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, + 0, 00, 10, 0, 0, 0, -10 }; + Matrix A = Matrix_(4, 6, data1); + Matrix actual = sub(A, 1, 3, 1, 5); - double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; - Matrix expected = Matrix_(2, 4, data2); + double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; + Matrix expected = Matrix_(2, 4, data2); - EQUALITY(actual,expected); + EQUALITY(actual,expected); } /* ************************************************************************* */ TEST( matrix, trans ) { - Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0); - Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - EQUALITY(trans(A),B); + Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0); + Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - const double* a = &A(0, 0); - DOUBLES_EQUAL(2.0,a[2],1e-9); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + const double* a = &A(0, 0); + DOUBLES_EQUAL(2.0,a[2],1e-9); } /* ************************************************************************* */ TEST( matrix, weighted_elimination ) { - // create a matrix to eliminate - Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., - 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); - Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); - Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + // create a matrix to eliminate + Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., + 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); + Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); + Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); - // expected values - Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., - -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); - Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); - Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); + // expected values + Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., + -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); + Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); + Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); - Vector r; - double di, sigma; - size_t i; + Vector r; + double di, sigma; + size_t i; - // perform elimination - Matrix A1 = A; - Vector b1 = b; - std::list > solution = - weighted_eliminate(A1, b1, sigmas); + // perform elimination + Matrix A1 = A; + Vector b1 = b; + std::list > solution = + weighted_eliminate(A1, b1, sigmas); - // unpack and verify - i = 0; - BOOST_FOREACH(boost::tie(r, di, sigma), solution){ - EXPECT(assert_equal(r, expectedR.row(i))); // verify r - DOUBLES_EQUAL(d(i), di, 1e-8); // verify d - DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma - i += 1; - } + // unpack and verify + i = 0; + BOOST_FOREACH(boost::tie(r, di, sigma), solution){ + EXPECT(assert_equal(r, expectedR.row(i))); // verify r + DOUBLES_EQUAL(d(i), di, 1e-8); // verify d + DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma + i += 1; + } } /* ************************************************************************* */ TEST( matrix, inverse_square_root ) { - Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25, - 0.0, 0.0, 0.0, 0.01); - Matrix actual = inverse_square_root(measurement_covariance); + Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25, + 0.0, 0.0, 0.0, 0.01); + Matrix actual = inverse_square_root(measurement_covariance); - Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, - 10.0); + Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, + 10.0); - EQUALITY(expected,actual); - EQUALITY(measurement_covariance,inverse(actual*actual)); + EQUALITY(expected,actual); + EQUALITY(measurement_covariance,inverse(actual*actual)); - // Randomly generated test. This test really requires inverse to - // be working well; if it's not, there's the possibility of a - // bug in inverse masking a bug in this routine since we - // use the same inverse routing inside inverse_square_root() - // as we use here to check it. + // Randomly generated test. This test really requires inverse to + // be working well; if it's not, there's the possibility of a + // bug in inverse masking a bug in this routine since we + // use the same inverse routing inside inverse_square_root() + // as we use here to check it. - Matrix M = Matrix_(5, 5, - 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, - 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, - -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, - -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, - 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); + Matrix M = Matrix_(5, 5, + 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, + 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, + -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, + -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, + 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); - expected = Matrix_(5, 5, - 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, - 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, - -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); - EQUALITY(expected, inverse_square_root(M)); + expected = Matrix_(5, 5, + 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, + 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, + -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); + EQUALITY(expected, inverse_square_root(M)); } @@ -951,80 +951,80 @@ TEST( matrix, inverse_square_root ) // we are checking against was generated via chol(M)' on octave TEST( matrix, LLt ) { - Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); - Matrix expected = Matrix_(5, 5, - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); + Matrix expected = Matrix_(5, 5, + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - EQUALITY(expected, LLt(M)); + EQUALITY(expected, LLt(M)); } /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), - expected = e + A * x; + Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + expected = e + A * x; - multiplyAdd(1, A, x, e); - EXPECT(assert_equal(expected, e)); + multiplyAdd(1, A, x, e); + EXPECT(assert_equal(expected, e)); } /* ************************************************************************* */ TEST( matrix, transposeMultiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), - expected = x + trans(A) * e; + Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + expected = x + trans(A) * e; - transposeMultiplyAdd(1, A, e, x); - EXPECT(assert_equal(expected, x)); + transposeMultiplyAdd(1, A, e, x); + EXPECT(assert_equal(expected, x)); } /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); - EXPECT(linear_dependent(A, B)); + Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); + EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); - EXPECT(linear_dependent(A, B)); + Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); + EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); - EXPECT(linear_independent(A, B)); + Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); + EXPECT(linear_independent(A, B)); } /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = Vector_(3, 2., 1., 0.); - Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) - * Matrix(trans(V1)); - Matrix U, V; - Vector s; - svd(A, U, s, V); - Matrix S = diag(s); - EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); - EXPECT(assert_equal(S,S1)); + Vector v = Vector_(3, 2., 1., 0.); + Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) + * Matrix(trans(V1)); + Matrix U, V; + Vector s; + svd(A, U, s, V); + Matrix S = diag(s); + EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); + EXPECT(assert_equal(S,S1)); } /* ************************************************************************* */ @@ -1035,39 +1035,39 @@ static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ TEST( matrix, svd2 ) { - Matrix U, V; - Vector s; + Matrix U, V; + Vector s; - Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); - Vector expected_s = Vector_(2, 3.,2.); - Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); + Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); + Vector expected_s = Vector_(2, 3.,2.); + Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); - svd(sampleA, U, s, V); + svd(sampleA, U, s, V); - EXPECT(assert_equal(expectedU,U)); - EXPECT(assert_equal(expected_s,s,1e-9)); - EXPECT(assert_equal(expectedV,V)); + EXPECT(assert_equal(expectedU,U)); + EXPECT(assert_equal(expected_s,s,1e-9)); + EXPECT(assert_equal(expectedV,V)); } /* ************************************************************************* */ TEST( matrix, svd3 ) { - Matrix U, V; - Vector s; + Matrix U, V; + Vector s; - Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); - Vector expected_s = Vector_(2, 3.0,2.0); - Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); + Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); + Vector expected_s = Vector_(2, 3.0,2.0); + Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); - svd(sampleAt, U, s, V); - Matrix S = diag(s); - Matrix t = U * S; - Matrix Vt = trans(V); + svd(sampleAt, U, s, V); + Matrix S = diag(s); + Matrix t = U * S; + Matrix Vt = trans(V); - EXPECT(assert_equal(sampleAt, prod(t, Vt))); - EXPECT(assert_equal(expectedU,U)); - EXPECT(assert_equal(expected_s,s,1e-9)); - EXPECT(assert_equal(expectedV,V)); + EXPECT(assert_equal(sampleAt, prod(t, Vt))); + EXPECT(assert_equal(expectedU,U)); + EXPECT(assert_equal(expected_s,s,1e-9)); + EXPECT(assert_equal(expectedV,V)); } /* ************************************************************************* */ @@ -1126,7 +1126,7 @@ TEST( matrix, DLT ) /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index ffbaca368..d8c62b121 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -23,12 +23,12 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testTestableAssertions, optional ) { - typedef boost::optional OptionalScalar; - LieScalar x(1.0); - OptionalScalar ox(x), dummy = boost::none; - EXPECT(assert_equal(ox, ox)); - EXPECT(assert_equal(x, ox)); - EXPECT(assert_equal(dummy, dummy)); + typedef boost::optional OptionalScalar; + LieScalar x(1.0); + OptionalScalar ox(x), dummy = boost::none; + EXPECT(assert_equal(ox, ox)); + EXPECT(assert_equal(x, ox)); + EXPECT(assert_equal(dummy, dummy)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index b8bb0333b..8fd151dbf 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -101,15 +101,15 @@ TEST( TestVector, sub ) /* ************************************************************************* */ TEST( TestVector, subInsert ) { - Vector big = zero(6), - small = ones(3); + Vector big = zero(6), + small = ones(3); - size_t i = 2; - subInsert(big, small, i); + size_t i = 2; + subInsert(big, small, i); - Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); + Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ @@ -154,66 +154,66 @@ TEST( TestVector, concatVectors) /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse ) { - // column from a matrix - Vector x(2); - x(0) = 1.0; x(1) = 2.0; + // column from a matrix + Vector x(2); + x(0) = 1.0; x(1) = 2.0; - // create sigmas - Vector sigmas(2); - sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.1; sigmas(1) = 0.2; + Vector weights = reciprocal(emul(sigmas,sigmas)); - // perform solve - Vector actual; double precision; - boost::tie(actual, precision) = weightedPseudoinverse(x, weights); + // perform solve + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); - // construct expected - Vector expected(2); - expected(0) = 0.5; expected(1) = 0.25; - double expPrecision = 200.0; + // construct expected + Vector expected(2); + expected(0) = 0.5; expected(1) = 0.25; + double expPrecision = 200.0; - // verify - EXPECT(assert_equal(expected,actual)); - EXPECT(fabs(expPrecision-precision) < 1e-5); + // verify + EXPECT(assert_equal(expected,actual)); + EXPECT(fabs(expPrecision-precision) < 1e-5); } /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_constraint ) { - // column from a matrix - Vector x(2); - x(0) = 1.0; x(1) = 2.0; + // column from a matrix + Vector x(2); + x(0) = 1.0; x(1) = 2.0; - // create sigmas - Vector sigmas(2); - sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.0; sigmas(1) = 0.2; + Vector weights = reciprocal(emul(sigmas,sigmas)); - // perform solve - Vector actual; double precision; - boost::tie(actual, precision) = weightedPseudoinverse(x, weights); + // perform solve + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); - // construct expected - Vector expected(2); - expected(0) = 1.0; expected(1) = 0.0; + // construct expected + Vector expected(2); + expected(0) = 1.0; expected(1) = 0.0; - // verify - EXPECT(assert_equal(expected,actual)); - EXPECT(isinf(precision)); + // verify + EXPECT(assert_equal(expected,actual)); + EXPECT(isinf(precision)); } /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_nan ) { - Vector a = Vector_(4, 1., 0., 0., 0.); - Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); - Vector weights = reciprocal(emul(sigmas,sigmas)); - Vector pseudo; double precision; - boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); + Vector a = Vector_(4, 1., 0., 0., 0.); + Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); + Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector pseudo; double precision; + boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); - Vector expected = Vector_(4, 1., 0., 0.,0.); - EXPECT(assert_equal(expected, pseudo)); - DOUBLES_EQUAL(100, precision, 1e-5); + Vector expected = Vector_(4, 1., 0., 0.,0.); + EXPECT(assert_equal(expected, pseudo)); + DOUBLES_EQUAL(100, precision, 1e-5); } /* ************************************************************************* */ @@ -251,50 +251,50 @@ TEST( TestVector, axpy ) /* ************************************************************************* */ TEST( TestVector, equals ) { - Vector v1 = Vector_(1, 0.0/std::numeric_limits::quiet_NaN()); //testing nan - Vector v2 = Vector_(1, 1.0); - double tol = 1.; - EXPECT(!equal_with_abs_tol(v1, v2, tol)); + Vector v1 = Vector_(1, 0.0/std::numeric_limits::quiet_NaN()); //testing nan + Vector v2 = Vector_(1, 1.0); + double tol = 1.; + EXPECT(!equal_with_abs_tol(v1, v2, tol)); } /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0), - v2 = zero(3); - EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than - EXPECT(greaterThanOrEqual(v1, v2)); // test equals + Vector v1 = Vector_(3, 1.0, 2.0, 3.0), + v2 = zero(3); + EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than + EXPECT(greaterThanOrEqual(v1, v2)); // test equals } /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = Vector_(3, 1.0, 2.0, 4.0); + Vector v = Vector_(3, 1.0, 2.0, 4.0); EXPECT(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0); - Vector v2 = Vector_(3, -2.0, -4.0, -6.0); - EXPECT(linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 1.0, 2.0, 3.0); + Vector v2 = Vector_(3, -2.0, -4.0, -6.0); + EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.0, -4.0, 0.0); - EXPECT(linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 0.0, 2.0, 0.0); + Vector v2 = Vector_(3, 0.0, -4.0, 0.0); + EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.1, -4.1, 0.0); - EXPECT(!linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 0.0, 2.0, 0.0); + Vector v2 = Vector_(3, 0.1, -4.1, 0.0); + EXPECT(!linear_dependent(v1, v2)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/timeMatrix.cpp b/gtsam/base/tests/timeMatrix.cpp index b85183854..9ed397ec8 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/gtsam/base/tests/timeMatrix.cpp @@ -36,41 +36,41 @@ using namespace gtsam; * - rev 2100 no pass: 18.45 sec , pass: 18.35 sec */ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { - // create a large number of matrices - // p = number of matrices - // m = rows per matrix - // n = columns per matrix - // reps = number of repetitions + // create a large number of matrices + // p = number of matrices + // m = rows per matrix + // n = columns per matrix + // reps = number of repetitions - // fill the matrices with identities - vector matrices; - for (size_t i=0; i matrices; + for (size_t i=0; i(M, j); - result = column(M, j); - elapsed = t.elapsed(); - } - return elapsed; + // extract a column + double elapsed; + Vector result; + { + boost::timer t; + for (size_t i=0; i(M, j); + result = column(M, j); + elapsed = t.elapsed(); + } + return elapsed; } /* @@ -188,24 +188,24 @@ double timeColumn(size_t reps) { * */ double timeHouseholder(size_t reps) { - // create a matrix - Matrix Abase = Matrix_(4, 7, - -5, 0, 5, 0, 0, 0, -1, - 00, -5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10, 0, 2, - 00, 10, 0, 0, 0,-10, -1); + // create a matrix + Matrix Abase = Matrix_(4, 7, + -5, 0, 5, 0, 0, 0, -1, + 00, -5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10, 0, 2, + 00, 10, 0, 0, 0,-10, -1); - // perform timing - double elapsed; - { - boost::timer t; - for (size_t i=0; i @@ -41,10 +41,10 @@ std::string timingPrefix; /* ************************************************************************* */ void TimingOutline::add(size_t usecs) { - t_ += usecs; - tIt_ += usecs; - t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); - ++ n_; + t_ += usecs; + tIt_ += usecs; + t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); + ++ n_; } /* ************************************************************************* */ @@ -53,38 +53,38 @@ TimingOutline::TimingOutline(const std::string& label) : /* ************************************************************************* */ size_t TimingOutline::time() const { - size_t time = 0; - bool hasChildren = false; - BOOST_FOREACH(const boost::shared_ptr& child, children_) { - if(child) { - time += child->time(); - hasChildren = true; - } - } - if(hasChildren) - return time; - else - return t_; + size_t time = 0; + bool hasChildren = false; + BOOST_FOREACH(const boost::shared_ptr& child, children_) { + if(child) { + time += child->time(); + hasChildren = true; + } + } + if(hasChildren) + return time; + else + return t_; } /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { - std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" << - n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 << - " max: " << double(tMax_)/1000000.0 << ")\n"; - for(size_t i=0; i 0) - childOutline += "."; - childOutline += (boost::format("%d") % i).str(); + if(childOutline.size() > 0) + childOutline += "."; + childOutline += (boost::format("%d") % i).str(); #else - childOutline += " "; + childOutline += " "; #endif - children_[i]->print(childOutline); - } - } + children_[i]->print(childOutline); + } + } } void TimingOutline::print2(const std::string& outline, const double parentTotal) const { @@ -130,33 +130,33 @@ void TimingOutline::print2(const std::string& outline, const double parentTotal) /* ************************************************************************* */ const boost::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr) { - assert(thisPtr.lock().get() == this); - // Resize if necessary - if(child >= children_.size()) - children_.resize(child + 1); - // Create child if necessary - if(children_[child]) { + assert(thisPtr.lock().get() == this); + // Resize if necessary + if(child >= children_.size()) + children_.resize(child + 1); + // Create child if necessary + if(children_[child]) { #ifndef NDEBUG - if(children_[child]->label_ != label) { - timingRoot->print(); - std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; - exit(1); - } + if(children_[child]->label_ != label) { + timingRoot->print(); + std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; + exit(1); + } #endif - } else { - children_[child].reset(new TimingOutline(label)); - children_[child]->parent_ = thisPtr; - } - return children_[child]; + } else { + children_[child].reset(new TimingOutline(label)); + children_[child]->parent_ = thisPtr; + } + return children_[child]; } /* ************************************************************************* */ void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS - assert(timer_.is_stopped()); + assert(timer_.is_stopped()); timer_.start(); #else - assert(!timerActive_); + assert(!timerActive_); timer_.restart(); *timerActive_ = true; #endif @@ -165,9 +165,9 @@ void TimingOutline::tic() { /* ************************************************************************* */ void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS - assert(!timer_.is_stopped()); + assert(!timer_.is_stopped()); timer_.stop(); - add((timer_.elapsed().user + timer_.elapsed().system) / 1000); + add((timer_.elapsed().user + timer_.elapsed().system) / 1000); #else assert(timerActive_); double elapsed = timer_.elapsed(); @@ -178,14 +178,14 @@ void TimingOutline::toc() { /* ************************************************************************* */ void TimingOutline::finishedIteration() { - if(tIt_ > tMax_) - tMax_ = tIt_; - if(tMin_ == 0 || tIt_ < tMin_) - tMin_ = tIt_; - tIt_ = 0; - for(size_t i=0; ifinishedIteration(); + if(tIt_ > tMax_) + tMax_ = tIt_; + if(tMin_ == 0 || tIt_ < tMin_) + tMin_ = tIt_; + tIt_ = 0; + for(size_t i=0; ifinishedIteration(); } /* ************************************************************************* */ @@ -199,47 +199,47 @@ void tic_(size_t id, const std::string& label) { /* ************************************************************************* */ void toc_(size_t id) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { - if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - != current->parent_.lock()->children_.end()) - std::cout << "gtsam timing: Incorrect ID passed to toc, expected " - << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() - << " \"" << current->label_ << "\", got " << id << std::endl; - else - std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); - } - current->toc(); - if(!current->parent_.lock()) { - std::cout << "gtsam timing: extra toc, already at the root" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: extra toc, already at the root"); - } - timingCurrent = current->parent_; + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ")" << std::endl; + boost::shared_ptr current(timingCurrent.lock()); + if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { + if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) + != current->parent_.lock()->children_.end()) + std::cout << "gtsam timing: Incorrect ID passed to toc, expected " + << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() + << " \"" << current->label_ << "\", got " << id << std::endl; + else + std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); + } + current->toc(); + if(!current->parent_.lock()) { + std::cout << "gtsam timing: extra toc, already at the root" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: extra toc, already at the root"); + } + timingCurrent = current->parent_; } /* ************************************************************************* */ void toc_(size_t id, const std::string& label) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ", " << label << ")" << std::endl; - bool check = false; + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ", " << label << ")" << std::endl; + bool check = false; #ifndef NDEBUG - // If NDEBUG is defined, still do this debug check if the granular debugging - // flag is enabled. If NDEBUG is not defined, always do this check. - check = true; + // If NDEBUG is defined, still do this debug check if the granular debugging + // flag is enabled. If NDEBUG is not defined, always do this check. + check = true; #endif - if(check || ISDEBUG("timing-debug")) { - if(label != timingCurrent.lock()->label_) { - std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; - timingRoot->print(); - exit(1); - } - } - toc_(id); + if(check || ISDEBUG("timing-debug")) { + if(label != timingCurrent.lock()->label_) { + std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; + timingRoot->print(); + exit(1); + } + } + toc_(id); } #ifdef ENABLE_OLD_TIMING @@ -247,12 +247,12 @@ void toc_(size_t id, const std::string& label) { /* ************************************************************************* */ // Timing class implementation void Timing::print() { - std::map::iterator it; - for(it = this->stats.begin(); it!=stats.end(); it++) { - Stats& s = it->second; - printf("%s: %g (%i times, min: %g, max: %g)\n", - it->first.c_str(), s.t, s.n, s.t_min, s.t_max); - } + std::map::iterator it; + for(it = this->stats.begin(); it!=stats.end(); it++) { + Stats& s = it->second; + printf("%s: %g (%i times, min: %g, max: %g)\n", + it->first.c_str(), s.t, s.n, s.t_min, s.t_max); + } } /* ************************************************************************* */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a42c2acc5..6d4cb192f 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -26,8 +26,8 @@ namespace gtsam { - /// Integer variable index type - typedef size_t Index; + /// Integer variable index type + typedef size_t Index; /** A function to convert indices to strings, for example by translating back * to a nonlinear key and then to a Symbol. */ @@ -38,48 +38,48 @@ namespace gtsam { /** The default IndexFormatter outputs the index */ static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; - /** - * Helper class that uses templates to select between two types based on - * whether TEST_TYPE is const or not. - */ - template - struct const_selector { - }; + /** + * Helper class that uses templates to select between two types based on + * whether TEST_TYPE is const or not. + */ + template + struct const_selector { + }; - /** Specialization for the non-const version */ - template - struct const_selector { - typedef AS_NON_CONST type; - }; + /** Specialization for the non-const version */ + template + struct const_selector { + typedef AS_NON_CONST type; + }; - /** Specialization for the const version */ - template - struct const_selector { - typedef AS_CONST type; - }; + /** Specialization for the const version */ + template + struct const_selector { + typedef AS_CONST type; + }; - /** - * Helper struct that encapsulates a value with a default, this is just used - * as a member object so you don't have to specify defaults in the class - * constructor. - */ - template - struct ValueWithDefault { - T value; + /** + * Helper struct that encapsulates a value with a default, this is just used + * as a member object so you don't have to specify defaults in the class + * constructor. + */ + template + struct ValueWithDefault { + T value; - /** Default constructor, initialize to default value supplied in template argument */ - ValueWithDefault() : value(defaultValue) {} + /** Default constructor, initialize to default value supplied in template argument */ + ValueWithDefault() : value(defaultValue) {} - /** Initialize to the given value */ - ValueWithDefault(const T& _value) : value(_value) {} + /** Initialize to the given value */ + ValueWithDefault(const T& _value) : value(_value) {} - /** Operator to access the value */ - T& operator*() { return value; } + /** Operator to access the value */ + T& operator*() { return value; } - /** Implicit conversion allows use in if statements for bool type, etc. */ - operator T() const { return value; } - }; + /** Implicit conversion allows use in if statements for bool type, etc. */ + operator T() const { return value; } + }; } diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 8afb2d617..041d4c206 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file AlgebraicDecisionTree.h - * @brief Algebraic Decision Trees - * @author Frank Dellaert - * @date Mar 14, 2011 + * @file AlgebraicDecisionTree.h + * @brief Algebraic Decision Trees + * @author Frank Dellaert + * @date Mar 14, 2011 */ #pragma once @@ -22,123 +22,123 @@ namespace gtsam { - /** - * Algebraic Decision Trees fix the range to double - * Just has some nice constructors and some syntactic sugar - * TODO: consider eliminating this class altogether? - */ - template - class AlgebraicDecisionTree: public DecisionTree { + /** + * Algebraic Decision Trees fix the range to double + * Just has some nice constructors and some syntactic sugar + * TODO: consider eliminating this class altogether? + */ + template + class AlgebraicDecisionTree: public DecisionTree { - public: + public: - typedef DecisionTree Super; + typedef DecisionTree Super; - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { - return 0.0; - } - static inline double one() { - return 1.0; - } - static inline double add(const double& a, const double& b) { - return a + b; - } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { - return a * b; - } - static inline double div(const double& a, const double& b) { - return a / b; - } - static inline double id(const double& x) { - return x; - } - }; + /** The Real ring with addition and multiplication */ + struct Ring { + static inline double zero() { + return 0.0; + } + static inline double one() { + return 1.0; + } + static inline double add(const double& a, const double& b) { + return a + b; + } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { + return a * b; + } + static inline double div(const double& a, const double& b) { + return a / b; + } + static inline double id(const double& x) { + return x; + } + }; - AlgebraicDecisionTree() : - Super(1.0) { - } + AlgebraicDecisionTree() : + Super(1.0) { + } - AlgebraicDecisionTree(const Super& add) : - Super(add) { - } + AlgebraicDecisionTree(const Super& add) : + Super(add) { + } - /** Create a new leaf function splitting on a variable */ - AlgebraicDecisionTree(const L& label, double y1, double y2) : - Super(label, y1, y2) { - } + /** Create a new leaf function splitting on a variable */ + AlgebraicDecisionTree(const L& label, double y1, double y2) : + Super(label, y1, y2) { + } - /** Create a new leaf function splitting on a variable */ - AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) : - Super(labelC, y1, y2) { - } + /** Create a new leaf function splitting on a variable */ + AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) : + Super(labelC, y1, y2) { + } - /** Create from keys and vector table */ - AlgebraicDecisionTree // - (const std::vector& labelCs, const std::vector& ys) { - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), - ys.end()); - } + /** Create from keys and vector table */ + AlgebraicDecisionTree // + (const std::vector& labelCs, const std::vector& ys) { + this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + ys.end()); + } - /** Create from keys and string table */ - AlgebraicDecisionTree // - (const std::vector& labelCs, const std::string& table) { - // Convert string to doubles - std::vector ys; - std::istringstream iss(table); - std::copy(std::istream_iterator(iss), - std::istream_iterator(), std::back_inserter(ys)); + /** Create from keys and string table */ + AlgebraicDecisionTree // + (const std::vector& labelCs, const std::string& table) { + // Convert string to doubles + std::vector ys; + std::istringstream iss(table); + std::copy(std::istream_iterator(iss), + std::istream_iterator(), std::back_inserter(ys)); - // now call recursive Create - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), - ys.end()); - } + // now call recursive Create + this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + ys.end()); + } - /** Create a new function splitting on a variable */ - template - AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(NULL) { - this->root_ = compose(begin, end, label); - } + /** Create a new function splitting on a variable */ + template + AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : + Super(NULL) { + this->root_ = compose(begin, end, label); + } - /** Convert */ - template - AlgebraicDecisionTree(const AlgebraicDecisionTree& other, - const std::map& map) { - this->root_ = this->template convert(other.root_, map, - Ring::id); - } + /** Convert */ + template + AlgebraicDecisionTree(const AlgebraicDecisionTree& other, + const std::map& map) { + this->root_ = this->template convert(other.root_, map, + Ring::id); + } - /** sum */ - AlgebraicDecisionTree operator+(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::add); - } + /** sum */ + AlgebraicDecisionTree operator+(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::add); + } - /** product */ - AlgebraicDecisionTree operator*(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::mul); - } + /** product */ + AlgebraicDecisionTree operator*(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::mul); + } - /** division */ - AlgebraicDecisionTree operator/(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::div); - } + /** division */ + AlgebraicDecisionTree operator/(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::div); + } - /** sum out variable */ - AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { - return this->combine(label, cardinality, &Ring::add); - } + /** sum out variable */ + AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { + return this->combine(label, cardinality, &Ring::add); + } - /** sum out variable */ - AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const { - return this->combine(labelC, &Ring::add); - } + /** sum out variable */ + AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const { + return this->combine(labelC, &Ring::add); + } - }; + }; // AlgebraicDecisionTree } diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index f6decc359..709c7350e 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file Assignment.h - * @brief An assignment from labels to a discrete value index (size_t) - * @author Frank Dellaert - * @date Feb 5, 2012 + * @file Assignment.h + * @brief An assignment from labels to a discrete value index (size_t) + * @author Frank Dellaert + * @date Feb 5, 2012 */ #pragma once @@ -26,62 +26,62 @@ namespace gtsam { - /** - * An assignment from labels to value index (size_t). - * Assigns to each label a value. Implemented as a simple map. - * A discrete factor takes an Assignment and returns a value. - */ - template - class Assignment: public std::map { - public: - void print(const std::string& s = "Assignment: ") const { - std::cout << s << ": "; - BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this) - std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; - std::cout << std::endl; - } + /** + * An assignment from labels to value index (size_t). + * Assigns to each label a value. Implemented as a simple map. + * A discrete factor takes an Assignment and returns a value. + */ + template + class Assignment: public std::map { + public: + void print(const std::string& s = "Assignment: ") const { + std::cout << s << ": "; + BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this) + std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; + std::cout << std::endl; + } - bool equals(const Assignment& other, double tol = 1e-9) const { - return (*this == other); - } - }; //Assignment + bool equals(const Assignment& other, double tol = 1e-9) const { + return (*this == other); + } + }; //Assignment - /** - * @brief Get Cartesian product consisting all possible configurations - * @param vector list of keys (label,cardinality) pairs. - * @return vector list of all possible value assignments - * - * This function returns a vector of Assignment values for all possible - * (Cartesian product) configurations of set of Keys which are nothing - * but (Label,cardinality) pairs. This function should NOT be called for - * more than a small number of variables and cardinalities. E.g. For 6 - * variables with each having cardinalities 4, we get 4096 possible - * configurations!! - */ - template - std::vector > cartesianProduct( - const std::vector >& keys) { - std::vector > allPossValues; - Assignment values; - typedef std::pair DiscreteKey; - BOOST_FOREACH(const DiscreteKey& key, keys) - values[key.first] = 0; //Initialize from 0 - while (1) { - allPossValues.push_back(values); - size_t j = 0; - for (j = 0; j < keys.size(); j++) { - L idx = keys[j].first; - values[idx]++; - if (values[idx] < keys[j].second) - break; - //Wrap condition - values[idx] = 0; - } - if (j == keys.size()) - break; - } - return allPossValues; - } + /** + * @brief Get Cartesian product consisting all possible configurations + * @param vector list of keys (label,cardinality) pairs. + * @return vector list of all possible value assignments + * + * This function returns a vector of Assignment values for all possible + * (Cartesian product) configurations of set of Keys which are nothing + * but (Label,cardinality) pairs. This function should NOT be called for + * more than a small number of variables and cardinalities. E.g. For 6 + * variables with each having cardinalities 4, we get 4096 possible + * configurations!! + */ + template + std::vector > cartesianProduct( + const std::vector >& keys) { + std::vector > allPossValues; + Assignment values; + typedef std::pair DiscreteKey; + BOOST_FOREACH(const DiscreteKey& key, keys) + values[key.first] = 0; //Initialize from 0 + while (1) { + allPossValues.push_back(values); + size_t j = 0; + for (j = 0; j < keys.size(); j++) { + L idx = keys[j].first; + values[idx]++; + if (values[idx] < keys[j].second) + break; + //Wrap condition + values[idx] = 0; + } + if (j == keys.size()) + break; + } + return allPossValues; + } } // namespace gtsam diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 057f014bf..a13dba513 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #pragma once @@ -38,637 +38,637 @@ using boost::assign::operator+=; namespace gtsam { - /*********************************************************************************/ - // Node - /*********************************************************************************/ + /*********************************************************************************/ + // Node + /*********************************************************************************/ #ifdef DT_DEBUG_MEMORY - template - int DecisionTree::Node::nrNodes = 0; + template + int DecisionTree::Node::nrNodes = 0; #endif - /*********************************************************************************/ - // Leaf - /*********************************************************************************/ - template - class DecisionTree::Leaf: public DecisionTree::Node { + /*********************************************************************************/ + // Leaf + /*********************************************************************************/ + template + class DecisionTree::Leaf: public DecisionTree::Node { - /** constant stored in this leaf */ - Y constant_; + /** constant stored in this leaf */ + Y constant_; - public: + public: - /** Constructor from constant */ - Leaf(const Y& constant) : - constant_(constant) {} + /** Constructor from constant */ + Leaf(const Y& constant) : + constant_(constant) {} - /** return the constant */ - const Y& constant() const { - return constant_; - } + /** return the constant */ + const Y& constant() const { + return constant_; + } - /// Leaf-Leaf equality - bool sameLeaf(const Leaf& q) const { - return constant_ == q.constant_; - } + /// Leaf-Leaf equality + bool sameLeaf(const Leaf& q) const { + return constant_ == q.constant_; + } - /// polymorphic equality: is q is a leaf, could be - bool sameLeaf(const Node& q) const { - return (q.isLeaf() && q.sameLeaf(*this)); - } + /// polymorphic equality: is q is a leaf, could be + bool sameLeaf(const Node& q) const { + return (q.isLeaf() && q.sameLeaf(*this)); + } - /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { - const Leaf* other = dynamic_cast (&q); - if (!other) return false; - return fabs(double(this->constant_ - other->constant_)) < tol; - } + /** equality up to tolerance */ + bool equals(const Node& q, double tol) const { + const Leaf* other = dynamic_cast (&q); + if (!other) return false; + return fabs(double(this->constant_ - other->constant_)) < tol; + } - /** print */ - void print(const std::string& s) const { - bool showZero = true; - if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; - } + /** print */ + void print(const std::string& s) const { + bool showZero = true; + if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; + } - /** to graphviz file */ - void dot(std::ostream& os, bool showZero) const { - if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" - << boost::format("%4.2g") % constant_ - << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, - } + /** to graphviz file */ + void dot(std::ostream& os, bool showZero) const { + if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" + << boost::format("%4.2g") % constant_ + << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, + } - /** evaluate */ - const Y& operator()(const Assignment& x) const { - return constant_; - } + /** evaluate */ + const Y& operator()(const Assignment& x) const { + return constant_; + } - /** apply unary operator */ - NodePtr apply(const Unary& op) const { - NodePtr f(new Leaf(op(constant_))); - return f; - } + /** apply unary operator */ + NodePtr apply(const Unary& op) const { + NodePtr f(new Leaf(op(constant_))); + return f; + } - // Apply binary operator "h = f op g" on Leaf node - // Note op is not assumed commutative so we need to keep track of order - // Simply calls apply on argument to call correct virtual method: - // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) - // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { - return g.apply_g_op_fL(*this, op); - } + // Apply binary operator "h = f op g" on Leaf node + // Note op is not assumed commutative so we need to keep track of order + // Simply calls apply on argument to call correct virtual method: + // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) + // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) + NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + return g.apply_g_op_fL(*this, op); + } - // Applying binary operator to two leaves results in a leaf - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { - NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL - return h; - } + // Applying binary operator to two leaves results in a leaf + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL + return h; + } - // If second argument is a Choice node, call it's apply with leaf as second - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { - return fC.apply_fC_op_gL(*this, op); // operand order back to normal - } + // If second argument is a Choice node, call it's apply with leaf as second + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + return fC.apply_fC_op_gL(*this, op); // operand order back to normal + } - /** choose a branch, create new memory ! */ - NodePtr choose(const L& label, size_t index) const { - return NodePtr(new Leaf(constant())); - } + /** choose a branch, create new memory ! */ + NodePtr choose(const L& label, size_t index) const { + return NodePtr(new Leaf(constant())); + } - bool isLeaf() const { return true; } + bool isLeaf() const { return true; } - }; // Leaf + }; // Leaf - /*********************************************************************************/ - // Choice - /*********************************************************************************/ - template - class DecisionTree::Choice: public DecisionTree::Node { + /*********************************************************************************/ + // Choice + /*********************************************************************************/ + template + class DecisionTree::Choice: public DecisionTree::Node { - /** the label of the variable on which we split */ - L label_; + /** the label of the variable on which we split */ + L label_; - /** The children of this Choice node. */ - std::vector branches_; + /** The children of this Choice node. */ + std::vector branches_; - private: - /** incremental allSame */ - size_t allSame_; + private: + /** incremental allSame */ + size_t allSame_; - typedef boost::shared_ptr ChoicePtr; + typedef boost::shared_ptr ChoicePtr; - public: + public: - virtual ~Choice() { + virtual ~Choice() { #ifdef DT_DEBUG_MEMORY - std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl; + std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl; #endif - } + } - /** If all branches of a choice node f are the same, just return a branch */ - static NodePtr Unique(const ChoicePtr& f) { + /** If all branches of a choice node f are the same, just return a branch */ + static NodePtr Unique(const ChoicePtr& f) { #ifndef DT_NO_PRUNING - if (f->allSame_) { - assert(f->branches().size() > 0); - NodePtr f0 = f->branches_[0]; - assert(f0->isLeaf()); - NodePtr newLeaf(new Leaf(boost::dynamic_pointer_cast(f0)->constant())); - return newLeaf; - } else + if (f->allSame_) { + assert(f->branches().size() > 0); + NodePtr f0 = f->branches_[0]; + assert(f0->isLeaf()); + NodePtr newLeaf(new Leaf(boost::dynamic_pointer_cast(f0)->constant())); + return newLeaf; + } else #endif - return f; - } + return f; + } - bool isLeaf() const { return false; } + bool isLeaf() const { return false; } - /** Constructor, given choice label and mandatory expected branch count */ - Choice(const L& label, size_t count) : - label_(label), allSame_(true) { - branches_.reserve(count); - } + /** Constructor, given choice label and mandatory expected branch count */ + Choice(const L& label, size_t count) : + label_(label), allSame_(true) { + branches_.reserve(count); + } - /** - * Construct from applying binary op to two Choice nodes - */ - Choice(const Choice& f, const Choice& g, const Binary& op) : - allSame_(true) { + /** + * Construct from applying binary op to two Choice nodes + */ + Choice(const Choice& f, const Choice& g, const Binary& op) : + allSame_(true) { - // Choose what to do based on label - if (f.label() > g.label()) { - // f higher than g - label_ = f.label(); - size_t count = f.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(f.branches_[i]->apply_f_op_g(g, op)); - } else if (g.label() > f.label()) { - // f lower than g - label_ = g.label(); - size_t count = g.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(g.branches_[i]->apply_g_op_fC(f, op)); - } else { - // f same level as g - label_ = f.label(); - size_t count = f.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(f.branches_[i]->apply_f_op_g(*g.branches_[i], op)); - } - } + // Choose what to do based on label + if (f.label() > g.label()) { + // f higher than g + label_ = f.label(); + size_t count = f.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(f.branches_[i]->apply_f_op_g(g, op)); + } else if (g.label() > f.label()) { + // f lower than g + label_ = g.label(); + size_t count = g.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(g.branches_[i]->apply_g_op_fC(f, op)); + } else { + // f same level as g + label_ = f.label(); + size_t count = f.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(f.branches_[i]->apply_f_op_g(*g.branches_[i], op)); + } + } - const L& label() const { - return label_; - } + const L& label() const { + return label_; + } - size_t nrChoices() const { - return branches_.size(); - } + size_t nrChoices() const { + return branches_.size(); + } - const std::vector& branches() const { - return branches_; - } + const std::vector& branches() const { + return branches_; + } - /** add a branch: TODO merge into constructor */ - void push_back(const NodePtr& node) { - // allSame_ is restricted to leaf nodes in a decision tree - if (allSame_ && !branches_.empty()) { - allSame_ = node->sameLeaf(*branches_.back()); - } - branches_.push_back(node); - } + /** add a branch: TODO merge into constructor */ + void push_back(const NodePtr& node) { + // allSame_ is restricted to leaf nodes in a decision tree + if (allSame_ && !branches_.empty()) { + allSame_ = node->sameLeaf(*branches_.back()); + } + branches_.push_back(node); + } - /** print (as a tree) */ - void print(const std::string& s) const { - std::cout << s << " Choice("; - // std::cout << this << ","; - std::cout << label_ << ") " << std::endl; + /** print (as a tree) */ + void print(const std::string& s) const { + std::cout << s << " Choice("; + // std::cout << this << ","; + std::cout << label_ << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) - branches_[i]->print((boost::format("%s %d") % s % i).str()); - } + branches_[i]->print((boost::format("%s %d") % s % i).str()); + } - /** output to graphviz (as a a graph) */ - void dot(std::ostream& os, bool showZero) const { - os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ - << "\"]\n"; - for (size_t i = 0; i < branches_.size(); i++) { - NodePtr branch = branches_[i]; + /** output to graphviz (as a a graph) */ + void dot(std::ostream& os, bool showZero) const { + os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ + << "\"]\n"; + for (size_t i = 0; i < branches_.size(); i++) { + NodePtr branch = branches_[i]; - // Check if zero - if (!showZero) { - const Leaf* leaf = dynamic_cast (branch.get()); - if (leaf && !leaf->constant()) continue; - } + // Check if zero + if (!showZero) { + const Leaf* leaf = dynamic_cast (branch.get()); + if (leaf && !leaf->constant()) continue; + } - os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - os << std::endl; - branch->dot(os, showZero); - } - } + os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; + if (i == 0) os << " [style=dashed]"; + if (i > 1) os << " [style=bold]"; + os << std::endl; + branch->dot(os, showZero); + } + } - /// Choice-Leaf equality: always false - bool sameLeaf(const Leaf& q) const { - return false; - } + /// Choice-Leaf equality: always false + bool sameLeaf(const Leaf& q) const { + return false; + } - /// polymorphic equality: if q is a leaf, could be... - bool sameLeaf(const Node& q) const { - return (q.isLeaf() && q.sameLeaf(*this)); - } + /// polymorphic equality: if q is a leaf, could be... + bool sameLeaf(const Node& q) const { + return (q.isLeaf() && q.sameLeaf(*this)); + } - /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { - const Choice* other = dynamic_cast (&q); - if (!other) return false; - if (this->label_ != other->label_) return false; - if (branches_.size() != other->branches_.size()) return false; - // we don't care about shared pointers being equal here - for (size_t i = 0; i < branches_.size(); i++) - if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false; - return true; - } + /** equality up to tolerance */ + bool equals(const Node& q, double tol) const { + const Choice* other = dynamic_cast (&q); + if (!other) return false; + if (this->label_ != other->label_) return false; + if (branches_.size() != other->branches_.size()) return false; + // we don't care about shared pointers being equal here + for (size_t i = 0; i < branches_.size(); i++) + if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false; + return true; + } - /** evaluate */ - const Y& operator()(const Assignment& x) const { + /** evaluate */ + const Y& operator()(const Assignment& x) const { #ifndef NDEBUG - typename Assignment::const_iterator it = x.find(label_); - if (it == x.end()) { - std::cout << "Trying to find value for " << label_ << std::endl; - throw std::invalid_argument( - "DecisionTree::operator(): value undefined for a label"); - } + typename Assignment::const_iterator it = x.find(label_); + if (it == x.end()) { + std::cout << "Trying to find value for " << label_ << std::endl; + throw std::invalid_argument( + "DecisionTree::operator(): value undefined for a label"); + } #endif - size_t index = x.at(label_); - NodePtr child = branches_[index]; - return (*child)(x); - } + size_t index = x.at(label_); + NodePtr child = branches_[index]; + return (*child)(x); + } - /** - * Construct from applying unary op to a Choice node - */ - Choice(const L& label, const Choice& f, const Unary& op) : - label_(label), allSame_(true) { + /** + * Construct from applying unary op to a Choice node + */ + Choice(const L& label, const Choice& f, const Unary& op) : + label_(label), allSame_(true) { - branches_.reserve(f.branches_.size()); // reserve space - BOOST_FOREACH (const NodePtr& branch, f.branches_) - push_back(branch->apply(op)); - } + branches_.reserve(f.branches_.size()); // reserve space + BOOST_FOREACH (const NodePtr& branch, f.branches_) + push_back(branch->apply(op)); + } - /** apply unary operator */ - NodePtr apply(const Unary& op) const { - boost::shared_ptr r(new Choice(label_, *this, op)); - return Unique(r); - } + /** apply unary operator */ + NodePtr apply(const Unary& op) const { + boost::shared_ptr r(new Choice(label_, *this, op)); + return Unique(r); + } - // Apply binary operator "h = f op g" on Choice node - // Note op is not assumed commutative so we need to keep track of order - // Simply calls apply on argument to call correct virtual method: - // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) - // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { - return g.apply_g_op_fC(*this, op); - } + // Apply binary operator "h = f op g" on Choice node + // Note op is not assumed commutative so we need to keep track of order + // Simply calls apply on argument to call correct virtual method: + // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) + // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) + NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + return g.apply_g_op_fC(*this, op); + } - // If second argument of binary op is Leaf node, recurse on branches - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { - boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(NodePtr branch, branches_) - h->push_back(fL.apply_f_op_g(*branch, op)); - return Unique(h); - } + // If second argument of binary op is Leaf node, recurse on branches + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + boost::shared_ptr h(new Choice(label(), nrChoices())); + BOOST_FOREACH(NodePtr branch, branches_) + h->push_back(fL.apply_f_op_g(*branch, op)); + return Unique(h); + } - // If second argument of binary op is Choice, call constructor - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { - boost::shared_ptr h(new Choice(fC, *this, op)); - return Unique(h); - } + // If second argument of binary op is Choice, call constructor + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + boost::shared_ptr h(new Choice(fC, *this, op)); + return Unique(h); + } - // If second argument of binary op is Leaf - template - NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { - boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(const NodePtr& branch, branches_) - h->push_back(branch->apply_f_op_g(gL, op)); - return Unique(h); - } + // If second argument of binary op is Leaf + template + NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { + boost::shared_ptr h(new Choice(label(), nrChoices())); + BOOST_FOREACH(const NodePtr& branch, branches_) + h->push_back(branch->apply_f_op_g(gL, op)); + return Unique(h); + } - /** choose a branch, recursively */ - NodePtr choose(const L& label, size_t index) const { - if (label_ == label) - return branches_[index]; // choose branch - - // second case, not label of interest, just recurse - boost::shared_ptr r(new Choice(label_, branches_.size())); - BOOST_FOREACH(const NodePtr& branch, branches_) - r->push_back(branch->choose(label, index)); - return Unique(r); - } + /** choose a branch, recursively */ + NodePtr choose(const L& label, size_t index) const { + if (label_ == label) + return branches_[index]; // choose branch + + // second case, not label of interest, just recurse + boost::shared_ptr r(new Choice(label_, branches_.size())); + BOOST_FOREACH(const NodePtr& branch, branches_) + r->push_back(branch->choose(label, index)); + return Unique(r); + } - }; // Choice + }; // Choice - /*********************************************************************************/ - // DecisionTree - /*********************************************************************************/ - template - DecisionTree::DecisionTree() { - } + /*********************************************************************************/ + // DecisionTree + /*********************************************************************************/ + template + DecisionTree::DecisionTree() { + } - template - DecisionTree::DecisionTree(const NodePtr& root) : - root_(root) { - } + template + DecisionTree::DecisionTree(const NodePtr& root) : + root_(root) { + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const Y& y) { - root_ = NodePtr(new Leaf(y)); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const Y& y) { + root_ = NodePtr(new Leaf(y)); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(// - const L& label, const Y& y1, const Y& y2) { - boost::shared_ptr a(new Choice(label, 2)); - NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); - a->push_back(l1); - a->push_back(l2); - root_ = Choice::Unique(a); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(// + const L& label, const Y& y1, const Y& y2) { + boost::shared_ptr a(new Choice(label, 2)); + NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); + a->push_back(l1); + a->push_back(l2); + root_ = Choice::Unique(a); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(// - const LabelC& labelC, const Y& y1, const Y& y2) { - if (labelC.second != 2) throw std::invalid_argument( - "DecisionTree: binary constructor called with non-binary label"); - boost::shared_ptr a(new Choice(labelC.first, 2)); - NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); - a->push_back(l1); - a->push_back(l2); - root_ = Choice::Unique(a); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(// + const LabelC& labelC, const Y& y1, const Y& y2) { + if (labelC.second != 2) throw std::invalid_argument( + "DecisionTree: binary constructor called with non-binary label"); + boost::shared_ptr a(new Choice(labelC.first, 2)); + NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); + a->push_back(l1); + a->push_back(l2); + root_ = Choice::Unique(a); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const std::vector& labelCs, - const std::vector& ys) { - // call recursive Create - root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const std::vector& labelCs, + const std::vector& ys) { + // call recursive Create + root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const std::vector& labelCs, - const std::string& table) { + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const std::vector& labelCs, + const std::string& table) { - // Convert std::string to doubles - std::vector ys; - std::istringstream iss(table); - copy(std::istream_iterator(iss), std::istream_iterator(), - back_inserter(ys)); + // Convert std::string to doubles + std::vector ys; + std::istringstream iss(table); + copy(std::istream_iterator(iss), std::istream_iterator(), + back_inserter(ys)); - // now call recursive Create - root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); - } + // now call recursive Create + root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); + } - /*********************************************************************************/ - template - template DecisionTree::DecisionTree( - Iterator begin, Iterator end, const L& label) { - root_ = compose(begin, end, label); - } + /*********************************************************************************/ + template + template DecisionTree::DecisionTree( + Iterator begin, Iterator end, const L& label) { + root_ = compose(begin, end, label); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const L& label, - const DecisionTree& f0, const DecisionTree& f1) { - std::vector functions; - functions += f0, f1; - root_ = compose(functions.begin(), functions.end(), label); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const L& label, + const DecisionTree& f0, const DecisionTree& f1) { + std::vector functions; + functions += f0, f1; + root_ = compose(functions.begin(), functions.end(), label); + } - /*********************************************************************************/ - template - template - DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op) { - root_ = convert(other.root_, map, op); - } + /*********************************************************************************/ + template + template + DecisionTree::DecisionTree(const DecisionTree& other, + const std::map& map, boost::function op) { + root_ = convert(other.root_, map, op); + } - /*********************************************************************************/ - // Called by two constructors above. - // Takes a label and a corresponding range of decision trees, and creates a new - // decision tree. However, the order of the labels needs to be respected, so we - // cannot just create a root Choice node on the label: if the label is not the - // highest label, we need to do a complicated and expensive recursive call. - template template - typename DecisionTree::NodePtr DecisionTree::compose( - Iterator begin, Iterator end, const L& label) const { + /*********************************************************************************/ + // Called by two constructors above. + // Takes a label and a corresponding range of decision trees, and creates a new + // decision tree. However, the order of the labels needs to be respected, so we + // cannot just create a root Choice node on the label: if the label is not the + // highest label, we need to do a complicated and expensive recursive call. + template template + typename DecisionTree::NodePtr DecisionTree::compose( + Iterator begin, Iterator end, const L& label) const { - // find highest label among branches - boost::optional highestLabel; - boost::optional nrChoices; - for (Iterator it = begin; it != end; it++) { - if (it->root_->isLeaf()) continue; - boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); - if (!highestLabel || c->label() > *highestLabel) { - highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); - } - } + // find highest label among branches + boost::optional highestLabel; + boost::optional nrChoices; + for (Iterator it = begin; it != end; it++) { + if (it->root_->isLeaf()) continue; + boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); + if (!highestLabel || c->label() > *highestLabel) { + highestLabel.reset(c->label()); + nrChoices.reset(c->nrChoices()); + } + } - // if label is already in correct order, just put together a choice on label - if (!highestLabel || label > *highestLabel) { - boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); - for (Iterator it = begin; it != end; it++) - choiceOnLabel->push_back(it->root_); - return Choice::Unique(choiceOnLabel); - } + // if label is already in correct order, just put together a choice on label + if (!highestLabel || label > *highestLabel) { + boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); + for (Iterator it = begin; it != end; it++) + choiceOnLabel->push_back(it->root_); + return Choice::Unique(choiceOnLabel); + } - // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); - // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { - // make a new set of functions for composing by iterating over the given - // functions, and selecting the appropriate branch. - std::vector functions; - for (Iterator it = begin; it != end; it++) { - // by restricting the input functions to value i for labelBelow - DecisionTree chosen = it->choose(*highestLabel, index); - functions.push_back(chosen); - } - // We then recurse, for all values of the highest label - NodePtr fi = compose(functions.begin(), functions.end(), label); - choiceOnHighestLabel->push_back(fi); - } - return Choice::Unique(choiceOnHighestLabel); - } + // Set up a new choice on the highest label + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); + // now, for all possible values of highestLabel + for (size_t index = 0; index < *nrChoices; index++) { + // make a new set of functions for composing by iterating over the given + // functions, and selecting the appropriate branch. + std::vector functions; + for (Iterator it = begin; it != end; it++) { + // by restricting the input functions to value i for labelBelow + DecisionTree chosen = it->choose(*highestLabel, index); + functions.push_back(chosen); + } + // We then recurse, for all values of the highest label + NodePtr fi = compose(functions.begin(), functions.end(), label); + choiceOnHighestLabel->push_back(fi); + } + return Choice::Unique(choiceOnHighestLabel); + } - /*********************************************************************************/ - // "create" is a bit of a complicated thing, but very useful. - // It takes a range of labels and a corresponding range of values, - // and creates a decision tree, as follows: - // - if there is only one label, creates a choice node with values in leaves - // - otherwise, it evenly splits up the range of values and creates a tree for - // each sub-range, and assigns that tree to first label's choices - // Example: - // create([B A],[1 2 3 4]) would call - // create([A],[1 2]) - // create([A],[3 4]) - // and produce - // B=0 - // A=0: 1 - // A=1: 2 - // B=1 - // A=0: 3 - // A=1: 4 - // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce - // exactly the same tree as above: the highest label is always the root. - // However, it will be *way* faster if labels are given highest to lowest. - template - template - typename DecisionTree::NodePtr DecisionTree::create( - It begin, It end, ValueIt beginY, ValueIt endY) const { + /*********************************************************************************/ + // "create" is a bit of a complicated thing, but very useful. + // It takes a range of labels and a corresponding range of values, + // and creates a decision tree, as follows: + // - if there is only one label, creates a choice node with values in leaves + // - otherwise, it evenly splits up the range of values and creates a tree for + // each sub-range, and assigns that tree to first label's choices + // Example: + // create([B A],[1 2 3 4]) would call + // create([A],[1 2]) + // create([A],[3 4]) + // and produce + // B=0 + // A=0: 1 + // A=1: 2 + // B=1 + // A=0: 3 + // A=1: 4 + // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce + // exactly the same tree as above: the highest label is always the root. + // However, it will be *way* faster if labels are given highest to lowest. + template + template + typename DecisionTree::NodePtr DecisionTree::create( + It begin, It end, ValueIt beginY, ValueIt endY) const { - // get crucial counts - size_t nrChoices = begin->second; - size_t size = endY - beginY; + // get crucial counts + size_t nrChoices = begin->second; + size_t size = endY - beginY; - // Find the next key to work on - It labelC = begin + 1; - if (labelC == end) { - // Base case: only one key left - // Create a simple choice node with values as leaves. - if (size != nrChoices) { - std::cout << "Trying to create DD on " << begin->first << std::endl; - std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl; - throw std::invalid_argument("DecisionTree::create invalid argument"); - } - boost::shared_ptr choice(new Choice(begin->first, endY - beginY)); - for (ValueIt y = beginY; y != endY; y++) - choice->push_back(NodePtr(new Leaf(*y))); - return Choice::Unique(choice); - } + // Find the next key to work on + It labelC = begin + 1; + if (labelC == end) { + // Base case: only one key left + // Create a simple choice node with values as leaves. + if (size != nrChoices) { + std::cout << "Trying to create DD on " << begin->first << std::endl; + std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl; + throw std::invalid_argument("DecisionTree::create invalid argument"); + } + boost::shared_ptr choice(new Choice(begin->first, endY - beginY)); + for (ValueIt y = beginY; y != endY; y++) + choice->push_back(NodePtr(new Leaf(*y))); + return Choice::Unique(choice); + } - // Recursive case: perform "Shannon expansion" - // Creates one tree (i.e.,function) for each choice of current key - // by calling create recursively, and then puts them all together. - std::vector functions; - size_t split = size / nrChoices; - for (size_t i = 0; i < nrChoices; i++, beginY += split) { - NodePtr f = create(labelC, end, beginY, beginY + split); - functions += DecisionTree(f); - } - return compose(functions.begin(), functions.end(), begin->first); - } + // Recursive case: perform "Shannon expansion" + // Creates one tree (i.e.,function) for each choice of current key + // by calling create recursively, and then puts them all together. + std::vector functions; + size_t split = size / nrChoices; + for (size_t i = 0; i < nrChoices; i++, beginY += split) { + NodePtr f = create(labelC, end, beginY, beginY + split); + functions += DecisionTree(f); + } + return compose(functions.begin(), functions.end(), begin->first); + } - /*********************************************************************************/ - template - template - typename DecisionTree::NodePtr DecisionTree::convert( - const typename DecisionTree::NodePtr& f, const std::map& map, - boost::function op) { + /*********************************************************************************/ + template + template + typename DecisionTree::NodePtr DecisionTree::convert( + const typename DecisionTree::NodePtr& f, const std::map& map, + boost::function op) { - typedef DecisionTree MX; - typedef typename MX::Leaf MXLeaf; - typedef typename MX::Choice MXChoice; - typedef typename MX::NodePtr MXNodePtr; - typedef DecisionTree LY; + typedef DecisionTree MX; + typedef typename MX::Leaf MXLeaf; + typedef typename MX::Choice MXChoice; + typedef typename MX::NodePtr MXNodePtr; + typedef DecisionTree LY; - // ugliness below because apparently we can't have templated virtual functions - // If leaf, apply unary conversion "op" and create a unique leaf - const MXLeaf* leaf = dynamic_cast (f.get()); - if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); + // ugliness below because apparently we can't have templated virtual functions + // If leaf, apply unary conversion "op" and create a unique leaf + const MXLeaf* leaf = dynamic_cast (f.get()); + if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); - // Check if Choice - boost::shared_ptr choice = boost::dynamic_pointer_cast (f); - if (!choice) throw std::invalid_argument( - "DecisionTree::Convert: Invalid NodePtr"); + // Check if Choice + boost::shared_ptr choice = boost::dynamic_pointer_cast (f); + if (!choice) throw std::invalid_argument( + "DecisionTree::Convert: Invalid NodePtr"); - // get new label - M oldLabel = choice->label(); - L newLabel = map.at(oldLabel); + // get new label + M oldLabel = choice->label(); + L newLabel = map.at(oldLabel); - // put together via Shannon expansion otherwise not sorted. - std::vector functions; - BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) { - LY converted(convert(branch, map, op)); - functions += converted; - } - return LY::compose(functions.begin(), functions.end(), newLabel); - } + // put together via Shannon expansion otherwise not sorted. + std::vector functions; + BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) { + LY converted(convert(branch, map, op)); + functions += converted; + } + return LY::compose(functions.begin(), functions.end(), newLabel); + } - /*********************************************************************************/ - template - bool DecisionTree::equals(const DecisionTree& other, double tol) const { - return root_->equals(*other.root_, tol); - } + /*********************************************************************************/ + template + bool DecisionTree::equals(const DecisionTree& other, double tol) const { + return root_->equals(*other.root_, tol); + } - template - void DecisionTree::print(const std::string& s) const { - root_->print(s); - } + template + void DecisionTree::print(const std::string& s) const { + root_->print(s); + } - template - bool DecisionTree::operator==(const DecisionTree& other) const { - return root_->equals(*other.root_); - } + template + bool DecisionTree::operator==(const DecisionTree& other) const { + return root_->equals(*other.root_); + } - template - const Y& DecisionTree::operator()(const Assignment& x) const { - return root_->operator ()(x); - } + template + const Y& DecisionTree::operator()(const Assignment& x) const { + return root_->operator ()(x); + } - template - DecisionTree DecisionTree::apply(const Unary& op) const { - return DecisionTree(root_->apply(op)); - } + template + DecisionTree DecisionTree::apply(const Unary& op) const { + return DecisionTree(root_->apply(op)); + } - /*********************************************************************************/ - template - DecisionTree DecisionTree::apply(const DecisionTree& g, - const Binary& op) const { - // apply the operaton on the root of both diagrams + /*********************************************************************************/ + template + DecisionTree DecisionTree::apply(const DecisionTree& g, + const Binary& op) const { + // apply the operaton on the root of both diagrams NodePtr h = root_->apply_f_op_g(*g.root_, op); // create a new class with the resulting root "h" - DecisionTree result(h); - return result; - } + DecisionTree result(h); + return result; + } - /*********************************************************************************/ - // The way this works: - // We have an ADT, picture it as a tree. - // At a certain depth, we have a branch on "label". - // The function "choose(label,index)" will return a tree of one less depth, - // where there is no more branch on "label": only the subtree under that - // branch point corresponding to the value "index" is left instead. - // The function below get all these smaller trees and "ops" them together. - template - DecisionTree DecisionTree::combine(const L& label, - size_t cardinality, const Binary& op) const { - DecisionTree result = choose(label, 0); - for (size_t index = 1; index < cardinality; index++) { - DecisionTree chosen = choose(label, index); - result = result.apply(chosen, op); - } - return result; - } + /*********************************************************************************/ + // The way this works: + // We have an ADT, picture it as a tree. + // At a certain depth, we have a branch on "label". + // The function "choose(label,index)" will return a tree of one less depth, + // where there is no more branch on "label": only the subtree under that + // branch point corresponding to the value "index" is left instead. + // The function below get all these smaller trees and "ops" them together. + template + DecisionTree DecisionTree::combine(const L& label, + size_t cardinality, const Binary& op) const { + DecisionTree result = choose(label, 0); + for (size_t index = 1; index < cardinality; index++) { + DecisionTree chosen = choose(label, index); + result = result.apply(chosen, op); + } + return result; + } - /*********************************************************************************/ - template - void DecisionTree::dot(std::ostream& os, bool showZero) const { - os << "digraph G {\n"; - root_->dot(os, showZero); - os << " [ordering=out]}" << std::endl; - } + /*********************************************************************************/ + template + void DecisionTree::dot(std::ostream& os, bool showZero) const { + os << "digraph G {\n"; + root_->dot(os, showZero); + os << " [ordering=out]}" << std::endl; + } - template - void DecisionTree::dot(const std::string& name, bool showZero) const { - std::ofstream os((name + ".dot").c_str()); - dot(os, showZero); - system( - ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); - } + template + void DecisionTree::dot(const std::string& name, bool showZero) const { + std::ofstream os((name + ".dot").c_str()); + dot(os, showZero); + system( + ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); + } /*********************************************************************************/ diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 8fabd6e82..d579ef5de 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #pragma once @@ -27,203 +27,203 @@ namespace gtsam { - /** - * Algebraic Decision Trees - * L = label for variables - * Y = function range (any algebra), e.g., bool, int, double - */ - template - class DecisionTree { + /** + * Algebraic Decision Trees + * L = label for variables + * Y = function range (any algebra), e.g., bool, int, double + */ + template + class DecisionTree { - public: + public: - /** Handy typedefs for unary and binary function types */ - typedef boost::function Unary; - typedef boost::function Binary; + /** Handy typedefs for unary and binary function types */ + typedef boost::function Unary; + typedef boost::function Binary; - /** A label annotated with cardinality */ - typedef std::pair LabelC; + /** A label annotated with cardinality */ + typedef std::pair LabelC; - /** DD's consist of Leaf and Choice nodes, both subclasses of Node */ - class Leaf; - class Choice; + /** DD's consist of Leaf and Choice nodes, both subclasses of Node */ + class Leaf; + class Choice; - /** ------------------------ Node base class --------------------------- */ - class Node { - public: - typedef boost::shared_ptr Ptr; + /** ------------------------ Node base class --------------------------- */ + class Node { + public: + typedef boost::shared_ptr Ptr; #ifdef DT_DEBUG_MEMORY - static int nrNodes; + static int nrNodes; #endif - // Constructor - Node() { + // Constructor + Node() { #ifdef DT_DEBUG_MEMORY - std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); + std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); #endif - } + } - // Destructor - virtual ~Node() { + // Destructor + virtual ~Node() { #ifdef DT_DEBUG_MEMORY - std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); + std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); #endif - } + } - // Unique ID for dot files - const void* id() const { return this; } + // Unique ID for dot files + const void* id() const { return this; } - // everything else is virtual, no documentation here as internal - virtual void print(const std::string& s = "") const = 0; - virtual void dot(std::ostream& os, bool showZero) const = 0; - virtual bool sameLeaf(const Leaf& q) const = 0; - virtual bool sameLeaf(const Node& q) const = 0; - virtual bool equals(const Node& other, double tol = 1e-9) const = 0; - virtual const Y& operator()(const Assignment& x) const = 0; - virtual Ptr apply(const Unary& op) const = 0; - virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; - virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; - virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; - virtual Ptr choose(const L& label, size_t index) const = 0; - virtual bool isLeaf() const = 0; - }; - /** ------------------------ Node base class --------------------------- */ + // everything else is virtual, no documentation here as internal + virtual void print(const std::string& s = "") const = 0; + virtual void dot(std::ostream& os, bool showZero) const = 0; + virtual bool sameLeaf(const Leaf& q) const = 0; + virtual bool sameLeaf(const Node& q) const = 0; + virtual bool equals(const Node& other, double tol = 1e-9) const = 0; + virtual const Y& operator()(const Assignment& x) const = 0; + virtual Ptr apply(const Unary& op) const = 0; + virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; + virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; + virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; + virtual Ptr choose(const L& label, size_t index) const = 0; + virtual bool isLeaf() const = 0; + }; + /** ------------------------ Node base class --------------------------- */ - public: + public: - /** A function is a shared pointer to the root of an ADD */ - typedef typename Node::Ptr NodePtr; + /** A function is a shared pointer to the root of an ADD */ + typedef typename Node::Ptr NodePtr; - /* an AlgebraicDecisionTree just contains the root */ - NodePtr root_; + /* an AlgebraicDecisionTree just contains the root */ + NodePtr root_; - protected: + protected: - /** Internal recursive function to create from keys, cardinalities, and Y values */ - template - NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; + /** Internal recursive function to create from keys, cardinalities, and Y values */ + template + NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; - /** Convert to a different type */ - template NodePtr - convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); + /** Convert to a different type */ + template NodePtr + convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); - /** Default constructor */ - DecisionTree(); + /** Default constructor */ + DecisionTree(); - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** Create a constant */ - DecisionTree(const Y& y); + /** Create a constant */ + DecisionTree(const Y& y); - /** Create a new leaf function splitting on a variable */ - DecisionTree(const L& label, const Y& y1, const Y& y2); + /** Create a new leaf function splitting on a variable */ + DecisionTree(const L& label, const Y& y1, const Y& y2); - /** Allow Label+Cardinality for convenience */ - DecisionTree(const LabelC& label, const Y& y1, const Y& y2); + /** Allow Label+Cardinality for convenience */ + DecisionTree(const LabelC& label, const Y& y1, const Y& y2); - /** Create from keys and string table */ - DecisionTree(const std::vector& labelCs, const std::vector& ys); + /** Create from keys and string table */ + DecisionTree(const std::vector& labelCs, const std::vector& ys); - /** Create from keys and string table */ - DecisionTree(const std::vector& labelCs, const std::string& table); + /** Create from keys and string table */ + DecisionTree(const std::vector& labelCs, const std::string& table); - /** Create DecisionTree from others */ - template - DecisionTree(Iterator begin, Iterator end, const L& label); + /** Create DecisionTree from others */ + template + DecisionTree(Iterator begin, Iterator end, const L& label); - /** Create DecisionTree from others others (binary version) */ - DecisionTree(const L& label, // - const DecisionTree& f0, const DecisionTree& f1); + /** Create DecisionTree from others others (binary version) */ + DecisionTree(const L& label, // + const DecisionTree& f0, const DecisionTree& f1); - /** Convert from a different type */ - template - DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op); + /** Convert from a different type */ + template + DecisionTree(const DecisionTree& other, + const std::map& map, boost::function op); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - /** GTSAM-style print */ - void print(const std::string& s = "DecisionTree") const; + /** GTSAM-style print */ + void print(const std::string& s = "DecisionTree") const; - // Testable - bool equals(const DecisionTree& other, double tol = 1e-9) const; + // Testable + bool equals(const DecisionTree& other, double tol = 1e-9) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /** Make virtual */ - virtual ~DecisionTree() { - } + /** Make virtual */ + virtual ~DecisionTree() { + } - /** equality */ - bool operator==(const DecisionTree& q) const; + /** equality */ + bool operator==(const DecisionTree& q) const; - /** evaluate */ - const Y& operator()(const Assignment& x) const; + /** evaluate */ + const Y& operator()(const Assignment& x) const; - /** apply Unary operation "op" to f */ - DecisionTree apply(const Unary& op) const; + /** apply Unary operation "op" to f */ + DecisionTree apply(const Unary& op) const; - /** apply binary operation "op" to f and g */ - DecisionTree apply(const DecisionTree& g, const Binary& op) const; + /** apply binary operation "op" to f and g */ + DecisionTree apply(const DecisionTree& g, const Binary& op) const; - /** create a new function where value(label)==index */ - DecisionTree choose(const L& label, size_t index) const { - NodePtr newRoot = root_->choose(label, index); - return DecisionTree(newRoot); - } + /** create a new function where value(label)==index */ + DecisionTree choose(const L& label, size_t index) const { + NodePtr newRoot = root_->choose(label, index); + return DecisionTree(newRoot); + } - /** combine subtrees on key with binary operation "op" */ - DecisionTree combine(const L& label, size_t cardinality, const Binary& op) const; + /** combine subtrees on key with binary operation "op" */ + DecisionTree combine(const L& label, size_t cardinality, const Binary& op) const; - /** combine with LabelC for convenience */ - DecisionTree combine(const LabelC& labelC, const Binary& op) const { - return combine(labelC.first, labelC.second, op); - } + /** combine with LabelC for convenience */ + DecisionTree combine(const LabelC& labelC, const Binary& op) const { + return combine(labelC.first, labelC.second, op); + } - /** output to graphviz format, stream version */ - void dot(std::ostream& os, bool showZero = true) const; + /** output to graphviz format, stream version */ + void dot(std::ostream& os, bool showZero = true) const; - /** output to graphviz format, open a file */ - void dot(const std::string& name, bool showZero = true) const; + /** output to graphviz format, open a file */ + void dot(const std::string& name, bool showZero = true) const; - /// @name Advanced Interface - /// @{ + /// @name Advanced Interface + /// @{ - // internal use only - DecisionTree(const NodePtr& root); + // internal use only + DecisionTree(const NodePtr& root); - // internal use only - template NodePtr - compose(Iterator begin, Iterator end, const L& label) const; + // internal use only + template NodePtr + compose(Iterator begin, Iterator end, const L& label) const; - /// @} + /// @} - }; // DecisionTree + }; // DecisionTree - /** free versions of apply */ + /** free versions of apply */ - template - DecisionTree apply(const DecisionTree& f, - const typename DecisionTree::Unary& op) { - return f.apply(op); - } + template + DecisionTree apply(const DecisionTree& f, + const typename DecisionTree::Unary& op) { + return f.apply(op); + } - template - DecisionTree apply(const DecisionTree& f, - const DecisionTree& g, - const typename DecisionTree::Binary& op) { - return f.apply(g, op); - } + template + DecisionTree apply(const DecisionTree& f, + const DecisionTree& g, + const typename DecisionTree::Binary& op) { + return f.apply(g, op); + } } // namespace gtsam diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index bb83cec16..8c3e2e7b6 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -28,76 +28,76 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DecisionTreeFactor::DecisionTreeFactor() { - } + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor() { + } - /* ******************************************************************************** */ - DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, - const ADT& potentials) : - DiscreteFactor(keys.indices()), Potentials(keys, potentials) { - } + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, + const ADT& potentials) : + DiscreteFactor(keys.indices()), Potentials(keys, potentials) { + } - /* *************************************************************************/ - DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) : - DiscreteFactor(c.keys()), Potentials(c) { - } + /* *************************************************************************/ + DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) : + DiscreteFactor(c.keys()), Potentials(c) { + } - /* ************************************************************************* */ - bool DecisionTreeFactor::equals(const This& other, double tol) const { - return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); - } + /* ************************************************************************* */ + bool DecisionTreeFactor::equals(const This& other, double tol) const { + return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); + } - /* ************************************************************************* */ - void DecisionTreeFactor::print(const string& s, - const IndexFormatter& formatter) const { - cout << s; - IndexFactor::print("IndexFactor:",formatter); - Potentials::print("Potentials:",formatter); - } + /* ************************************************************************* */ + void DecisionTreeFactor::print(const string& s, + const IndexFormatter& formatter) const { + cout << s; + IndexFactor::print("IndexFactor:",formatter); + Potentials::print("Potentials:",formatter); + } - /* ************************************************************************* */ - DecisionTreeFactor DecisionTreeFactor::apply // - (const DecisionTreeFactor& f, ADT::Binary op) const { - map cs; // new cardinalities - // make unique key-cardinality map - BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j); - BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j); - // Convert map into keys - DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, cs) - keys.push_back(key); - // apply operand - ADT result = ADT::apply(f, op); - // Make a new factor - return DecisionTreeFactor(keys, result); - } + /* ************************************************************************* */ + DecisionTreeFactor DecisionTreeFactor::apply // + (const DecisionTreeFactor& f, ADT::Binary op) const { + map cs; // new cardinalities + // make unique key-cardinality map + BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j); + BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j); + // Convert map into keys + DiscreteKeys keys; + BOOST_FOREACH(const DiscreteKey& key, cs) + keys.push_back(key); + // apply operand + ADT result = ADT::apply(f, op); + // Make a new factor + return DecisionTreeFactor(keys, result); + } - /* ************************************************************************* */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine // - (size_t nrFrontals, ADT::Binary op) const { + /* ************************************************************************* */ + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine // + (size_t nrFrontals, ADT::Binary op) const { - if (nrFrontals == 0 || nrFrontals > size()) throw invalid_argument( - (boost::format( - "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d") - % nrFrontals % size()).str()); + if (nrFrontals == 0 || nrFrontals > size()) throw invalid_argument( + (boost::format( + "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d") + % nrFrontals % size()).str()); - // sum over nrFrontals keys - size_t i; - ADT result(*this); - for (i = 0; i < nrFrontals; i++) { - Index j = keys()[i]; - result = result.combine(j, cardinality(j), op); - } + // sum over nrFrontals keys + size_t i; + ADT result(*this); + for (i = 0; i < nrFrontals; i++) { + Index j = keys()[i]; + result = result.combine(j, cardinality(j), op); + } - // create new factor, note we start keys after nrFrontals - DiscreteKeys dkeys; - for (; i < keys().size(); i++) { - Index j = keys()[i]; - dkeys.push_back(DiscreteKey(j,cardinality(j))); - } - return boost::make_shared(dkeys, result); - } + // create new factor, note we start keys after nrFrontals + DiscreteKeys dkeys; + for (; i < keys().size(); i++) { + Index j = keys()[i]; + dkeys.push_back(DiscreteKey(j,cardinality(j))); + } + return boost::make_shared(dkeys, result); + } /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f75c658a2..01d410ef0 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -30,126 +30,126 @@ namespace gtsam { - class DiscreteConditional; + class DiscreteConditional; - /** - * A discrete probabilistic factor - */ - class DecisionTreeFactor: public DiscreteFactor, public Potentials { + /** + * A discrete probabilistic factor + */ + class DecisionTreeFactor: public DiscreteFactor, public Potentials { - public: + public: - // typedefs needed to play nice with gtsam - typedef DecisionTreeFactor This; - typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + // typedefs needed to play nice with gtsam + typedef DecisionTreeFactor This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** Default constructor for I/O */ - DecisionTreeFactor(); + /** Default constructor for I/O */ + DecisionTreeFactor(); - /** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */ - DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials); + /** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */ + DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials); - /** Constructor from Indices and (string or doubles) */ - template - DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) : - DiscreteFactor(keys.indices()), Potentials(keys, table) { - } + /** Constructor from Indices and (string or doubles) */ + template + DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) : + DiscreteFactor(keys.indices()), Potentials(keys, table) { + } - /** Construct from a DiscreteConditional type */ - DecisionTreeFactor(const DiscreteConditional& c); + /** Construct from a DiscreteConditional type */ + DecisionTreeFactor(const DiscreteConditional& c); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - /// equality - bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; + /// equality + bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; - // print - virtual void print(const std::string& s = "DecisionTreeFactor:\n", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + // print + virtual void print(const std::string& s = "DecisionTreeFactor:\n", + const IndexFormatter& formatter = DefaultIndexFormatter) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /// Value is just look up in AlgebraicDecisonTree - virtual double operator()(const Values& values) const { - return Potentials::operator()(values); - } + /// Value is just look up in AlgebraicDecisonTree + virtual double operator()(const Values& values) const { + return Potentials::operator()(values); + } - /// multiply two factors - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { - return apply(f, ADT::Ring::mul); - } + /// multiply two factors + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { + return apply(f, ADT::Ring::mul); + } - /// divide by factor f (safely) - DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { - return apply(f, safe_div); - } + /// divide by factor f (safely) + DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { + return apply(f, safe_div); + } - /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const { - return *this; - } + /// Convert into a decisiontree + virtual DecisionTreeFactor toDecisionTreeFactor() const { + return *this; + } - /// Create new factor by summing all values with the same separator values - shared_ptr sum(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::add); - } + /// Create new factor by summing all values with the same separator values + shared_ptr sum(size_t nrFrontals) const { + return combine(nrFrontals, ADT::Ring::add); + } - /// Create new factor by maximizing over all values with the same separator values - shared_ptr max(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::max); - } + /// Create new factor by maximizing over all values with the same separator values + shared_ptr max(size_t nrFrontals) const { + return combine(nrFrontals, ADT::Ring::max); + } - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /** - * Apply binary operator (*this) "op" f - * @param f the second argument for op - * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials - */ - DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; + /** + * Apply binary operator (*this) "op" f + * @param f the second argument for op + * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials + */ + DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; - /** - * Combine frontal variables using binary operator "op" - * @param nrFrontals nr. of frontal to combine variables in this factor - * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials - * @return shared pointer to newly created DecisionTreeFactor - */ - shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; + /** + * Combine frontal variables using binary operator "op" + * @param nrFrontals nr. of frontal to combine variables in this factor + * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials + * @return shared pointer to newly created DecisionTreeFactor + */ + shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; - /** - * @brief Permutes the keys in Potentials and DiscreteFactor - * - * This re-implements the permuteWithInverse() in both Potentials - * and DiscreteFactor by doing both of them together. - */ + /** + * @brief Permutes the keys in Potentials and DiscreteFactor + * + * This re-implements the permuteWithInverse() in both Potentials + * and DiscreteFactor by doing both of them together. + */ - void permuteWithInverse(const Permutation& inversePermutation){ - DiscreteFactor::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } + void permuteWithInverse(const Permutation& inversePermutation){ + DiscreteFactor::permuteWithInverse(inversePermutation); + Potentials::permuteWithInverse(inversePermutation); + } - /** - * Apply a reduction, which is a remapping of variable indices. - */ + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - DiscreteFactor::reduceWithInverse(inverseReduction); + DiscreteFactor::reduceWithInverse(inverseReduction); Potentials::reduceWithInverse(inverseReduction); } - /// @} - }; + /// @} + }; // DecisionTreeFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 1d21d0a4b..98a61f7bd 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -23,18 +23,18 @@ namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class BayesNet ; + // Explicitly instantiate so we don't have to include everywhere + template class BayesNet ; - /* ************************************************************************* */ - void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_front(boost::make_shared(s)); - } + /* ************************************************************************* */ + void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { + bayesNet.push_front(boost::make_shared(s)); + } - /* ************************************************************************* */ - void add(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_back(boost::make_shared(s)); - } + /* ************************************************************************* */ + void add(DiscreteBayesNet& bayesNet, const Signature& s) { + bayesNet.push_back(boost::make_shared(s)); + } /* ************************************************************************* */ double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values) { @@ -45,23 +45,23 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { - // solve each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) - conditional->solveInPlace(*result); - return result; - } + /* ************************************************************************* */ + DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { + // solve each node in turn in topological sort order (parents first) + DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) + conditional->solveInPlace(*result); + return result; + } - /* ************************************************************************* */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { - // sample each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) - conditional->sampleInPlace(*result); - return result; - } + /* ************************************************************************* */ + DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { + // sample each node in turn in topological sort order (parents first) + DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + conditional->sampleInPlace(*result); + return result; + } /* ************************************************************************* */ } // namespace diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index c8286f3a9..1e96f9919 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -25,22 +25,22 @@ namespace gtsam { - typedef BayesNet DiscreteBayesNet; + typedef BayesNet DiscreteBayesNet; - /** Add a DiscreteCondtional */ - void add(DiscreteBayesNet&, const Signature& s); + /** Add a DiscreteCondtional */ + void add(DiscreteBayesNet&, const Signature& s); - /** Add a DiscreteCondtional in front, when listing parents first*/ - void add_front(DiscreteBayesNet&, const Signature& s); + /** Add a DiscreteCondtional in front, when listing parents first*/ + void add_front(DiscreteBayesNet&, const Signature& s); - //** evaluate for given Values */ + //** evaluate for given Values */ double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values); /** Optimize function for back-substitution. */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); + DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); - /** Do ancestral sampling */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); + /** Do ancestral sampling */ + DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); } // namespace diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index b42e3ba24..2e03e5478 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -34,28 +34,28 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const size_t nrFrontals, - const DecisionTreeFactor& f) : - IndexConditional(f.keys(), nrFrontals), Potentials( - f / (*f.sum(nrFrontals))) { - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const size_t nrFrontals, + const DecisionTreeFactor& f) : + IndexConditional(f.keys(), nrFrontals), Potentials( + f / (*f.sum(nrFrontals))) { + } - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal) : - IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { -// assert(nrFrontals() == 1); - if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout - << (firstFrontalKey()) << endl; //TODO Print all keys - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal) : + IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { +// assert(nrFrontals() == 1); + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout + << (firstFrontalKey()) << endl; //TODO Print all keys + } - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const Signature& signature) : - IndexConditional(signature.indices(), 1), Potentials( - signature.discreteKeysParentsFirst(), signature.cpt()) { - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const Signature& signature) : + IndexConditional(signature.indices(), 1), Potentials( + signature.discreteKeysParentsFirst(), signature.cpt()) { + } /* ******************************************************************************** */ void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { @@ -70,145 +70,145 @@ namespace gtsam { && Potentials::equals(other, tol); } - /* ******************************************************************************** */ - Potentials::ADT DiscreteConditional::choose( - const Values& parentsValues) const { - ADT pFS(*this); - BOOST_FOREACH(Index key, parents()) - try { - Index j = (key); - size_t value = parentsValues.at(j); - pFS = pFS.choose(j, value); - } catch (exception&) { - throw runtime_error( - "DiscreteConditional::choose: parent value missing"); - }; - return pFS; - } + /* ******************************************************************************** */ + Potentials::ADT DiscreteConditional::choose( + const Values& parentsValues) const { + ADT pFS(*this); + BOOST_FOREACH(Index key, parents()) + try { + Index j = (key); + size_t value = parentsValues.at(j); + pFS = pFS.choose(j, value); + } catch (exception&) { + throw runtime_error( + "DiscreteConditional::choose: parent value missing"); + }; + return pFS; + } - /* ******************************************************************************** */ - void DiscreteConditional::solveInPlace(Values& values) const { -// OLD -// assert(nrFrontals() == 1); -// Index j = (firstFrontalKey()); -// size_t mpe = solve(values); // Solve for variable -// values[j] = mpe; // store result in partial solution -// OLD + /* ******************************************************************************** */ + void DiscreteConditional::solveInPlace(Values& values) const { +// OLD +// assert(nrFrontals() == 1); +// Index j = (firstFrontalKey()); +// size_t mpe = solve(values); // Solve for variable +// values[j] = mpe; // store result in partial solution +// OLD - // TODO: is this really the fastest way? I think it is. + // TODO: is this really the fastest way? I think it is. - //The following is to make make adjustment for nFrontals \neq 1 - ADT pFS = choose(values); // P(F|S=parentsValues) + //The following is to make make adjustment for nFrontals \neq 1 + ADT pFS = choose(values); // P(F|S=parentsValues) - // Initialize - Values mpe; - double maxP = 0; + // Initialize + Values mpe; + double maxP = 0; - DiscreteKeys keys; - BOOST_FOREACH(Index idx, frontals()) { - DiscreteKey dk(idx,cardinality(idx)); - keys & dk; - } - // Get all Possible Configurations - vector allPosbValues = cartesianProduct(keys); + DiscreteKeys keys; + BOOST_FOREACH(Index idx, frontals()) { + DiscreteKey dk(idx,cardinality(idx)); + keys & dk; + } + // Get all Possible Configurations + vector allPosbValues = cartesianProduct(keys); - // Find the MPE - BOOST_FOREACH(Values& frontalVals, allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } + // Find the MPE + BOOST_FOREACH(Values& frontalVals, allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } - //set values (inPlace) to mpe - BOOST_FOREACH(Index j, frontals()) { - values[j] = mpe[j]; - } - } + //set values (inPlace) to mpe + BOOST_FOREACH(Index j, frontals()) { + values[j] = mpe[j]; + } + } - /* ******************************************************************************** */ - void DiscreteConditional::sampleInPlace(Values& values) const { - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t sampled = sample(values); // Sample variable - values[j] = sampled; // store result in partial solution - } + /* ******************************************************************************** */ + void DiscreteConditional::sampleInPlace(Values& values) const { + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + size_t sampled = sample(values); // Sample variable + values[j] = sampled; // store result in partial solution + } - /* ******************************************************************************** */ - size_t DiscreteConditional::solve(const Values& parentsValues) const { + /* ******************************************************************************** */ + size_t DiscreteConditional::solve(const Values& parentsValues) const { - // TODO: is this really the fastest way? I think it is. - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + // TODO: is this really the fastest way? I think it is. + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - // Then, find the max over all remaining - // TODO, only works for one key now, seems horribly slow this way - size_t mpe = 0; - Values frontals; - double maxP = 0; - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = value; - } - } - return mpe; - } + // Then, find the max over all remaining + // TODO, only works for one key now, seems horribly slow this way + size_t mpe = 0; + Values frontals; + double maxP = 0; + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + for (size_t value = 0; value < cardinality(j); value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = value; + } + } + return mpe; + } - /* ******************************************************************************** */ - size_t DiscreteConditional::sample(const Values& parentsValues) const { + /* ******************************************************************************** */ + size_t DiscreteConditional::sample(const Values& parentsValues) const { - using boost::uniform_real; - static boost::mt19937 gen(2); // random number generator + using boost::uniform_real; + static boost::mt19937 gen(2); // random number generator - bool debug = ISDEBUG("DiscreteConditional::sample"); + bool debug = ISDEBUG("DiscreteConditional::sample"); - // Get the correct conditional density - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - if (debug) GTSAM_PRINT(pFS); + // Get the correct conditional density + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + if (debug) GTSAM_PRINT(pFS); - // get cumulative distribution function (cdf) - // TODO, only works for one key now, seems horribly slow this way - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t nj = cardinality(j); - vector cdf(nj); - Values frontals; - double sum = 0; - for (size_t value = 0; value < nj; value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - sum += pValueS; // accumulate - if (debug) cout << sum << " "; - if (pValueS == 1) { - if (debug) cout << "--> " << value << endl; - return value; // shortcut exit - } - cdf[value] = sum; - } + // get cumulative distribution function (cdf) + // TODO, only works for one key now, seems horribly slow this way + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + size_t nj = cardinality(j); + vector cdf(nj); + Values frontals; + double sum = 0; + for (size_t value = 0; value < nj; value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + sum += pValueS; // accumulate + if (debug) cout << sum << " "; + if (pValueS == 1) { + if (debug) cout << "--> " << value << endl; + return value; // shortcut exit + } + cdf[value] = sum; + } - // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html - uniform_real<> dist(0, cdf.back()); - boost::variate_generator > die(gen, dist); - size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); - if (debug) cout << "-> " << sampled << endl; + // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html + uniform_real<> dist(0, cdf.back()); + boost::variate_generator > die(gen, dist); + size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); + if (debug) cout << "-> " << sampled << endl; - return sampled; + return sampled; - return 0; - } + return 0; + } - /* ******************************************************************************** */ - void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ - IndexConditional::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } + /* ******************************************************************************** */ + void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ + IndexConditional::permuteWithInverse(inversePermutation); + Potentials::permuteWithInverse(inversePermutation); + } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index a11bdca45..7f5c99c62 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -26,52 +26,52 @@ namespace gtsam { - /** - * Discrete Conditional Density - * Derives from DecisionTreeFactor - */ - class DiscreteConditional: public IndexConditional, public Potentials { + /** + * Discrete Conditional Density + * Derives from DecisionTreeFactor + */ + class DiscreteConditional: public IndexConditional, public Potentials { - public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor FactorType; - typedef boost::shared_ptr shared_ptr; - typedef IndexConditional Base; + public: + // typedefs needed to play nice with gtsam + typedef DiscreteFactor FactorType; + typedef boost::shared_ptr shared_ptr; + typedef IndexConditional Base; - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** default constructor needed for serialization */ - DiscreteConditional() { - } + /** default constructor needed for serialization */ + DiscreteConditional() { + } - /** constructor from factor */ - DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); + /** constructor from factor */ + DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); - /** Construct from signature */ - DiscreteConditional(const Signature& signature); + /** Construct from signature */ + DiscreteConditional(const Signature& signature); - /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ - DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal); + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal); - /** - * Combine several conditional into a single one. - * The conditionals must be given in increasing order, meaning that the parents - * of any conditional may not include a conditional coming before it. - * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. - * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. - * */ - template - static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /** + * Combine several conditional into a single one. + * The conditionals must be given in increasing order, meaning that the parents + * of any conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. + * */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ /// GTSAM-style print void print(const std::string& s = "Discrete Conditional: ", @@ -80,74 +80,74 @@ namespace gtsam { /// GTSAM-style equals bool equals(const DiscreteConditional& other, double tol = 1e-9) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ /// Evaluate, just look up in AlgebraicDecisonTree virtual double operator()(const Values& values) const { return Potentials::operator()(values); } - /** Convert to a factor */ - DecisionTreeFactor::shared_ptr toFactor() const { - return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); - } + /** Convert to a factor */ + DecisionTreeFactor::shared_ptr toFactor() const { + return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); + } - /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ - ADT choose(const Assignment& parentsValues) const; + /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ + ADT choose(const Assignment& parentsValues) const; - /** - * solve a conditional - * @param parentsValues Known values of the parents - * @return MPE value of the child (1 frontal variable). - */ - size_t solve(const Values& parentsValues) const; + /** + * solve a conditional + * @param parentsValues Known values of the parents + * @return MPE value of the child (1 frontal variable). + */ + size_t solve(const Values& parentsValues) const; - /** - * sample - * @param parentsValues Known values of the parents - * @return sample from conditional - */ - size_t sample(const Values& parentsValues) const; + /** + * sample + * @param parentsValues Known values of the parents + * @return sample from conditional + */ + size_t sample(const Values& parentsValues) const; - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /// solve a conditional, in place - void solveInPlace(Values& parentsValues) const; + /// solve a conditional, in place + void solveInPlace(Values& parentsValues) const; - /// sample in place, stores result in partial solution - void sampleInPlace(Values& parentsValues) const; + /// sample in place, stores result in partial solution + void sampleInPlace(Values& parentsValues) const; - /** - * Permutes both IndexConditional and Potentials. - */ - void permuteWithInverse(const Permutation& inversePermutation); + /** + * Permutes both IndexConditional and Potentials. + */ + void permuteWithInverse(const Permutation& inversePermutation); - /// @} + /// @} - }; + }; // DiscreteConditional - /* ************************************************************************* */ - template - DiscreteConditional::shared_ptr DiscreteConditional::Combine( - ITERATOR firstConditional, ITERATOR lastConditional) { - // TODO: check for being a clique + /* ************************************************************************* */ + template + DiscreteConditional::shared_ptr DiscreteConditional::Combine( + ITERATOR firstConditional, ITERATOR lastConditional) { + // TODO: check for being a clique - // multiply all the potentials of the given conditionals - size_t nrFrontals = 0; - DecisionTreeFactor product; - for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { - DiscreteConditional::shared_ptr c = *it; - DecisionTreeFactor::shared_ptr factor = c->toFactor(); - product = (*factor) * product; - } - // and then create a new multi-frontal conditional - return boost::make_shared(nrFrontals,product); - } + // multiply all the potentials of the given conditionals + size_t nrFrontals = 0; + DecisionTreeFactor product; + for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { + DiscreteConditional::shared_ptr c = *it; + DecisionTreeFactor::shared_ptr factor = c->toFactor(); + product = (*factor) * product; + } + // and then create a new multi-frontal conditional + return boost::make_shared(nrFrontals,product); + } }// gtsam diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index caec9788f..1d53f1afd 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -24,9 +24,9 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteFactor::DiscreteFactor() { - } + /* ******************************************************************************** */ + DiscreteFactor::DiscreteFactor() { + } /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 2f411997c..499979eaf 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -23,100 +23,100 @@ namespace gtsam { - class DecisionTreeFactor; - class DiscreteConditional; + class DecisionTreeFactor; + class DiscreteConditional; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor - */ - class DiscreteFactor: public IndexFactor { + /** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ + class DiscreteFactor: public IndexFactor { - public: + public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor This; - typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + // typedefs needed to play nice with gtsam + typedef DiscreteFactor This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - protected: + protected: - /// Construct n-way factor - DiscreteFactor(const std::vector& js) : - IndexFactor(js) { - } + /// Construct n-way factor + DiscreteFactor(const std::vector& js) : + IndexFactor(js) { + } - /// Construct unary factor - DiscreteFactor(Index j) : - IndexFactor(j) { - } + /// Construct unary factor + DiscreteFactor(Index j) : + IndexFactor(j) { + } - /// Construct binary factor - DiscreteFactor(Index j1, Index j2) : - IndexFactor(j1, j2) { - } + /// Construct binary factor + DiscreteFactor(Index j1, Index j2) : + IndexFactor(j1, j2) { + } - /// construct from container - template - DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : - IndexFactor(beginKey, endKey) { - } + /// construct from container + template + DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : + IndexFactor(beginKey, endKey) { + } - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /// Default constructor for I/O - DiscreteFactor(); + /// Default constructor for I/O + DiscreteFactor(); - /// Virtual destructor - virtual ~DiscreteFactor() {} + /// Virtual destructor + virtual ~DiscreteFactor() {} - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - // print - virtual void print(const std::string& s = "DiscreteFactor\n", - const IndexFormatter& formatter - =DefaultIndexFormatter) const { - IndexFactor::print(s,formatter); - } + // print + virtual void print(const std::string& s = "DiscreteFactor\n", + const IndexFormatter& formatter + =DefaultIndexFormatter) const { + IndexFactor::print(s,formatter); + } - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /// Find value for given assignment of values to variables - virtual double operator()(const Values&) const = 0; + /// Find value for given assignment of values to variables + virtual double operator()(const Values&) const = 0; - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor - virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; + /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; - virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; + virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation){ - IndexFactor::permuteWithInverse(inversePermutation); - } + /** + * Permutes the factor, but for efficiency requires the permutation + * to already be inverted. + */ + virtual void permuteWithInverse(const Permutation& inversePermutation){ + IndexFactor::permuteWithInverse(inversePermutation); + } - /** - * Apply a reduction, which is a remapping of variable indices. - */ + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactor::reduceWithInverse(inverseReduction); + IndexFactor::reduceWithInverse(inverseReduction); } - /// @} - }; + /// @} + }; // DiscreteFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 467611c5d..ab9cd1de2 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -24,56 +24,56 @@ namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class FactorGraph ; - template class EliminationTree ; + // Explicitly instantiate so we don't have to include everywhere + template class FactorGraph ; + template class EliminationTree ; - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph() { - } + /* ************************************************************************* */ + DiscreteFactorGraph::DiscreteFactorGraph() { + } - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph( - const BayesNet& bayesNet) : - FactorGraph(bayesNet) { - } + /* ************************************************************************* */ + DiscreteFactorGraph::DiscreteFactorGraph( + const BayesNet& bayesNet) : + FactorGraph(bayesNet) { + } - /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); - return keys; - } + /* ************************************************************************* */ + FastSet DiscreteFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } - /* ************************************************************************* */ - DecisionTreeFactor DiscreteFactorGraph::product() const { - DecisionTreeFactor result; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) result = (*factor) * result; - return result; - } + /* ************************************************************************* */ + DecisionTreeFactor DiscreteFactorGraph::product() const { + DecisionTreeFactor result; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) result = (*factor) * result; + return result; + } - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteFactor::Values &values) const { - double product = 1.0; - BOOST_FOREACH( const sharedFactor& factor, factors_ ) - product *= (*factor)(values); - return product; - } + /* ************************************************************************* */ + double DiscreteFactorGraph::operator()( + const DiscreteFactor::Values &values) const { + double product = 1.0; + BOOST_FOREACH( const sharedFactor& factor, factors_ ) + product *= (*factor)(values); + 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); - } - } + /* ************************************************************************* */ + 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); + } + } /* ************************************************************************* */ void DiscreteFactorGraph::permuteWithInverse( @@ -93,32 +93,32 @@ namespace gtsam { } } - /* ************************************************************************* */ - std::pair // - EliminateDiscrete(const FactorGraph& factors, size_t num) { + /* ************************************************************************* */ + std::pair // + EliminateDiscrete(const FactorGraph& factors, size_t num) { - // PRODUCT: multiply all factors - tic(1, "product"); - DecisionTreeFactor product; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors){ - product = (*factor) * product; - } + // PRODUCT: multiply all factors + tic(1, "product"); + DecisionTreeFactor product; + BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors){ + product = (*factor) * product; + } - toc(1, "product"); + toc(1, "product"); - // sum out frontals, this is the factor on the separator - tic(2, "sum"); - DecisionTreeFactor::shared_ptr sum = product.sum(num); - toc(2, "sum"); + // sum out frontals, this is the factor on the separator + tic(2, "sum"); + DecisionTreeFactor::shared_ptr sum = product.sum(num); + toc(2, "sum"); - // now divide product/sum to get conditional - tic(3, "divide"); - DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); - toc(3, "divide"); - tictoc_finishedIteration(); + // now divide product/sum to get conditional + tic(3, "divide"); + DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); + toc(3, "divide"); + tictoc_finishedIteration(); - return std::make_pair(cond, sum); - } + return std::make_pair(cond, sum); + } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 323adca7b..f70f0840e 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -29,56 +29,56 @@ namespace gtsam { class DiscreteFactorGraph: public FactorGraph { public: - /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef std::vector Indices; + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - /** Construct empty factor graph */ - DiscreteFactorGraph(); + /** Construct empty factor graph */ + DiscreteFactorGraph(); - /** Constructor from a factor graph of GaussianFactor or a derived type */ - template - DiscreteFactorGraph(const FactorGraph& fg) { - push_back(fg); - } + /** Constructor from a factor graph of GaussianFactor or a derived type */ + template + DiscreteFactorGraph(const FactorGraph& fg) { + push_back(fg); + } - /** construct from a BayesNet */ - DiscreteFactorGraph(const BayesNet& bayesNet); + /** construct from a BayesNet */ + DiscreteFactorGraph(const BayesNet& bayesNet); - template - void add(const DiscreteKey& j, SOURCE table) { - DiscreteKeys keys; - keys.push_back(j); - push_back(boost::make_shared(keys, table)); - } + template + void add(const DiscreteKey& j, SOURCE table) { + DiscreteKeys keys; + keys.push_back(j); + push_back(boost::make_shared(keys, table)); + } - template - void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { - DiscreteKeys keys; - keys.push_back(j1); - keys.push_back(j2); - push_back(boost::make_shared(keys, table)); - } + template + void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { + DiscreteKeys keys; + keys.push_back(j1); + keys.push_back(j2); + push_back(boost::make_shared(keys, table)); + } - /** add shared discreteFactor immediately from arguments */ - template - void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(keys, table)); - } + /** add shared discreteFactor immediately from arguments */ + template + void add(const DiscreteKeys& keys, SOURCE table) { + push_back(boost::make_shared(keys, table)); + } - /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + /** Return the set of variables involved in the factors (set union) */ + FastSet keys() const; - /** return product of all factors as a single factor */ - DecisionTreeFactor product() const; + /** return product of all factors as a single factor */ + DecisionTreeFactor product() const; - /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ - double operator()(const DiscreteFactor::Values & values) const; + /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ + double operator()(const DiscreteFactor::Values & values) const; - /// print - void print(const std::string& s = "DiscreteFactorGraph", - const IndexFormatter& formatter =DefaultIndexFormatter) const; + /// print + void print(const std::string& s = "DiscreteFactorGraph", + const IndexFormatter& formatter =DefaultIndexFormatter) const; /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); @@ -91,6 +91,6 @@ public: /** Main elimination function for DiscreteFactorGraph */ std::pair, DecisionTreeFactor::shared_ptr> EliminateDiscrete(const FactorGraph& factors, - size_t nrFrontals = 1); + size_t nrFrontals = 1); } // namespace gtsam diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index 9db8bee79..df9f5c3d9 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -23,33 +23,33 @@ namespace gtsam { - using namespace std; + using namespace std; - DiscreteKeys::DiscreteKeys(const vector& cs) { - for (size_t i = 0; i < cs.size(); i++) { - string name = boost::str(boost::format("v%1%") % i); - push_back(DiscreteKey(i, cs[i])); - } - } + DiscreteKeys::DiscreteKeys(const vector& cs) { + for (size_t i = 0; i < cs.size(); i++) { + string name = boost::str(boost::format("v%1%") % i); + push_back(DiscreteKey(i, cs[i])); + } + } - vector DiscreteKeys::indices() const { - vector < Index > js; - BOOST_FOREACH(const DiscreteKey& key, *this) - js.push_back(key.first); - return js; - } + vector DiscreteKeys::indices() const { + vector < Index > js; + BOOST_FOREACH(const DiscreteKey& key, *this) + js.push_back(key.first); + return js; + } - map DiscreteKeys::cardinalities() const { - map cs; - cs.insert(begin(),end()); -// BOOST_FOREACH(const DiscreteKey& key, *this) -// cs.insert(key); - return cs; - } + map DiscreteKeys::cardinalities() const { + map cs; + cs.insert(begin(),end()); +// BOOST_FOREACH(const DiscreteKey& key, *this) +// cs.insert(key); + return cs; + } - DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2) { - DiscreteKeys keys(key1); - return keys & key2; - } + DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2) { + DiscreteKeys keys(key1); + return keys & key2; + } } diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 488240286..96afba934 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -26,45 +26,45 @@ namespace gtsam { - /** - * Key type for discrete conditionals - * Includes name and cardinality - */ - typedef std::pair DiscreteKey; + /** + * Key type for discrete conditionals + * Includes name and cardinality + */ + typedef std::pair DiscreteKey; - /// DiscreteKeys is a set of keys that can be assembled using the & operator - struct DiscreteKeys: public std::vector { + /// DiscreteKeys is a set of keys that can be assembled using the & operator + struct DiscreteKeys: public std::vector { - /// Default constructor - DiscreteKeys() { - } + /// Default constructor + DiscreteKeys() { + } - /// Construct from a key - DiscreteKeys(const DiscreteKey& key) { - push_back(key); - } + /// Construct from a key + DiscreteKeys(const DiscreteKey& key) { + push_back(key); + } - /// Construct from a vector of keys - DiscreteKeys(const std::vector& keys) : - std::vector(keys) { - } + /// Construct from a vector of keys + DiscreteKeys(const std::vector& keys) : + std::vector(keys) { + } - /// Construct from cardinalities with default names - DiscreteKeys(const std::vector& cs); + /// Construct from cardinalities with default names + DiscreteKeys(const std::vector& cs); - /// Return a vector of indices - std::vector indices() const; + /// Return a vector of indices + std::vector indices() const; - /// Return a map from index to cardinality - std::map cardinalities() const; + /// Return a map from index to cardinality + std::map cardinalities() const; - /// Add a key (non-const!) - DiscreteKeys& operator&(const DiscreteKey& key) { - push_back(key); - return *this; - } - }; // DiscreteKeys + /// Add a key (non-const!) + DiscreteKeys& operator&(const DiscreteKey& key) { + push_back(key); + return *this; + } + }; // DiscreteKeys - /// Create a list from two keys - DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); + /// Create a list from two keys + DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); } diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index ff07e5a6c..411e4a4dd 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -36,41 +36,41 @@ namespace gtsam { public: - /** Construct a marginals class. - * @param graph The factor graph defining the full joint density on all variables. - */ - DiscreteMarginals(const DiscreteFactorGraph& graph) { - typedef JunctionTree DiscreteJT; - GenericMultifrontalSolver solver(graph); - bayesTree_ = *solver.eliminate(&EliminateDiscrete); - } + /** Construct a marginals class. + * @param graph The factor graph defining the full joint density on all variables. + */ + DiscreteMarginals(const DiscreteFactorGraph& graph) { + typedef JunctionTree DiscreteJT; + GenericMultifrontalSolver solver(graph); + bayesTree_ = *solver.eliminate(&EliminateDiscrete); + } - /** Compute the marginal of a single variable */ - DiscreteFactor::shared_ptr operator()(Index variable) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(variable, &EliminateDiscrete); - return marginalFactor; - } + /** Compute the marginal of a single variable */ + DiscreteFactor::shared_ptr operator()(Index variable) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = bayesTree_.marginalFactor(variable, &EliminateDiscrete); + return marginalFactor; + } - /** Compute the marginal of a single variable - * @param key DiscreteKey of the Variable - * @return Vector of marginal probabilities - */ - Vector marginalProbabilities(const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(key.first, &EliminateDiscrete); + /** Compute the marginal of a single variable + * @param key DiscreteKey of the Variable + * @return Vector of marginal probabilities + */ + Vector marginalProbabilities(const DiscreteKey& key) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = bayesTree_.marginalFactor(key.first, &EliminateDiscrete); - //Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second ; ++ state) { - DiscreteFactor::Values values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } + //Create result + Vector vResult(key.second); + for (size_t state = 0; state < key.second ; ++ state) { + DiscreteFactor::Values values; + values[key.first] = state; + vResult(state) = (*marginalFactor)(values); + } + return vResult; + } }; diff --git a/gtsam/discrete/DiscreteSequentialSolver.cpp b/gtsam/discrete/DiscreteSequentialSolver.cpp index e4bc502c8..f01cd72bb 100644 --- a/gtsam/discrete/DiscreteSequentialSolver.cpp +++ b/gtsam/discrete/DiscreteSequentialSolver.cpp @@ -23,52 +23,52 @@ namespace gtsam { - template class GenericSequentialSolver ; + template class GenericSequentialSolver ; - /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { + /* ************************************************************************* */ + DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { - static const bool debug = false; + static const bool debug = false; - if (debug) this->factors_->print("DiscreteSequentialSolver, eliminating "); - if (debug) this->eliminationTree_->print( - "DiscreteSequentialSolver, elimination tree "); + if (debug) this->factors_->print("DiscreteSequentialSolver, eliminating "); + if (debug) this->eliminationTree_->print( + "DiscreteSequentialSolver, elimination tree "); - // Eliminate using the elimination tree - tic(1, "eliminate"); - DiscreteBayesNet::shared_ptr bayesNet = eliminate(); - toc(1, "eliminate"); + // Eliminate using the elimination tree + tic(1, "eliminate"); + DiscreteBayesNet::shared_ptr bayesNet = eliminate(); + toc(1, "eliminate"); - if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); + if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); - // Allocate the solution vector if it is not already allocated + // Allocate the solution vector if it is not already allocated - // Back-substitute - tic(2, "optimize"); - DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); - toc(2, "optimize"); + // Back-substitute + tic(2, "optimize"); + DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); + toc(2, "optimize"); - if (debug) solution->print("DiscreteSequentialSolver, solution "); + if (debug) solution->print("DiscreteSequentialSolver, solution "); - return solution; - } + return solution; + } - /* ************************************************************************* */ - Vector DiscreteSequentialSolver::marginalProbabilities( - const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = Base::marginalFactor(key.first, &EliminateDiscrete); + /* ************************************************************************* */ + Vector DiscreteSequentialSolver::marginalProbabilities( + const DiscreteKey& key) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = Base::marginalFactor(key.first, &EliminateDiscrete); - //Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second; ++state) { - DiscreteFactor::Values values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } + //Create result + Vector vResult(key.second); + for (size_t state = 0; state < key.second; ++state) { + DiscreteFactor::Values values; + values[key.first] = state; + vResult(state) = (*marginalFactor)(values); + } + return vResult; + } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteSequentialSolver.h b/gtsam/discrete/DiscreteSequentialSolver.h index 6adb32ef6..fa1279bc8 100644 --- a/gtsam/discrete/DiscreteSequentialSolver.h +++ b/gtsam/discrete/DiscreteSequentialSolver.h @@ -23,91 +23,91 @@ #include namespace gtsam { - // The base class provides all of the needed functionality + // The base class provides all of the needed functionality - class DiscreteSequentialSolver: public GenericSequentialSolver { + class DiscreteSequentialSolver: public GenericSequentialSolver { - protected: - typedef GenericSequentialSolver Base; - typedef boost::shared_ptr shared_ptr; + protected: + typedef GenericSequentialSolver Base; + typedef boost::shared_ptr shared_ptr; - public: + public: - /** - * The problem we are trying to solve (SUM or MPE). - */ - typedef enum { - BEL, // Belief updating (or conditional updating) - MPE, // Most-Probable-Explanation - MAP - // Maximum A Posteriori hypothesis - } ProblemType; + /** + * The problem we are trying to solve (SUM or MPE). + */ + typedef enum { + BEL, // Belief updating (or conditional updating) + MPE, // Most-Probable-Explanation + MAP + // Maximum A Posteriori hypothesis + } ProblemType; - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - DiscreteSequentialSolver(const FactorGraph& factorGraph) : - Base(factorGraph) { - } + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the work of elimination. + */ + DiscreteSequentialSolver(const FactorGraph& factorGraph) : + Base(factorGraph) { + } - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - DiscreteSequentialSolver( - const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : - Base(factorGraph, variableIndex) { - } + /** + * Construct the solver with a shared pointer to a factor graph and to a + * VariableIndex. The solver will store these pointers, so this constructor + * is the fastest. + */ + DiscreteSequentialSolver( + const FactorGraph::shared_ptr& factorGraph, + const VariableIndex::shared_ptr& variableIndex) : + Base(factorGraph, variableIndex) { + } - const EliminationTree& eliminationTree() const { - return *eliminationTree_; - } + const EliminationTree& eliminationTree() const { + return *eliminationTree_; + } - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - BayesNet::shared_ptr eliminate() const { - return Base::eliminate(&EliminateDiscrete); - } + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + BayesNet::shared_ptr eliminate() const { + return Base::eliminate(&EliminateDiscrete); + } - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - */ - DiscreteFactorGraph::shared_ptr jointFactorGraph( - const std::vector& js) const { - DiscreteFactorGraph::shared_ptr results(new DiscreteFactorGraph( - *Base::jointFactorGraph(js, &EliminateDiscrete))); - return results; - } + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a factor + * graph. + */ + DiscreteFactorGraph::shared_ptr jointFactorGraph( + const std::vector& js) const { + DiscreteFactorGraph::shared_ptr results(new DiscreteFactorGraph( + *Base::jointFactorGraph(js, &EliminateDiscrete))); + return results; + } - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - DiscreteFactor::shared_ptr marginalFactor(Index j) const { - return Base::marginalFactor(j, &EliminateDiscrete); - } + /** + * Compute the marginal density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + DiscreteFactor::shared_ptr marginalFactor(Index j) const { + return Base::marginalFactor(j, &EliminateDiscrete); + } - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a - * Vector of the probability values. - */ - Vector marginalProbabilities(const DiscreteKey& key) const; + /** + * Compute the marginal density over a variable, by integrating out + * all of the other variables. This function returns the result as a + * Vector of the probability values. + */ + Vector marginalProbabilities(const DiscreteKey& key) const; - /** - * Compute the MPE solution of the DiscreteFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - DiscreteFactor::sharedValues optimize() const; + /** + * Compute the MPE solution of the DiscreteFactorGraph. This + * eliminates to create a BayesNet and then back-substitutes this BayesNet to + * obtain the solution. + */ + DiscreteFactor::sharedValues optimize() const; - }; + }; } // gtsam diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index cf903a3a2..c8fb3a11d 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -23,42 +23,42 @@ using namespace std; namespace gtsam { - // explicit instantiation - template class DecisionTree ; - template class AlgebraicDecisionTree ; + // explicit instantiation + template class DecisionTree ; + template class AlgebraicDecisionTree ; - /* ************************************************************************* */ - double Potentials::safe_div(const double& a, const double& b) { - // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b)); - // The use for safe_div is when we divide the product factor by the sum factor. - // If the product or sum is zero, we accord zero probability to the event. - return (a == 0 || b == 0) ? 0 : (a / b); - } + /* ************************************************************************* */ + double Potentials::safe_div(const double& a, const double& b) { + // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b)); + // The use for safe_div is when we divide the product factor by the sum factor. + // If the product or sum is zero, we accord zero probability to the event. + return (a == 0 || b == 0) ? 0 : (a / b); + } - /* ******************************************************************************** */ - Potentials::Potentials() : - ADT(1.0) { - } + /* ******************************************************************************** */ + Potentials::Potentials() : + ADT(1.0) { + } - /* ******************************************************************************** */ - Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) : - ADT(decisionTree), cardinalities_(keys.cardinalities()) { - } + /* ******************************************************************************** */ + Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) : + ADT(decisionTree), cardinalities_(keys.cardinalities()) { + } - /* ************************************************************************* */ - bool Potentials::equals(const Potentials& other, double tol) const { - return ADT::equals(other, tol); - } + /* ************************************************************************* */ + bool Potentials::equals(const Potentials& other, double tol) const { + return ADT::equals(other, tol); + } - /* ************************************************************************* */ - void Potentials::print(const string& s, - const IndexFormatter& formatter) const { - cout << s << "\n Cardinalities: "; - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - cout << formatter(key.first) << "=" << formatter(key.second) << " "; - cout << endl; - ADT::print(" "); - } + /* ************************************************************************* */ + void Potentials::print(const string& s, + const IndexFormatter& formatter) const { + cout << s << "\n Cardinalities: "; + BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + cout << formatter(key.first) << "=" << formatter(key.second) << " "; + cout << endl; + ADT::print(" "); + } /* ************************************************************************* */ template @@ -83,16 +83,16 @@ namespace gtsam { cardinalities_ = keys.cardinalities(); } - /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& inversePermutation) { + /* ************************************************************************* */ + void Potentials::permuteWithInverse(const Permutation& inversePermutation) { remapIndices(inversePermutation); - } + } /* ************************************************************************* */ void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { remapIndices(inverseReduction); } - /* ************************************************************************* */ + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 5d8ace371..f2ba655b0 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -27,67 +27,67 @@ namespace gtsam { - /** - * A base class for both DiscreteFactor and DiscreteConditional - */ - class Potentials: public AlgebraicDecisionTree { + /** + * A base class for both DiscreteFactor and DiscreteConditional + */ + class Potentials: public AlgebraicDecisionTree { - public: + public: - typedef AlgebraicDecisionTree ADT; + typedef AlgebraicDecisionTree ADT; - protected: + protected: - /// Cardinality for each key, used in combine - std::map cardinalities_; + /// Cardinality for each key, used in combine + std::map cardinalities_; - /** Constructor from ColumnIndex, and ADT */ - Potentials(const ADT& potentials) : - ADT(potentials) { - } + /** Constructor from ColumnIndex, and ADT */ + Potentials(const ADT& potentials) : + ADT(potentials) { + } - // Safe division for probabilities - static double safe_div(const double& a, const double& b); + // Safe division for probabilities + static double safe_div(const double& a, const double& b); // Apply either a permutation or a reduction template void remapIndices(const P& remapping); - public: + public: - /** Default constructor for I/O */ - Potentials(); + /** Default constructor for I/O */ + Potentials(); - /** Constructor from Indices and ADT */ - Potentials(const DiscreteKeys& keys, const ADT& decisionTree); + /** Constructor from Indices and ADT */ + Potentials(const DiscreteKeys& keys, const ADT& decisionTree); - /** Constructor from Indices and (string or doubles) */ - template - Potentials(const DiscreteKeys& keys, SOURCE table) : - ADT(keys, table), cardinalities_(keys.cardinalities()) { - } + /** Constructor from Indices and (string or doubles) */ + template + Potentials(const DiscreteKeys& keys, SOURCE table) : + ADT(keys, table), cardinalities_(keys.cardinalities()) { + } - // Testable - bool equals(const Potentials& other, double tol = 1e-9) const; - void print(const std::string& s = "Potentials: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + // Testable + bool equals(const Potentials& other, double tol = 1e-9) const; + void print(const std::string& s = "Potentials: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; - size_t cardinality(Index j) const { return cardinalities_.at(j);} - - /** - * @brief Permutes the keys in Potentials - * - * This permutes the Indices and performs necessary re-ordering of ADD. - * This is virtual so that derived types e.g. DecisionTreeFactor can - * re-implement it. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation); + size_t cardinality(Index j) const { return cardinalities_.at(j);} /** - * Apply a reduction, which is a remapping of variable indices. - */ + * @brief Permutes the keys in Potentials + * + * This permutes the Indices and performs necessary re-ordering of ADD. + * This is virtual so that derived types e.g. DecisionTreeFactor can + * re-implement it. + */ + virtual void permuteWithInverse(const Permutation& inversePermutation); + + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction); - }; // Potentials + }; // Potentials } // namespace gtsam diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 76ccad9db..006c851eb 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -26,197 +26,197 @@ namespace gtsam { - using namespace std; + using namespace std; - namespace qi = boost::spirit::qi; + namespace qi = boost::spirit::qi; namespace ph = boost::phoenix; - // parser for strings of form "99/1 80/20" etc... - namespace parser { - typedef string::const_iterator It; - using boost::phoenix::val; - using boost::phoenix::ref; - using boost::phoenix::push_back; + // parser for strings of form "99/1 80/20" etc... + namespace parser { + typedef string::const_iterator It; + using boost::phoenix::val; + using boost::phoenix::ref; + using boost::phoenix::push_back; - // Special rows, true and false - Signature::Row createF() { - Signature::Row r(2); - r[0] = 1; - r[1] = 0; - return r; - } - Signature::Row createT() { - Signature::Row r(2); - r[0] = 0; - r[1] = 1; - return r; - } - Signature::Row T = createT(), F = createF(); + // Special rows, true and false + Signature::Row createF() { + Signature::Row r(2); + r[0] = 1; + r[1] = 0; + return r; + } + Signature::Row createT() { + Signature::Row r(2); + r[0] = 0; + r[1] = 1; + return r; + } + Signature::Row T = createT(), F = createF(); - // Special tables (inefficient, but do we care for user input?) - Signature::Table logic(bool ff, bool ft, bool tf, bool tt) { - Signature::Table t(4); - t[0] = ff ? T : F; - t[1] = ft ? T : F; - t[2] = tf ? T : F; - t[3] = tt ? T : F; - return t; - } + // Special tables (inefficient, but do we care for user input?) + Signature::Table logic(bool ff, bool ft, bool tf, bool tt) { + Signature::Table t(4); + t[0] = ff ? T : F; + t[1] = ft ? T : F; + t[2] = tf ? T : F; + t[3] = tt ? T : F; + return t; + } - struct Grammar { - qi::rule table, or_, and_, rows; - qi::rule true_, false_, row; - Grammar() { - table = or_ | and_ | rows; - or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)]; - and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)]; - rows = +(row | true_ | false_); // only loads first of the rows under boost 1.42 - row = qi::double_ >> +("/" >> qi::double_); - true_ = qi::lit("T")[qi::_val = T]; - false_ = qi::lit("F")[qi::_val = F]; - } - } grammar; + struct Grammar { + qi::rule table, or_, and_, rows; + qi::rule true_, false_, row; + Grammar() { + table = or_ | and_ | rows; + or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)]; + and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)]; + rows = +(row | true_ | false_); // only loads first of the rows under boost 1.42 + row = qi::double_ >> +("/" >> qi::double_); + true_ = qi::lit("T")[qi::_val = T]; + false_ = qi::lit("F")[qi::_val = F]; + } + } grammar; - // Create simpler parsing function to avoid the issue of only parsing a single row - bool parse_table(const string& spec, Signature::Table& table) { - // check for OR, AND on whole phrase - It f = spec.begin(), l = spec.end(); - if (qi::parse(f, l, - qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) || - qi::parse(f, l, - qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)])) - return true; + // Create simpler parsing function to avoid the issue of only parsing a single row + bool parse_table(const string& spec, Signature::Table& table) { + // check for OR, AND on whole phrase + It f = spec.begin(), l = spec.end(); + if (qi::parse(f, l, + qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) || + qi::parse(f, l, + qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)])) + return true; - // tokenize into separate rows - istringstream iss(spec); - string token; - while (iss >> token) { - Signature::Row values; - It tf = token.begin(), tl = token.end(); - bool r = qi::parse(tf, tl, - qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) | - qi::lit("T")[ph::ref(values) = T] | - qi::lit("F")[ph::ref(values) = F] ); - if (!r) - return false; - table.push_back(values); - } + // tokenize into separate rows + istringstream iss(spec); + string token; + while (iss >> token) { + Signature::Row values; + It tf = token.begin(), tl = token.end(); + bool r = qi::parse(tf, tl, + qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) | + qi::lit("T")[ph::ref(values) = T] | + qi::lit("F")[ph::ref(values) = F] ); + if (!r) + return false; + table.push_back(values); + } - return true; - } - } // \namespace parser + return true; + } + } // \namespace parser - ostream& operator <<(ostream &os, const Signature::Row &row) { - os << row[0]; - for (size_t i = 1; i < row.size(); i++) - os << " " << row[i]; - return os; - } + ostream& operator <<(ostream &os, const Signature::Row &row) { + os << row[0]; + for (size_t i = 1; i < row.size(); i++) + os << " " << row[i]; + return os; + } - ostream& operator <<(ostream &os, const Signature::Table &table) { - for (size_t i = 0; i < table.size(); i++) - os << table[i] << endl; - return os; - } + ostream& operator <<(ostream &os, const Signature::Table &table) { + for (size_t i = 0; i < table.size(); i++) + os << table[i] << endl; + return os; + } - Signature::Signature(const DiscreteKey& key) : - key_(key) { - } + Signature::Signature(const DiscreteKey& key) : + key_(key) { + } - DiscreteKeys Signature::discreteKeysParentsFirst() const { - DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, parents_) - keys.push_back(key); - keys.push_back(key_); - return keys; - } + DiscreteKeys Signature::discreteKeysParentsFirst() const { + DiscreteKeys keys; + BOOST_FOREACH(const DiscreteKey& key, parents_) + keys.push_back(key); + keys.push_back(key_); + return keys; + } - vector Signature::indices() const { - vector js; - js.push_back(key_.first); - BOOST_FOREACH(const DiscreteKey& key, parents_) - js.push_back(key.first); - return js; - } + vector Signature::indices() const { + vector js; + js.push_back(key_.first); + BOOST_FOREACH(const DiscreteKey& key, parents_) + js.push_back(key.first); + return js; + } - vector Signature::cpt() const { - vector cpt; - if (table_) { - BOOST_FOREACH(const Row& row, *table_) - BOOST_FOREACH(const double& x, row) - cpt.push_back(x); - } - return cpt; - } + vector Signature::cpt() const { + vector cpt; + if (table_) { + BOOST_FOREACH(const Row& row, *table_) + BOOST_FOREACH(const double& x, row) + cpt.push_back(x); + } + return cpt; + } - Signature& Signature::operator,(const DiscreteKey& parent) { - parents_.push_back(parent); - return *this; - } + Signature& Signature::operator,(const DiscreteKey& parent) { + parents_.push_back(parent); + return *this; + } - static void normalize(Signature::Row& row) { - double sum = 0; - for (size_t i = 0; i < row.size(); i++) - sum += row[i]; - for (size_t i = 0; i < row.size(); i++) - row[i] /= sum; - } + static void normalize(Signature::Row& row) { + double sum = 0; + for (size_t i = 0; i < row.size(); i++) + sum += row[i]; + for (size_t i = 0; i < row.size(); i++) + row[i] /= sum; + } - Signature& Signature::operator=(const string& spec) { - spec_.reset(spec); - Table table; - // NOTE: using simpler parse function to ensure boost back compatibility -// parser::It f = spec.begin(), l = spec.end(); - bool success = // -// qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar - parser::parse_table(spec, table); - if (success) { - BOOST_FOREACH(Row& row, table) - normalize(row); - table_.reset(table); - } - return *this; - } + Signature& Signature::operator=(const string& spec) { + spec_.reset(spec); + Table table; + // NOTE: using simpler parse function to ensure boost back compatibility +// parser::It f = spec.begin(), l = spec.end(); + bool success = // +// qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar + parser::parse_table(spec, table); + if (success) { + BOOST_FOREACH(Row& row, table) + normalize(row); + table_.reset(table); + } + return *this; + } - Signature& Signature::operator=(const Table& t) { - Table table = t; - BOOST_FOREACH(Row& row, table) - normalize(row); - table_.reset(table); - return *this; - } + Signature& Signature::operator=(const Table& t) { + Table table = t; + BOOST_FOREACH(Row& row, table) + normalize(row); + table_.reset(table); + return *this; + } - ostream& operator <<(ostream &os, const Signature &s) { - os << s.key_.first; - if (s.parents_.empty()) { - os << " % "; - } else { - os << " | " << s.parents_[0].first; - for (size_t i = 1; i < s.parents_.size(); i++) - os << " && " << s.parents_[i].first; - os << " = "; - } - os << (s.spec_ ? *s.spec_ : "no spec") << endl; - if (s.table_) - os << (*s.table_); - else - os << "spec could not be parsed" << endl; - return os; - } + ostream& operator <<(ostream &os, const Signature &s) { + os << s.key_.first; + if (s.parents_.empty()) { + os << " % "; + } else { + os << " | " << s.parents_[0].first; + for (size_t i = 1; i < s.parents_.size(); i++) + os << " && " << s.parents_[i].first; + os << " = "; + } + os << (s.spec_ ? *s.spec_ : "no spec") << endl; + if (s.table_) + os << (*s.table_); + else + os << "spec could not be parsed" << endl; + return os; + } - Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) { - Signature s(key); - return s, parent; - } + Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) { + Signature s(key); + return s, parent; + } - Signature operator%(const DiscreteKey& key, const string& parent) { - Signature s(key); - return s = parent; - } + Signature operator%(const DiscreteKey& key, const string& parent) { + Signature s(key); + return s = parent; + } - Signature operator%(const DiscreteKey& key, const Signature::Table& parent) { - Signature s(key); - return s = parent; - } + Signature operator%(const DiscreteKey& key, const Signature::Table& parent) { + Signature s(key); + return s = parent; + } } // namespace gtsam diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 937bd6e1b..0b3d0879d 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -24,112 +24,112 @@ namespace gtsam { - /** - * Signature for a discrete conditional density, used to construct conditionals. - * - * The format is (Key % string) for nodes with no parents, - * and (Key | Key, Key = string) for nodes with parents. - * - * The string specifies a conditional probability spec in the 00 01 10 11 order. - * For three-valued, it would be 00 01 02 10 11 12 20 21 22, etc... - * - * For example, given the following keys - * - * Key A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), - * B("Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea"); - * - * These are all valid signatures (Asia network example): - * - * A % "99/1" - * S % "50/50" - * T|A = "99/1 95/5" - * L|S = "99/1 90/10" - * B|S = "70/30 40/60" - * E|T,L = "F F F 1" - * X|E = "95/5 2/98" - * D|E,B = "9/1 2/8 3/7 1/9" - */ - class Signature { + /** + * Signature for a discrete conditional density, used to construct conditionals. + * + * The format is (Key % string) for nodes with no parents, + * and (Key | Key, Key = string) for nodes with parents. + * + * The string specifies a conditional probability spec in the 00 01 10 11 order. + * For three-valued, it would be 00 01 02 10 11 12 20 21 22, etc... + * + * For example, given the following keys + * + * Key A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), + * B("Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea"); + * + * These are all valid signatures (Asia network example): + * + * A % "99/1" + * S % "50/50" + * T|A = "99/1 95/5" + * L|S = "99/1 90/10" + * B|S = "70/30 40/60" + * E|T,L = "F F F 1" + * X|E = "95/5 2/98" + * D|E,B = "9/1 2/8 3/7 1/9" + */ + class Signature { - public: + public: - /** Data type for the CPT */ - typedef std::vector Row; - typedef std::vector Table; + /** Data type for the CPT */ + typedef std::vector Row; + typedef std::vector Table; - private: + private: - /** the variable key */ - DiscreteKey key_; + /** the variable key */ + DiscreteKey key_; - /** the parent keys */ - DiscreteKeys parents_; + /** the parent keys */ + DiscreteKeys parents_; - // the given CPT specification string - boost::optional spec_; + // the given CPT specification string + boost::optional spec_; - // the CPT as parsed, if successful - boost::optional table_; + // the CPT as parsed, if successful + boost::optional
table_; - public: + public: - /** Constructor from DiscreteKey */ - Signature(const DiscreteKey& key); + /** Constructor from DiscreteKey */ + Signature(const DiscreteKey& key); - /** the variable key */ - const DiscreteKey& key() const { - return key_; - } + /** the variable key */ + const DiscreteKey& key() const { + return key_; + } - /** the parent keys */ - const DiscreteKeys& parents() const { - return parents_; - } + /** the parent keys */ + const DiscreteKeys& parents() const { + return parents_; + } - /** All keys, with variable key last */ - DiscreteKeys discreteKeysParentsFirst() const; + /** All keys, with variable key last */ + DiscreteKeys discreteKeysParentsFirst() const; - /** All key indices, with variable key first */ - std::vector indices() const; + /** All key indices, with variable key first */ + std::vector indices() const; - // the CPT as parsed, if successful - const boost::optional
& table() const { - return table_; - } + // the CPT as parsed, if successful + const boost::optional
& table() const { + return table_; + } - // the CPT as a vector of doubles, with key's values most rapidly changing - std::vector cpt() const; + // the CPT as a vector of doubles, with key's values most rapidly changing + std::vector cpt() const; - /** Add a parent */ - Signature& operator,(const DiscreteKey& parent); + /** Add a parent */ + Signature& operator,(const DiscreteKey& parent); - /** Add the CPT spec - Fails in boost 1.40 */ - Signature& operator=(const std::string& spec); + /** Add the CPT spec - Fails in boost 1.40 */ + Signature& operator=(const std::string& spec); - /** Add the CPT spec directly as a table */ - Signature& operator=(const Table& table); + /** Add the CPT spec directly as a table */ + Signature& operator=(const Table& table); - /** provide streaming */ - friend std::ostream& operator <<(std::ostream &os, const Signature &s); - }; + /** provide streaming */ + friend std::ostream& operator <<(std::ostream &os, const Signature &s); + }; - /** - * Helper function to create Signature objects - * example: Signature s = D | E; - */ - Signature operator|(const DiscreteKey& key, const DiscreteKey& parent); + /** + * Helper function to create Signature objects + * example: Signature s = D | E; + */ + Signature operator|(const DiscreteKey& key, const DiscreteKey& parent); - /** - * Helper function to create Signature objects - * example: Signature s(D % "99/1"); - * Uses string parser, which requires BOOST 1.42 or higher - */ - Signature operator%(const DiscreteKey& key, const std::string& parent); + /** + * Helper function to create Signature objects + * example: Signature s(D % "99/1"); + * Uses string parser, which requires BOOST 1.42 or higher + */ + Signature operator%(const DiscreteKey& key, const std::string& parent); - /** - * Helper function to create Signature objects, using table construction directly - * example: Signature s(D % table); - */ - Signature operator%(const DiscreteKey& key, const Signature::Table& parent); + /** + * Helper function to create Signature objects, using table construction directly + * example: Signature s(D % table); + */ + Signature operator%(const DiscreteKey& key, const Signature::Table& parent); } diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 8ab3b6a14..d0cd548a7 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /* - * @file testDecisionTree.cpp - * @brief Develop DecisionTree - * @author Frank Dellaert - * @date Mar 6, 2011 + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @date Mar 6, 2011 */ #include @@ -48,7 +48,7 @@ template class AlgebraicDecisionTree; template void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT - f.dot(filename); + f.dot(filename); #endif } @@ -76,54 +76,54 @@ void dot(const T&f, const string& filename) { size_t muls = 0, adds = 0; boost::timer timer; void resetCounts() { - muls = 0; - adds = 0; - timer.restart(); + muls = 0; + adds = 0; + timer.restart(); } void printCounts(const string& s) { #ifndef DISABLE_TIMING - cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds - % (1000 * timer.elapsed()) << endl; + cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds + % (1000 * timer.elapsed()) << endl; #endif - resetCounts(); + resetCounts(); } double mul(const double& a, const double& b) { - muls++; - return a * b; + muls++; + return a * b; } double add_(const double& a, const double& b) { - adds++; - return a + b; + adds++; + return a + b; } /* ******************************************************************************** */ // test ADT TEST(ADT, example3) { - // Create labels - DiscreteKey A(0,2), B(1,2), C(2,2), D(3,2), E(4,2); + // Create labels + DiscreteKey A(0,2), B(1,2), C(2,2), D(3,2), E(4,2); - // Literals - ADT a(A, 0.5, 0.5); - ADT notb(B, 1, 0); - ADT c(C, 0.1, 0.9); - ADT d(D, 0.1, 0.9); - ADT note(E, 0.9, 0.1); + // Literals + ADT a(A, 0.5, 0.5); + ADT notb(B, 1, 0); + ADT c(C, 0.1, 0.9); + ADT d(D, 0.1, 0.9); + ADT note(E, 0.9, 0.1); - ADT cnotb = c * notb; - dot(cnotb, "ADT-cnotb"); + ADT cnotb = c * notb; + dot(cnotb, "ADT-cnotb"); // a.print("a: "); // cnotb.print("cnotb: "); - ADT acnotb = a * cnotb; -// acnotb.print("acnotb: "); -// acnotb.printCache("acnotb Cache:"); + ADT acnotb = a * cnotb; +// acnotb.print("acnotb: "); +// acnotb.printCache("acnotb Cache:"); - dot(acnotb, "ADT-acnotb"); + dot(acnotb, "ADT-acnotb"); - ADT big = apply(apply(d, note, &mul), acnotb, &add_); - dot(big, "ADT-big"); + ADT big = apply(apply(d, note, &mul), acnotb, &add_); + dot(big, "ADT-big"); } /* ******************************************************************************** */ @@ -132,238 +132,238 @@ TEST(ADT, example3) /** Convert Signature into CPT */ ADT create(const Signature& signature) { - ADT p(signature.discreteKeysParentsFirst(), signature.cpt()); - static size_t count = 0; - const DiscreteKey& key = signature.key(); - string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); - dot(p, dotfile); - return p; + ADT p(signature.discreteKeysParentsFirst(), signature.cpt()); + static size_t count = 0; + const DiscreteKey& key = signature.key(); + string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); + dot(p, dotfile); + return p; } /* ************************************************************************* */ // test Asia Joint TEST(ADT, joint) { - DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2); + DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2); - resetCounts(); - ADT pA = create(A % "99/1"); - ADT pS = create(S % "50/50"); - ADT pT = create(T | A = "99/1 95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pB = create(B | S = "70/30 40/60"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); - printCounts("Asia CPTs"); + resetCounts(); + ADT pA = create(A % "99/1"); + ADT pS = create(S % "50/50"); + ADT pT = create(T | A = "99/1 95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pB = create(B | S = "70/30 40/60"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + printCounts("Asia CPTs"); - // Create joint - resetCounts(); - ADT joint = pA; - dot(joint, "Asia-A"); - joint = apply(joint, pS, &mul); - dot(joint, "Asia-AS"); - joint = apply(joint, pT, &mul); - dot(joint, "Asia-AST"); - joint = apply(joint, pL, &mul); - dot(joint, "Asia-ASTL"); - joint = apply(joint, pB, &mul); - dot(joint, "Asia-ASTLB"); - joint = apply(joint, pE, &mul); - dot(joint, "Asia-ASTLBE"); - joint = apply(joint, pX, &mul); - dot(joint, "Asia-ASTLBEX"); - joint = apply(joint, pD, &mul); - dot(joint, "Asia-ASTLBEXD"); - EXPECT_LONGS_EQUAL(346, muls); - printCounts("Asia joint"); + // Create joint + resetCounts(); + ADT joint = pA; + dot(joint, "Asia-A"); + joint = apply(joint, pS, &mul); + dot(joint, "Asia-AS"); + joint = apply(joint, pT, &mul); + dot(joint, "Asia-AST"); + joint = apply(joint, pL, &mul); + dot(joint, "Asia-ASTL"); + joint = apply(joint, pB, &mul); + dot(joint, "Asia-ASTLB"); + joint = apply(joint, pE, &mul); + dot(joint, "Asia-ASTLBE"); + joint = apply(joint, pX, &mul); + dot(joint, "Asia-ASTLBEX"); + joint = apply(joint, pD, &mul); + dot(joint, "Asia-ASTLBEXD"); + EXPECT_LONGS_EQUAL(346, muls); + printCounts("Asia joint"); - ADT pASTL = pA; - pASTL = apply(pASTL, pS, &mul); - pASTL = apply(pASTL, pT, &mul); - pASTL = apply(pASTL, pL, &mul); + ADT pASTL = pA; + pASTL = apply(pASTL, pS, &mul); + pASTL = apply(pASTL, pT, &mul); + pASTL = apply(pASTL, pL, &mul); - // test combine - ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_); - EXPECT(assert_equal(pA, fAa)); - ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_); - EXPECT(assert_equal(pA, fAb)); + // test combine + ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_); + EXPECT(assert_equal(pA, fAa)); + ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_); + EXPECT(assert_equal(pA, fAb)); } /* ************************************************************************* */ // test Inference with joint TEST(ADT, inference) { - DiscreteKey A(0,2), D(1,2),// - B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2); + DiscreteKey A(0,2), D(1,2),// + B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2); - resetCounts(); - ADT pA = create(A % "99/1"); - ADT pS = create(S % "50/50"); - ADT pT = create(T | A = "99/1 95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pB = create(B | S = "70/30 40/60"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); - // printCounts("Inference CPTs"); + resetCounts(); + ADT pA = create(A % "99/1"); + ADT pS = create(S % "50/50"); + ADT pT = create(T | A = "99/1 95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pB = create(B | S = "70/30 40/60"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + // printCounts("Inference CPTs"); - // Create joint - resetCounts(); - ADT joint = pA; - dot(joint, "Joint-Product-A"); - joint = apply(joint, pS, &mul); - dot(joint, "Joint-Product-AS"); - joint = apply(joint, pT, &mul); - dot(joint, "Joint-Product-AST"); - joint = apply(joint, pL, &mul); - dot(joint, "Joint-Product-ASTL"); - joint = apply(joint, pB, &mul); - dot(joint, "Joint-Product-ASTLB"); - joint = apply(joint, pE, &mul); - dot(joint, "Joint-Product-ASTLBE"); - joint = apply(joint, pX, &mul); - dot(joint, "Joint-Product-ASTLBEX"); - joint = apply(joint, pD, &mul); - dot(joint, "Joint-Product-ASTLBEXD"); - EXPECT_LONGS_EQUAL(370, muls); // different ordering - printCounts("Asia product"); + // Create joint + resetCounts(); + ADT joint = pA; + dot(joint, "Joint-Product-A"); + joint = apply(joint, pS, &mul); + dot(joint, "Joint-Product-AS"); + joint = apply(joint, pT, &mul); + dot(joint, "Joint-Product-AST"); + joint = apply(joint, pL, &mul); + dot(joint, "Joint-Product-ASTL"); + joint = apply(joint, pB, &mul); + dot(joint, "Joint-Product-ASTLB"); + joint = apply(joint, pE, &mul); + dot(joint, "Joint-Product-ASTLBE"); + joint = apply(joint, pX, &mul); + dot(joint, "Joint-Product-ASTLBEX"); + joint = apply(joint, pD, &mul); + dot(joint, "Joint-Product-ASTLBEXD"); + EXPECT_LONGS_EQUAL(370, muls); // different ordering + printCounts("Asia product"); - ADT marginal = joint; - marginal = marginal.combine(X, &add_); - dot(marginal, "Joint-Sum-ADBLEST"); - marginal = marginal.combine(T, &add_); - dot(marginal, "Joint-Sum-ADBLES"); - marginal = marginal.combine(S, &add_); - dot(marginal, "Joint-Sum-ADBLE"); - marginal = marginal.combine(E, &add_); - dot(marginal, "Joint-Sum-ADBL"); - EXPECT_LONGS_EQUAL(161, adds); - printCounts("Asia sum"); + ADT marginal = joint; + marginal = marginal.combine(X, &add_); + dot(marginal, "Joint-Sum-ADBLEST"); + marginal = marginal.combine(T, &add_); + dot(marginal, "Joint-Sum-ADBLES"); + marginal = marginal.combine(S, &add_); + dot(marginal, "Joint-Sum-ADBLE"); + marginal = marginal.combine(E, &add_); + dot(marginal, "Joint-Sum-ADBL"); + EXPECT_LONGS_EQUAL(161, adds); + printCounts("Asia sum"); } /* ************************************************************************* */ TEST(ADT, factor_graph) { - DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); + DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); - resetCounts(); - ADT pS = create(S % "50/50"); - ADT pT = create(T % "95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create(B | E = "1/8 7/9"); - ADT pB = create(B | S = "70/30 40/60"); - // printCounts("Create CPTs"); + resetCounts(); + ADT pS = create(S % "50/50"); + ADT pT = create(T % "95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create(B | E = "1/8 7/9"); + ADT pB = create(B | S = "70/30 40/60"); + // printCounts("Create CPTs"); - // Create joint - resetCounts(); - ADT fg = pS; - fg = apply(fg, pT, &mul); - fg = apply(fg, pL, &mul); - fg = apply(fg, pB, &mul); - fg = apply(fg, pE, &mul); - fg = apply(fg, pX, &mul); - fg = apply(fg, pD, &mul); - dot(fg, "FactorGraph"); - EXPECT_LONGS_EQUAL(158, muls); - printCounts("Asia FG"); + // Create joint + resetCounts(); + ADT fg = pS; + fg = apply(fg, pT, &mul); + fg = apply(fg, pL, &mul); + fg = apply(fg, pB, &mul); + fg = apply(fg, pE, &mul); + fg = apply(fg, pX, &mul); + fg = apply(fg, pD, &mul); + dot(fg, "FactorGraph"); + EXPECT_LONGS_EQUAL(158, muls); + printCounts("Asia FG"); - fg = fg.combine(X, &add_); - dot(fg, "Marginalized-6X"); - fg = fg.combine(T, &add_); - dot(fg, "Marginalized-5T"); - fg = fg.combine(S, &add_); - dot(fg, "Marginalized-4S"); - fg = fg.combine(E, &add_); - dot(fg, "Marginalized-3E"); - fg = fg.combine(L, &add_); - dot(fg, "Marginalized-2L"); - EXPECT(adds = 54); - printCounts("marginalize"); + fg = fg.combine(X, &add_); + dot(fg, "Marginalized-6X"); + fg = fg.combine(T, &add_); + dot(fg, "Marginalized-5T"); + fg = fg.combine(S, &add_); + dot(fg, "Marginalized-4S"); + fg = fg.combine(E, &add_); + dot(fg, "Marginalized-3E"); + fg = fg.combine(L, &add_); + dot(fg, "Marginalized-2L"); + EXPECT(adds = 54); + printCounts("marginalize"); - // BLESTX + // BLESTX - // Eliminate X - ADT fE = pX; - dot(fE, "Eliminate-01-fEX"); - fE = fE.combine(X, &add_); - dot(fE, "Eliminate-02-fE"); - printCounts("Eliminate X"); + // Eliminate X + ADT fE = pX; + dot(fE, "Eliminate-01-fEX"); + fE = fE.combine(X, &add_); + dot(fE, "Eliminate-02-fE"); + printCounts("Eliminate X"); - // Eliminate T - ADT fLE = pT; - fLE = apply(fLE, pE, &mul); - dot(fLE, "Eliminate-03-fLET"); - fLE = fLE.combine(T, &add_); - dot(fLE, "Eliminate-04-fLE"); - printCounts("Eliminate T"); + // Eliminate T + ADT fLE = pT; + fLE = apply(fLE, pE, &mul); + dot(fLE, "Eliminate-03-fLET"); + fLE = fLE.combine(T, &add_); + dot(fLE, "Eliminate-04-fLE"); + printCounts("Eliminate T"); - // Eliminate S - ADT fBL = pS; - fBL = apply(fBL, pL, &mul); - fBL = apply(fBL, pB, &mul); - dot(fBL, "Eliminate-05-fBLS"); - fBL = fBL.combine(S, &add_); - dot(fBL, "Eliminate-06-fBL"); - printCounts("Eliminate S"); + // Eliminate S + ADT fBL = pS; + fBL = apply(fBL, pL, &mul); + fBL = apply(fBL, pB, &mul); + dot(fBL, "Eliminate-05-fBLS"); + fBL = fBL.combine(S, &add_); + dot(fBL, "Eliminate-06-fBL"); + printCounts("Eliminate S"); - // Eliminate E - ADT fBL2 = fE; - fBL2 = apply(fBL2, fLE, &mul); - fBL2 = apply(fBL2, pD, &mul); - dot(fBL2, "Eliminate-07-fBLE"); - fBL2 = fBL2.combine(E, &add_); - dot(fBL2, "Eliminate-08-fBL2"); - printCounts("Eliminate E"); + // Eliminate E + ADT fBL2 = fE; + fBL2 = apply(fBL2, fLE, &mul); + fBL2 = apply(fBL2, pD, &mul); + dot(fBL2, "Eliminate-07-fBLE"); + fBL2 = fBL2.combine(E, &add_); + dot(fBL2, "Eliminate-08-fBL2"); + printCounts("Eliminate E"); - // Eliminate L - ADT fB = fBL; - fB = apply(fB, fBL2, &mul); - dot(fB, "Eliminate-09-fBL"); - fB = fB.combine(L, &add_); - dot(fB, "Eliminate-10-fB"); - printCounts("Eliminate L"); + // Eliminate L + ADT fB = fBL; + fB = apply(fB, fBL2, &mul); + dot(fB, "Eliminate-09-fBL"); + fB = fB.combine(L, &add_); + dot(fB, "Eliminate-10-fB"); + printCounts("Eliminate L"); } /* ************************************************************************* */ // test equality TEST(ADT, equality_noparser) { - DiscreteKey A(0,2), B(1,2); - Signature::Table tableA, tableB; - Signature::Row rA, rB; - rA += 80, 20; rB += 60, 40; - tableA += rA; tableB += rB; + DiscreteKey A(0,2), B(1,2); + Signature::Table tableA, tableB; + Signature::Row rA, rB; + rA += 80, 20; rB += 60, 40; + tableA += rA; tableB += rB; - // Check straight equality - ADT pA1 = create(A % tableA); - ADT pA2 = create(A % tableA); - EXPECT(pA1 == pA2); // should be equal + // Check straight equality + ADT pA1 = create(A % tableA); + ADT pA2 = create(A % tableA); + EXPECT(pA1 == pA2); // should be equal - // Check equality after apply - ADT pB = create(B % tableB); - ADT pAB1 = apply(pA1, pB, &mul); - ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + // Check equality after apply + ADT pB = create(B % tableB); + ADT pAB1 = apply(pA1, pB, &mul); + ADT pAB2 = apply(pB, pA1, &mul); + EXPECT(pAB2 == pAB1); } /* ************************************************************************* */ // test equality TEST(ADT, equality_parser) { - DiscreteKey A(0,2), B(1,2); - // Check straight equality - ADT pA1 = create(A % "80/20"); - ADT pA2 = create(A % "80/20"); - EXPECT(pA1 == pA2); // should be equal + DiscreteKey A(0,2), B(1,2); + // Check straight equality + ADT pA1 = create(A % "80/20"); + ADT pA2 = create(A % "80/20"); + EXPECT(pA1 == pA2); // should be equal - // Check equality after apply - ADT pB = create(B % "60/40"); - ADT pAB1 = apply(pA1, pB, &mul); - ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + // Check equality after apply + ADT pB = create(B % "60/40"); + ADT pAB1 = apply(pA1, pB, &mul); + ADT pAB2 = apply(pB, pA1, &mul); + EXPECT(pAB2 == pAB1); } /* ******************************************************************************** */ @@ -371,43 +371,43 @@ TEST(ADT, equality_parser) // test constructor from strings TEST(ADT, constructor) { - DiscreteKey v0(0,2), v1(1,3); - Assignment x00, x01, x02, x10, x11, x12; - x00[0] = 0, x00[1] = 0; - x01[0] = 0, x01[1] = 1; - x02[0] = 0, x02[1] = 2; - x10[0] = 1, x10[1] = 0; - x11[0] = 1, x11[1] = 1; - x12[0] = 1, x12[1] = 2; + DiscreteKey v0(0,2), v1(1,3); + Assignment x00, x01, x02, x10, x11, x12; + x00[0] = 0, x00[1] = 0; + x01[0] = 0, x01[1] = 1; + x02[0] = 0, x02[1] = 2; + x10[0] = 1, x10[1] = 0; + x11[0] = 1, x11[1] = 1; + x12[0] = 1, x12[1] = 2; - ADT f1(v0 & v1, "0 1 2 3 4 5"); - EXPECT_DOUBLES_EQUAL(0, f1(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(1, f1(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(2, f1(x02), 1e-9); - EXPECT_DOUBLES_EQUAL(3, f1(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(4, f1(x11), 1e-9); - EXPECT_DOUBLES_EQUAL(5, f1(x12), 1e-9); + ADT f1(v0 & v1, "0 1 2 3 4 5"); + EXPECT_DOUBLES_EQUAL(0, f1(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(1, f1(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(2, f1(x02), 1e-9); + EXPECT_DOUBLES_EQUAL(3, f1(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(4, f1(x11), 1e-9); + EXPECT_DOUBLES_EQUAL(5, f1(x12), 1e-9); - ADT f2(v1 & v0, "0 1 2 3 4 5"); - EXPECT_DOUBLES_EQUAL(0, f2(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(2, f2(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(4, f2(x02), 1e-9); - EXPECT_DOUBLES_EQUAL(1, f2(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(3, f2(x11), 1e-9); - EXPECT_DOUBLES_EQUAL(5, f2(x12), 1e-9); + ADT f2(v1 & v0, "0 1 2 3 4 5"); + EXPECT_DOUBLES_EQUAL(0, f2(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(2, f2(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(4, f2(x02), 1e-9); + EXPECT_DOUBLES_EQUAL(1, f2(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(3, f2(x11), 1e-9); + EXPECT_DOUBLES_EQUAL(5, f2(x12), 1e-9); - DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2); - vector table(5 * 4 * 3 * 2); - double x = 0; - BOOST_FOREACH(double& t, table) - t = x++; - ADT f3(z0 & z1 & z2 & z3, table); - Assignment assignment; - assignment[0] = 0; - assignment[1] = 0; - assignment[2] = 0; - assignment[3] = 1; - EXPECT_DOUBLES_EQUAL(1, f3(assignment), 1e-9); + DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2); + vector table(5 * 4 * 3 * 2); + double x = 0; + BOOST_FOREACH(double& t, table) + t = x++; + ADT f3(z0 & z1 & z2 & z3, table); + Assignment assignment; + assignment[0] = 0; + assignment[1] = 0; + assignment[2] = 0; + assignment[3] = 1; + EXPECT_DOUBLES_EQUAL(1, f3(assignment), 1e-9); } /* ************************************************************************* */ @@ -415,109 +415,109 @@ TEST(ADT, constructor) // Only works if DiscreteKeys are binary, as size_t has binary cardinality! TEST(ADT, conversion) { - DiscreteKey X(0,2), Y(1,2); - ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); - dot(fDiscreteKey, "conversion-f1"); + DiscreteKey X(0,2), Y(1,2); + ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); + dot(fDiscreteKey, "conversion-f1"); - std::map ordering; - ordering[0] = 5; - ordering[1] = 2; + std::map ordering; + ordering[0] = 5; + ordering[1] = 2; - AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); - // f1.print("f1"); - // f2.print("f2"); - dot(fIndexKey, "conversion-f2"); + AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); + // f1.print("f1"); + // f2.print("f2"); + dot(fIndexKey, "conversion-f2"); - Assignment x00, x01, x02, x10, x11, x12; - x00[5] = 0, x00[2] = 0; - x01[5] = 0, x01[2] = 1; - x10[5] = 1, x10[2] = 0; - x11[5] = 1, x11[2] = 1; - EXPECT_DOUBLES_EQUAL(0.2, fIndexKey(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(0.5, fIndexKey(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(0.3, fIndexKey(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(0.6, fIndexKey(x11), 1e-9); + Assignment x00, x01, x02, x10, x11, x12; + x00[5] = 0, x00[2] = 0; + x01[5] = 0, x01[2] = 1; + x10[5] = 1, x10[2] = 0; + x11[5] = 1, x11[2] = 1; + EXPECT_DOUBLES_EQUAL(0.2, fIndexKey(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, fIndexKey(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(0.3, fIndexKey(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(0.6, fIndexKey(x11), 1e-9); } /* ******************************************************************************** */ // test operations in elimination TEST(ADT, elimination) { - DiscreteKey A(0,2), B(1,3), C(2,2); - ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); - dot(f1, "elimination-f1"); + DiscreteKey A(0,2), B(1,3), C(2,2); + ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); + dot(f1, "elimination-f1"); - { - // sum out lower key - ADT actualSum = f1.sum(C); - ADT expectedSum(A & B, "3 7 11 9 6 10"); - CHECK(assert_equal(expectedSum,actualSum)); + { + // sum out lower key + ADT actualSum = f1.sum(C); + ADT expectedSum(A & B, "3 7 11 9 6 10"); + CHECK(assert_equal(expectedSum,actualSum)); - // normalize - ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // - 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10; - ADT expected(A & B & C, cpt); - CHECK(assert_equal(expected,actual)); - } + // normalize + ADT actual = f1 / actualSum; + vector cpt; + cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // + 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10; + ADT expected(A & B & C, cpt); + CHECK(assert_equal(expected,actual)); + } - { - // sum out lower 2 keys - ADT actualSum = f1.sum(C).sum(B); - ADT expectedSum(A, 21, 25); - CHECK(assert_equal(expectedSum,actualSum)); + { + // sum out lower 2 keys + ADT actualSum = f1.sum(C).sum(B); + ADT expectedSum(A, 21, 25); + CHECK(assert_equal(expectedSum,actualSum)); - // normalize - ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // - 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25; - ADT expected(A & B & C, cpt); - CHECK(assert_equal(expected,actual)); - } + // normalize + ADT actual = f1 / actualSum; + vector cpt; + cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // + 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25; + ADT expected(A & B & C, cpt); + CHECK(assert_equal(expected,actual)); + } } /* ******************************************************************************** */ // Test non-commutative op TEST(ADT, div) { - DiscreteKey A(0,2), B(1,2); + DiscreteKey A(0,2), B(1,2); - // Literals - ADT a(A, 8, 16); - ADT b(B, 2, 4); - ADT expected_a_div_b(A & B, "4 2 8 4"); // 8/2 8/4 16/2 16/4 - ADT expected_b_div_a(A & B, "0.25 0.5 0.125 0.25"); // 2/8 4/8 2/16 4/16 - EXPECT(assert_equal(expected_a_div_b, a / b)); - EXPECT(assert_equal(expected_b_div_a, b / a)); + // Literals + ADT a(A, 8, 16); + ADT b(B, 2, 4); + ADT expected_a_div_b(A & B, "4 2 8 4"); // 8/2 8/4 16/2 16/4 + ADT expected_b_div_a(A & B, "0.25 0.5 0.125 0.25"); // 2/8 4/8 2/16 4/16 + EXPECT(assert_equal(expected_a_div_b, a / b)); + EXPECT(assert_equal(expected_b_div_a, b / a)); } /* ******************************************************************************** */ // test zero shortcut TEST(ADT, zero) { - DiscreteKey A(0,2), B(1,2); + DiscreteKey A(0,2), B(1,2); - // Literals - ADT a(A, 0, 1); - ADT notb(B, 1, 0); - ADT anotb = a * notb; - // GTSAM_PRINT(anotb); - Assignment x00, x01, x10, x11; - x00[0] = 0, x00[1] = 0; - x01[0] = 0, x01[1] = 1; - x10[0] = 1, x10[1] = 0; - x11[0] = 1, x11[1] = 1; - EXPECT_DOUBLES_EQUAL(0, anotb(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(0, anotb(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(1, anotb(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9); + // Literals + ADT a(A, 0, 1); + ADT notb(B, 1, 0); + ADT anotb = a * notb; + // GTSAM_PRINT(anotb); + Assignment x00, x01, x10, x11; + x00[0] = 0, x00[1] = 0; + x01[0] = 0, x01[1] = 1; + x10[0] = 1, x10[1] = 0; + x11[0] = 1, x11[1] = 1; + EXPECT_DOUBLES_EQUAL(0, anotb(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(0, anotb(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(1, anotb(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9); } /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 1dfcd3057..1dfd56eec 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /* - * @file testDecisionTree.cpp - * @brief Develop DecisionTree - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #include @@ -35,7 +35,7 @@ using namespace gtsam; template void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT - f.dot(filename); + f.dot(filename); #endif } @@ -51,190 +51,190 @@ typedef DecisionTree CrazyDecisionTree; // check that DecisionTree typedef DecisionTree DT; struct Ring { - static inline int zero() { - return 0; - } - static inline int one() { - return 1; - } - static inline int add(const int& a, const int& b) { - return a + b; - } - static inline int mul(const int& a, const int& b) { - return a * b; - } + static inline int zero() { + return 0; + } + static inline int one() { + return 1; + } + static inline int add(const int& a, const int& b) { + return a + b; + } + static inline int mul(const int& a, const int& b) { + return a * b; + } }; /* ******************************************************************************** */ // test DT TEST(DT, example) { - // Create labels - string A("A"), B("B"), C("C"); + // Create labels + string A("A"), B("B"), C("C"); - // create a value - Assignment x00, x01, x10, x11; - x00[A] = 0, x00[B] = 0; - x01[A] = 0, x01[B] = 1; - x10[A] = 1, x10[B] = 0; - x11[A] = 1, x11[B] = 1; + // create a value + Assignment x00, x01, x10, x11; + x00[A] = 0, x00[B] = 0; + x01[A] = 0, x01[B] = 1; + x10[A] = 1, x10[B] = 0; + x11[A] = 1, x11[B] = 1; - // A - DT a(A, 0, 5); - LONGS_EQUAL(0,a(x00)) - LONGS_EQUAL(5,a(x10)) - DOT(a); + // A + DT a(A, 0, 5); + LONGS_EQUAL(0,a(x00)) + LONGS_EQUAL(5,a(x10)) + DOT(a); - // pruned - DT p(A, 2, 2); - LONGS_EQUAL(2,p(x00)) - LONGS_EQUAL(2,p(x10)) - DOT(p); + // pruned + DT p(A, 2, 2); + LONGS_EQUAL(2,p(x00)) + LONGS_EQUAL(2,p(x10)) + DOT(p); - // \neg B - DT notb(B, 5, 0); - LONGS_EQUAL(5,notb(x00)) - LONGS_EQUAL(5,notb(x10)) - DOT(notb); + // \neg B + DT notb(B, 5, 0); + LONGS_EQUAL(5,notb(x00)) + LONGS_EQUAL(5,notb(x10)) + DOT(notb); - // apply, two nodes, in natural order - DT anotb = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,anotb(x00)) - LONGS_EQUAL(0,anotb(x01)) - LONGS_EQUAL(25,anotb(x10)) - LONGS_EQUAL(0,anotb(x11)) - DOT(anotb); + // apply, two nodes, in natural order + DT anotb = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,anotb(x00)) + LONGS_EQUAL(0,anotb(x01)) + LONGS_EQUAL(25,anotb(x10)) + LONGS_EQUAL(0,anotb(x11)) + DOT(anotb); - // check pruning - DT pnotb = apply(p, notb, &Ring::mul); - LONGS_EQUAL(10,pnotb(x00)) - LONGS_EQUAL( 0,pnotb(x01)) - LONGS_EQUAL(10,pnotb(x10)) - LONGS_EQUAL( 0,pnotb(x11)) - DOT(pnotb); + // check pruning + DT pnotb = apply(p, notb, &Ring::mul); + LONGS_EQUAL(10,pnotb(x00)) + LONGS_EQUAL( 0,pnotb(x01)) + LONGS_EQUAL(10,pnotb(x10)) + LONGS_EQUAL( 0,pnotb(x11)) + DOT(pnotb); - // check pruning - DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); - LONGS_EQUAL(0,zeros(x00)) - LONGS_EQUAL(0,zeros(x01)) - LONGS_EQUAL(0,zeros(x10)) - LONGS_EQUAL(0,zeros(x11)) - DOT(zeros); + // check pruning + DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); + LONGS_EQUAL(0,zeros(x00)) + LONGS_EQUAL(0,zeros(x01)) + LONGS_EQUAL(0,zeros(x10)) + LONGS_EQUAL(0,zeros(x11)) + DOT(zeros); - // apply, two nodes, in switched order - DT notba = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,notba(x00)) - LONGS_EQUAL(0,notba(x01)) - LONGS_EQUAL(25,notba(x10)) - LONGS_EQUAL(0,notba(x11)) - DOT(notba); + // apply, two nodes, in switched order + DT notba = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,notba(x00)) + LONGS_EQUAL(0,notba(x01)) + LONGS_EQUAL(25,notba(x10)) + LONGS_EQUAL(0,notba(x11)) + DOT(notba); - // Test choose 0 - DT actual0 = notba.choose(A, 0); - EXPECT(assert_equal(DT(0.0), actual0)); - DOT(actual0); + // Test choose 0 + DT actual0 = notba.choose(A, 0); + EXPECT(assert_equal(DT(0.0), actual0)); + DOT(actual0); - // Test choose 1 - DT actual1 = notba.choose(A, 1); - EXPECT(assert_equal(DT(B, 25, 0), actual1)); - DOT(actual1); + // Test choose 1 + DT actual1 = notba.choose(A, 1); + EXPECT(assert_equal(DT(B, 25, 0), actual1)); + DOT(actual1); - // apply, two nodes at same level - DT a_and_a = apply(a, a, &Ring::mul); - LONGS_EQUAL(0,a_and_a(x00)) - LONGS_EQUAL(0,a_and_a(x01)) - LONGS_EQUAL(25,a_and_a(x10)) - LONGS_EQUAL(25,a_and_a(x11)) - DOT(a_and_a); + // apply, two nodes at same level + DT a_and_a = apply(a, a, &Ring::mul); + LONGS_EQUAL(0,a_and_a(x00)) + LONGS_EQUAL(0,a_and_a(x01)) + LONGS_EQUAL(25,a_and_a(x10)) + LONGS_EQUAL(25,a_and_a(x11)) + DOT(a_and_a); - // create a function on C - DT c(C, 0, 5); + // create a function on C + DT c(C, 0, 5); - // and a model assigning stuff to C - Assignment x101; - x101[A] = 1, x101[B] = 0, x101[C] = 1; + // and a model assigning stuff to C + Assignment x101; + x101[A] = 1, x101[B] = 0, x101[C] = 1; - // mul notba with C - DT notbac = apply(notba, c, &Ring::mul); - LONGS_EQUAL(125,notbac(x101)) - DOT(notbac); + // mul notba with C + DT notbac = apply(notba, c, &Ring::mul); + LONGS_EQUAL(125,notbac(x101)) + DOT(notbac); - // mul now in different order - DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); - LONGS_EQUAL(125,acnotb(x101)) - DOT(acnotb); + // mul now in different order + DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); + LONGS_EQUAL(125,acnotb(x101)) + DOT(acnotb); } /* ******************************************************************************** */ // test Conversion enum Label { - U, V, X, Y, Z + U, V, X, Y, Z }; typedef DecisionTree BDT; bool convert(const int& y) { - return y != 0; + return y != 0; } TEST(DT, conversion) { - // Create labels - string A("A"), B("B"); + // Create labels + string A("A"), B("B"); - // apply, two nodes, in natural order - DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul); + // apply, two nodes, in natural order + DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul); - // convert - map ordering; - ordering[A] = X; - ordering[B] = Y; - boost::function op = convert; - BDT f2(f1, ordering, op); - // f1.print("f1"); - // f2.print("f2"); + // convert + map ordering; + ordering[A] = X; + ordering[B] = Y; + boost::function op = convert; + BDT f2(f1, ordering, op); + // f1.print("f1"); + // f2.print("f2"); - // create a value - Assignment