From 204b36fc054c27ddd04ce2912d82a12d93351a64 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 1 Apr 2012 22:21:07 +0000 Subject: [PATCH 01/72] Changes to make ROS hooks work with cmake --- .cproject | 395 ++++++++++++++++++++++---------------------- CMakeLists.txt | 30 +++- wrap/CMakeLists.txt | 7 +- wrap/wrap.cpp | 23 ++- 4 files changed, 238 insertions(+), 217 deletions(-) diff --git a/.cproject b/.cproject index c6699da3e..00c33f6be 100644 --- a/.cproject +++ b/.cproject @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -465,6 +459,7 @@ make + testGraph.run true false @@ -536,6 +531,7 @@ make + testInference.run true false @@ -543,6 +539,7 @@ make + testGaussianFactor.run true false @@ -550,6 +547,7 @@ make + testJunctionTree.run true false @@ -557,6 +555,7 @@ make + testSymbolicBayesNet.run true false @@ -564,6 +563,7 @@ make + testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -689,26 +689,18 @@ true true - + make - -j2 - all + -j5 + nonlinear.testValues.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -745,18 +737,26 @@ true true - + make - -j5 - nonlinear.testValues.run + -j2 + all true true true - + make - -j5 - nonlinear.testOrdering.run + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -793,30 +793,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -1091,7 +1067,6 @@ make - testErrors.run true false @@ -1547,6 +1522,7 @@ make + testSimulated2DOriented.run true false @@ -1586,6 +1562,7 @@ make + testSimulated2D.run true false @@ -1593,6 +1570,7 @@ make + testSimulated3D.run true false @@ -1776,6 +1754,7 @@ make + tests/testGaussianISAM2 true false @@ -1797,6 +1776,102 @@ 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 -j2 @@ -1980,137 +2055,18 @@ true true - + make - -j2 - testRot3.run + -j5 + wrap_gtsam 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 - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + VERBOSE=1 + wrap_gtsam true false true @@ -2155,6 +2111,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/CMakeLists.txt b/CMakeLists.txt index ebfd16e5f..04bc6ccf2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,16 +58,23 @@ if(NOT FIRST_PASS_DONE) 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_EXAMPLES "Enable/Disable building of examples" ON) -option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) +#SET(FOO_BUILD_SHARED OFF CACHE BOOL "Build libfoo shared library") +#set(GTSAM_BUILD_TESTS ON CACHE BOOL "Enable/Disable building of tests") +#set(GTSAM_BUILD_TIMING ON CACHE BOOL "Enable/Disable building of timing scripts") +#set(GTSAM_BUILD_EXAMPLES ON CACHE BOOL "Enable/Disable building of examples") +#set(GTSAM_BUILD_WRAP ON CACHE BOOL "Enable/Disable building of matlab wrap utility (necessary for matlab interface)") +#set(GTSAM_BUILD_CONVENIENCE_LIBRARIES OFF CACHE BOOL "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install") + +option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) +option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) +option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) +option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON) -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) -option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) -option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) -option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) +option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) +option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) +option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) # Add the Quaternion Build Flag if requested if (GTSAM_USE_QUATERNIONS) @@ -126,6 +133,11 @@ endif(GTSAM_BUILD_EXAMPLES) set(FIRST_PASS_DONE true CACHE BOOL "Internally used to mark whether cmake has been run multiple times") mark_as_advanced(FIRST_PASS_DONE) +# Record the root dir for gtsam - needed during external builds, e.g., ROS +set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) +message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + + # print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 4e045cf66..d753c5d76 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -37,9 +37,14 @@ set(moduleName gtsam) include(GtsamMatlabWrap) find_mexextension() +# get binary path for wrap executable to allow for different build configurations (e.g., ROS) +if (NOT EXECUTABLE_OUTPUT_PATH) + set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}) +endif() + # Code generation command add_custom_target(wrap_gtsam ALL COMMAND - ./wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}" + ${EXECUTABLE_OUTPUT_PATH}/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}" DEPENDS wrap) set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 8801fef76..f55927a5e 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -45,19 +45,28 @@ void generate_matlab_toolbox(const string& mexExt, module.matlab_code(toolboxPath,mexExt,mexFlags); } +/** Displays usage information */ +void usage() { + cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; + cerr << "usage: wrap mexExtension interfacePath moduleName toolboxPath [mexFlags]" << endl; + cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl; + cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; + cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; + cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; + cerr << " [mexFlags] : extra flags for the mex command" << endl; +} + /** * main parses arguments and calls generate_matlab_toolbox above * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { if (argc<6 || argc>7) { - cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; - cerr << "usage: wrap mexExtension interfacePath moduleName toolboxPath [mexFlags]" << endl; - cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl; - cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; - cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; - cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; - cerr << " [mexFlags] : extra flags for the mex command" << endl; + cerr << "Invalid arguments:\n"; + for (int i=0; i Date: Mon, 2 Apr 2012 20:04:43 +0000 Subject: [PATCH 02/72] Fixed compile warning --- gtsam/nonlinear/ISAM2.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index a59bc97dc..228b6e491 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -238,8 +238,9 @@ boost::shared_ptr > ISAM2::recalculate( } } } else { - if(theta_.size() > newKeys.size()) // Only if some variables are unconstrained + if(theta_.size() > newKeys.size()) { // Only if some variables are unconstrained BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } + } } Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); From 36c921bb9f875c1b40986b41ab2073cb88e7fdf0 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 2 Apr 2012 20:06:00 +0000 Subject: [PATCH 03/72] Reordered gradient calculation to improve computation performance --- gtsam/nonlinear/ISAM2.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index f37d02312..8d3777d8c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -216,7 +216,10 @@ struct ISAM2Clique : public BayesTreeCliqueBaseget_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); - gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(), + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons +// gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(), +// -conditional.get_S().transpose() * conditional.get_d(); + gradientContribution_ << -(conditional.get_d().transpose() * conditional.get_R() * conditional.permutation().transpose()).transpose(), -conditional.get_S().transpose() * conditional.get_d(); } From 829b2a7ffc5d0b7491c4a17231791ad586e661cb Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 2 Apr 2012 20:08:51 +0000 Subject: [PATCH 04/72] Changed CCOLAMD parameters to examine all entries instead of pushing variables with many factors to the end of the ordering. --- gtsam/inference/inference.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp index e1c250fcf..9b7252030 100644 --- a/gtsam/inference/inference.cpp +++ b/gtsam/inference/inference.cpp @@ -60,11 +60,11 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s for(size_t i=0; isecond; } /// Access the index for the requested key, throws std::out_of_range if the @@ -101,7 +101,7 @@ public: /// behavior of std::map) Index operator[](Key key) const { const_iterator i=order_.find(key); - if(i == order_.end()) throw std::out_of_range(std::string()); + if(i == order_.end()) throw std::out_of_range(std::string("Attempting to access a key from an ordering that does not contain that key")); else return i->second; } /** Returns an iterator pointing to the symbol/index pair with the requested, @@ -134,7 +134,7 @@ public: /** Try insert, but will fail if the key is already present */ iterator insert(const value_type& key_order) { std::pair it_ok(tryInsert(key_order)); - if(!it_ok.second) throw std::invalid_argument(std::string()); + if(!it_ok.second) throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); else return it_ok.first; } From ee532f2b1ffbb378c1fb8eb25c445f31dacffc22 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 2 Apr 2012 20:17:15 +0000 Subject: [PATCH 07/72] Included the inline header at the end of the standard header --- tests/testGaussianISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 1df8cdff3..4c798aa8d 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -24,7 +24,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include #include From 8cd7662dab50ed4259a5d2063b1400496f84631e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 2 Apr 2012 20:43:34 +0000 Subject: [PATCH 08/72] Removed commented code --- gtsam/nonlinear/ISAM2.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8d3777d8c..3a8d2d3bf 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -217,8 +217,6 @@ struct ISAM2Clique : public BayesTreeCliqueBase Date: Tue, 3 Apr 2012 22:06:06 +0000 Subject: [PATCH 09/72] Bug fix when relinearizing on first step in iSAM2 --- gtsam/nonlinear/ISAM2.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 228b6e491..0c05d15c4 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -743,7 +743,8 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { g.setZero(); // Sum up contributions for each clique - gradientAtZeroTreeAdder(bayesTree.root(), g); + if(bayesTree.root()) + gradientAtZeroTreeAdder(bayesTree.root(), g); } } From 14d20cff0b7e7d475d8398a85df5d170dacd834a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 3 Apr 2012 23:20:03 +0000 Subject: [PATCH 10/72] Bug fix when getting nonlinear error debug statistics --- gtsam/nonlinear/ISAM2.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0c05d15c4..b0f05a3ee 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -561,15 +561,15 @@ ISAM2Result ISAM2::update( if(debug) delta_.print("delta_: "); //toc(9,"solve"); + result.cliques = this->nodes().size(); + deltaDoglegUptodate_ = false; + deltaUptodate_ = false; + tic(10,"evaluate error after"); if(params_.evaluateNonlinearError) result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); toc(10,"evaluate error after"); - result.cliques = this->nodes().size(); - deltaDoglegUptodate_ = false; - deltaUptodate_ = false; - return result; } From 48e3916ea242ef328385cd111cec9cbc09b3b535 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 5 Apr 2012 17:44:22 +0000 Subject: [PATCH 11/72] removed commented cmake code --- CMakeLists.txt | 7 ------- 1 file changed, 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 04bc6ccf2..610d026d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,13 +58,6 @@ if(NOT FIRST_PASS_DONE) endif() # Configurable Options -#SET(FOO_BUILD_SHARED OFF CACHE BOOL "Build libfoo shared library") -#set(GTSAM_BUILD_TESTS ON CACHE BOOL "Enable/Disable building of tests") -#set(GTSAM_BUILD_TIMING ON CACHE BOOL "Enable/Disable building of timing scripts") -#set(GTSAM_BUILD_EXAMPLES ON CACHE BOOL "Enable/Disable building of examples") -#set(GTSAM_BUILD_WRAP ON CACHE BOOL "Enable/Disable building of matlab wrap utility (necessary for matlab interface)") -#set(GTSAM_BUILD_CONVENIENCE_LIBRARIES OFF CACHE BOOL "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install") - option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) From da1c65b3949d5d311f46004b86c5870da4281920 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 6 Apr 2012 18:56:02 +0000 Subject: [PATCH 12/72] Comment format --- gtsam/linear/VectorValues.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 96bafcd16..a0af31c70 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -30,7 +30,7 @@ VectorValues::VectorValues(const VectorValues& other) { /* ************************************************************************* */ VectorValues& VectorValues::operator=(const VectorValues& rhs) { if(this != &rhs) { - resizeLike(rhs); // Copy structure + resizeLike(rhs); // Copy structure values_ = rhs.values_; // Copy values } return *this; From 45f2101f480feafed79278f05241d954bd445e83 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 6 Apr 2012 18:56:07 +0000 Subject: [PATCH 13/72] Fixed deep copy in iSAM2 and added unit test to reproduce problem --- gtsam/inference/BayesTree-inl.h | 25 +++- gtsam/inference/BayesTree.h | 25 ++-- gtsam/inference/BayesTreeCliqueBase.h | 15 +- gtsam/nonlinear/ISAM2.cpp | 43 +++++- gtsam/nonlinear/ISAM2.h | 40 ++--- tests/testGaussianISAM2.cpp | 205 ++++++++++++++------------ 6 files changed, 216 insertions(+), 137 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 95995a4e6..6881e9a69 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -315,6 +315,20 @@ namespace gtsam { root_ = new_clique; } + /* ************************************************************************* */ + template + BayesTree::BayesTree(const This& other) { + *this = other; + } + + /* ************************************************************************* */ + template + BayesTree& BayesTree::operator=(const This& other) { + this->clear(); + other.cloneTo(*this); + return *this; + } + /* ************************************************************************* */ template void BayesTree::print(const string& s) const { @@ -568,7 +582,16 @@ namespace gtsam { } } -/* ************************************************************************* */ + /* ************************************************************************* */ + template + void BayesTree::cloneTo( + This& newTree, const sharedClique& subtree, const sharedClique& parent) const { + sharedClique newClique(subtree->clone()); + newTree.addClique(newClique, parent); + BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { + cloneTo(newTree, childClique, newClique); + } + } } /// namespace gtsam diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 06a47ba8e..3df697a48 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -54,6 +54,7 @@ namespace gtsam { public: + typedef BayesTree This; typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > sharedBayesNet; @@ -146,6 +147,12 @@ namespace gtsam { /** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */ BayesTree(const BayesNet& bayesNet); + /** Copy constructor */ + BayesTree(const This& other); + + /** Assignment operator */ + This& operator=(const This& other); + /// @} /// @name Advanced Constructors /// @{ @@ -273,21 +280,15 @@ namespace gtsam { sharedClique insert(const sharedConditional& clique, std::list& children, bool isRootClique = false); - ///TODO: comment - void cloneTo(shared_ptr& newTree) const { + private: + + /** deep copy to another tree */ + void cloneTo(This& newTree) const { cloneTo(newTree, root(), sharedClique()); } - private: - - /** deep copy from another tree */ - void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const { - sharedClique newClique(subtree->clone()); - newTree->addClique(newClique, parent); - BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { - cloneTo(newTree, childClique, newClique); - } - } + /** deep copy to another tree */ + void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 82cec6f76..194b616b7 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -134,7 +134,9 @@ namespace gtsam { */ static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } - ///TODO: comment + /** 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 */ @@ -165,6 +167,17 @@ namespace gtsam { void assertInvariants() const; private: + + /** Cliques cannot be copied except by the clone() method, which does not + * copy the parent and child pointers. + */ + 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; } + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b0f05a3ee..6bb74dd8a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -42,7 +42,6 @@ static const double batchThreshold = 0.65; ISAM2::ISAM2(const ISAM2Params& params): delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { - // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } @@ -51,11 +50,51 @@ ISAM2::ISAM2(const ISAM2Params& params): ISAM2::ISAM2(): delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true) { - // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2& other): + delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) { + *this = other; +} + +/* ************************************************************************* */ +ISAM2& ISAM2::operator=(const ISAM2& rhs) { + // Copy BayesTree + this->Base::operator=(rhs); + + // Copy our variables + // When we have Permuted<...>, it is only necessary to copy this permuted + // view and not the original, because copying the permuted view automatically + // copies the original. + theta_ = rhs.theta_; + variableIndex_ = rhs.variableIndex_; + delta_ = rhs.delta_; + deltaNewton_ = rhs.deltaNewton_; + RgProd_ = rhs.RgProd_; + deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_; + deltaUptodate_ = rhs.deltaUptodate_; + deltaReplacedMask_ = rhs.deltaReplacedMask_; + nonlinearFactors_ = rhs.nonlinearFactors_; + ordering_ = rhs.ordering_; + params_ = rhs.params_; + doglegDelta_ = rhs.doglegDelta_; + +#ifndef NDEBUG + lastRelinVariables_ = rhs.lastRelinVariables_; +#endif + lastAffectedVariableCount = rhs.lastAffectedVariableCount; + lastAffectedFactorCount = rhs.lastAffectedFactorCount; + lastAffectedCliqueCount = rhs.lastAffectedCliqueCount; + lastAffectedMarkedCount = rhs.lastAffectedMarkedCount; + lastBacksubVariableCount = rhs.lastBacksubVariableCount; + lastNnzTop = rhs.lastNnzTop; + + return *this; +} + /* ************************************************************************* */ FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3a8d2d3bf..5e7adc19d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -329,6 +329,10 @@ protected: * delta will always be updated if necessary when requested with getDelta() * or calculateEstimate(). * + * This does not need to be permuted because any change in variable ordering + * that would cause a permutation will also mark variables as needing to be + * updated in this mask. + * * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ @@ -356,6 +360,7 @@ private: public: + typedef ISAM2 This; ///< This class typedef BayesTree Base; ///< The BayesTree base class /** Create an empty ISAM2 instance */ @@ -364,39 +369,16 @@ public: /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); + /** Copy constructor */ + ISAM2(const ISAM2& other); + + /** Assignment operator */ + ISAM2& operator=(const ISAM2& rhs); + typedef Base::Clique Clique; ///< A clique typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef Base::Cliques Cliques; ///< List of Clique typedef from base class - void cloneTo(boost::shared_ptr& newISAM2) const { - boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); - Base::cloneTo(bayesTree); - newISAM2->theta_ = theta_; - newISAM2->variableIndex_ = variableIndex_; - newISAM2->deltaUnpermuted_ = deltaUnpermuted_; - newISAM2->delta_ = delta_; - newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_; - newISAM2->deltaNewton_ = deltaNewton_; - newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_; - newISAM2->RgProd_ = RgProd_; - newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_; - newISAM2->deltaUptodate_ = deltaUptodate_; - newISAM2->deltaReplacedMask_ = deltaReplacedMask_; - newISAM2->nonlinearFactors_ = nonlinearFactors_; - newISAM2->ordering_ = ordering_; - newISAM2->params_ = params_; - newISAM2->doglegDelta_ = doglegDelta_; -#ifndef NDEBUG - newISAM2->lastRelinVariables_ = lastRelinVariables_; -#endif - newISAM2->lastAffectedVariableCount = lastAffectedVariableCount; - newISAM2->lastAffectedFactorCount = lastAffectedFactorCount; - newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount; - newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount; - newISAM2->lastBacksubVariableCount = lastBacksubVariableCount; - newISAM2->lastNnzTop = lastNnzTop; - } - /** * Add new factors, updating the solution and relinearizing as needed. * diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f30574d0b..5ff24070c 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -28,6 +28,102 @@ using boost::shared_ptr; const double tol = 1e-4; +ISAM2 createSlamlikeISAM2() { + + // Pose and landmark key types from planarSLAM + using planarSLAM::PoseKey; + using planarSLAM::PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); + Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + return isam; +} + /* ************************************************************************* */ TEST_UNSAFE(ISAM2, AddVariables) { @@ -778,105 +874,30 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) /* ************************************************************************* */ TEST(ISAM2, clone) { - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; + ISAM2 clone1; - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - - // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); - Values fullinit; - planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); + ISAM2 isam = createSlamlikeISAM2(); + clone1 = isam; - Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + ISAM2 clone2(isam); - isam.update(newfactors, init); + // Modify original isam + NonlinearFactorGraph factors; + factors.add(BetweenFactor(Symbol('x',0), Symbol('x',10), + isam.calculateEstimate(Symbol('x',0)).between(isam.calculateEstimate(Symbol('x',10))), sharedUnit(3))); + isam.update(factors); + + CHECK(assert_equal(createSlamlikeISAM2(), clone2)); } - EXPECT(isam_check(fullgraph, fullinit, isam)); + // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced + // if the references in the iSAM2 copy point to the old instance which deleted at + // the end of the {...} section above. + ISAM2 temp = createSlamlikeISAM2(); - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } - - // CLONING... - boost::shared_ptr isam2 - = boost::shared_ptr(new ISAM2()); - isam.cloneTo(isam2); - - CHECK(assert_equal(isam, *isam2)); + CHECK(assert_equal(createSlamlikeISAM2(), clone1)); + CHECK(assert_equal(clone1, temp)); } /* ************************************************************************* */ From 0e7f5e6d45ce751a99a0255443a8fd7276f985d2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 6 Apr 2012 20:22:42 +0000 Subject: [PATCH 14/72] Removed unused code - all moved to gtsam2 --- gtsam/base/BTree.h | 409 ----------------------- gtsam/base/DSF.h | 174 ---------- gtsam/base/DSFVector.cpp | 97 ------ gtsam/base/DSFVector.h | 79 ----- gtsam/base/FixedVector.h | 118 ------- gtsam/base/tests/testBTree.cpp | 210 ------------ gtsam/base/tests/testDSF.cpp | 280 ---------------- gtsam/base/tests/testDSFVector.cpp | 171 ---------- gtsam/base/tests/testFixedVector.cpp | 87 ----- gtsam/geometry/Tensor1.h | 85 ----- gtsam/geometry/Tensor1Expression.h | 181 ---------- gtsam/geometry/Tensor2.h | 84 ----- gtsam/geometry/Tensor2Expression.h | 310 ----------------- gtsam/geometry/Tensor3.h | 106 ------ gtsam/geometry/Tensor3Expression.h | 194 ----------- gtsam/geometry/Tensor4.h | 58 ---- gtsam/geometry/Tensor5.h | 75 ----- gtsam/geometry/Tensor5Expression.h | 135 -------- gtsam/geometry/projectiveGeometry.cpp | 71 ---- gtsam/geometry/projectiveGeometry.h | 124 ------- gtsam/geometry/tensorInterface.cpp | 26 -- gtsam/geometry/tensorInterface.h | 108 ------ gtsam/geometry/tensors.h | 45 --- gtsam/geometry/tests/testFundamental.cpp | 73 ---- gtsam/geometry/tests/testHomography2.cpp | 188 ----------- gtsam/geometry/tests/testTensors.cpp | 240 ------------- gtsam/inference/FactorGraph-inl.h | 1 - gtsam/slam/simulated3D.cpp | 43 --- gtsam/slam/simulated3D.h | 126 ------- gtsam/slam/tests/testSimulated3D.cpp | 63 ---- 30 files changed, 3961 deletions(-) delete mode 100644 gtsam/base/BTree.h delete mode 100644 gtsam/base/DSF.h delete mode 100644 gtsam/base/DSFVector.cpp delete mode 100644 gtsam/base/DSFVector.h delete mode 100644 gtsam/base/FixedVector.h delete mode 100644 gtsam/base/tests/testBTree.cpp delete mode 100644 gtsam/base/tests/testDSF.cpp delete mode 100644 gtsam/base/tests/testDSFVector.cpp delete mode 100644 gtsam/base/tests/testFixedVector.cpp delete mode 100644 gtsam/geometry/Tensor1.h delete mode 100644 gtsam/geometry/Tensor1Expression.h delete mode 100644 gtsam/geometry/Tensor2.h delete mode 100644 gtsam/geometry/Tensor2Expression.h delete mode 100644 gtsam/geometry/Tensor3.h delete mode 100644 gtsam/geometry/Tensor3Expression.h delete mode 100644 gtsam/geometry/Tensor4.h delete mode 100644 gtsam/geometry/Tensor5.h delete mode 100644 gtsam/geometry/Tensor5Expression.h delete mode 100644 gtsam/geometry/projectiveGeometry.cpp delete mode 100644 gtsam/geometry/projectiveGeometry.h delete mode 100644 gtsam/geometry/tensorInterface.cpp delete mode 100644 gtsam/geometry/tensorInterface.h delete mode 100644 gtsam/geometry/tensors.h delete mode 100644 gtsam/geometry/tests/testFundamental.cpp delete mode 100644 gtsam/geometry/tests/testHomography2.cpp delete mode 100644 gtsam/geometry/tests/testTensors.cpp delete mode 100644 gtsam/slam/simulated3D.cpp delete mode 100644 gtsam/slam/simulated3D.h delete mode 100644 gtsam/slam/tests/testSimulated3D.cpp diff --git a/gtsam/base/BTree.h b/gtsam/base/BTree.h deleted file mode 100644 index 2a40465a3..000000000 --- a/gtsam/base/BTree.h +++ /dev/null @@ -1,409 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BTree.h - * @brief purely functional binary tree - * @author Chris Beall - * @author Frank Dellaert - * @date Feb 3, 2010 - */ - -#include -#include -#include -#include - -namespace gtsam { - - /** - * @brief Binary tree - * @ingroup base - */ - template - class BTree { - - public: - - typedef std::pair value_type; - - private: - - /** - * Node in a tree - */ - struct Node { - - const size_t height_; - const value_type keyValue_; - const BTree left, right; - - /** default constructor */ - Node() { - } - - /** - * Create leaf node with height 1 - * @param keyValue (key,value) pair - */ - Node(const value_type& keyValue) : - height_(1), keyValue_(keyValue) { - } - - /** - * Create a node from two subtrees and a key value pair - */ - Node(const BTree& l, const value_type& keyValue, const BTree& r) : - height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1), - keyValue_(keyValue), left(l), right(r) { - } - - inline const KEY& key() const { return keyValue_.first;} - inline const VALUE& value() const { return keyValue_.second;} - - }; // Node - - // We store a shared pointer to the root of the functional tree - // composed of Node classes. If root_==NULL, the tree is empty. - typedef boost::shared_ptr sharedNode; - sharedNode root_; - - inline const value_type& keyValue() const { return root_->keyValue_;} - inline const KEY& key() const { return root_->key(); } - inline const VALUE& value() const { return root_->value(); } - inline const BTree& left() const { return root_->left; } - inline const BTree& right() const { return root_->right; } - - /** create a new balanced tree out of two trees and a key-value pair */ - static BTree balance(const BTree& l, const value_type& xd, const BTree& r) { - size_t hl = l.height(), hr = r.height(); - if (hl > hr + 2) { - const BTree& ll = l.left(), lr = l.right(); - if (ll.height() >= lr.height()) - return BTree(ll, l.keyValue(), BTree(lr, xd, r)); - else { - BTree _left(ll, l.keyValue(), lr.left()); - BTree _right(lr.right(), xd, r); - return BTree(_left, lr.keyValue(), _right); - } - } else if (hr > hl + 2) { - const BTree& rl = r.left(), rr = r.right(); - if (rr.height() >= rl.height()) - return BTree(BTree(l, xd, rl), r.keyValue(), rr); - else { - BTree _left(l, xd, rl.left()); - BTree _right(rl.right(), r.keyValue(), rr); - return BTree(_left, rl.keyValue(), _right); - } - } else - return BTree(l, xd, r); - } - - public: - - /** default constructor creates an empty tree */ - BTree() { - } - - /** copy constructor */ - BTree(const BTree& other) : - root_(other.root_) { - } - - /** create leaf from key-value pair */ - BTree(const value_type& keyValue) : - root_(new Node(keyValue)) { - } - - /** create from key-value pair and left, right subtrees */ - BTree(const BTree& l, const value_type& keyValue, const BTree& r) : - root_(new Node(l, keyValue, r)) { - } - - /** Check whether tree is empty */ - bool empty() const { - return !root_; - } - - /** add a key-value pair */ - BTree add(const value_type& xd) const { - if (empty()) return BTree(xd); - const KEY& x = xd.first; - if (x == key()) - return BTree(left(), xd, right()); - else if (x < key()) - return balance(left().add(xd), keyValue(), right()); - else - return balance(left(), keyValue(), right().add(xd)); - } - - /** add a key-value pair */ - BTree add(const KEY& x, const VALUE& d) const { - return add(std::make_pair(x, d)); - } - - /** member predicate */ - bool mem(const KEY& x) const { - if (!root_) return false; - if (x == key()) return true; - if (x < key()) - return left().mem(x); - else - return right().mem(x); - } - - /** Check whether trees are *exactly* the same (occupy same memory) */ - inline bool same(const BTree& other) const { - return (other.root_ == root_); - } - - /** - * Check whether trees are structurally the same, - * i.e., contain the same values in same tree-structure. - */ - bool operator==(const BTree& other) const { - if (other.root_ == root_) return true; // if same, we're done - if (empty() && !other.empty()) return false; - if (!empty() && other.empty()) return false; - // both non-empty, recurse: check this key-value pair and subtrees... - return (keyValue() == other.keyValue()) && (left() == other.left()) - && (right() == other.right()); - } - - inline bool operator!=(const BTree& other) const { - return !operator==(other); - } - - /** minimum key binding */ - const value_type& min() const { - if (!root_) throw std::invalid_argument("BTree::min: empty tree"); - if (left().empty()) return keyValue(); - return left().min(); - } - - /** remove minimum key binding */ - BTree remove_min() const { - if (!root_) throw std::invalid_argument("BTree::remove_min: empty tree"); - if (left().empty()) return right(); - return balance(left().remove_min(), keyValue(), right()); - } - - /** merge two trees */ - static BTree merge(const BTree& t1, const BTree& t2) { - if (t1.empty()) return t2; - if (t2.empty()) return t1; - const value_type& xd = t2.min(); - return balance(t1, xd, t2.remove_min()); - } - - /** remove a key-value pair */ - BTree remove(const KEY& x) const { - if (!root_) return BTree(); - if (x == key()) - return merge(left(), right()); - else if (x < key()) - return balance(left().remove(x), keyValue(), right()); - else - return balance(left(), keyValue(), right().remove(x)); - } - - /** Return height of the tree, 0 if empty */ - size_t height() const { - return (root_ != NULL) ? root_->height_ : 0; - } - - /** return size of the tree */ - size_t size() const { - if (!root_) return 0; - return left().size() + 1 + right().size(); - } - - /** - * find a value given a key, throws exception when not found - * Optimized non-recursive version as [find] is crucial for speed - */ - const VALUE& find(const KEY& k) const { - const Node* node = root_.get(); - while (node) { - const KEY& key = node->key(); - if (k < key) node = node->left.root_.get(); - else if (key < k) node = node->right.root_.get(); - else return node->value(); - } - - throw std::invalid_argument("BTree::find: key not found"); - } - - /** print in-order */ - void print(const std::string& s = "") const { - if (empty()) return; - KEY k = key(); - std::stringstream ss; - ss << height(); - k.print(s + ss.str() + " "); - left().print(s + "L "); - right().print(s + "R "); - } - - /** iterate over tree */ - void iter(boost::function f) const { - if (!root_) return; - left().iter(f); - f(key(), value()); - right().iter(f); - } - - /** map key-values in tree over function f that computes a new value */ - template - BTree map(boost::function f) const { - if (empty()) return BTree (); - std::pair xd(key(), f(key(), value())); - return BTree (left().map(f), xd, right().map(f)); - } - - /** - * t.fold(f,a) computes [(f kN dN ... (f k1 d1 a)...)], - * where [k1 ... kN] are the keys of all bindings in [m], - * and [d1 ... dN] are the associated data. - * The associated values are passed to [f] in reverse sort order - */ - template - ACC fold(boost::function f, - const ACC& a) const { - if (!root_) return a; - ACC ar = right().fold(f, a); // fold over right subtree - ACC am = f(key(), value(), ar); // apply f with current value - return left().fold(f, am); // fold over left subtree - } - - /** - * @brief Const iterator - * Not trivial: iterator keeps a stack to indicate current path from root_ - */ - class const_iterator { - - private: - - typedef const_iterator Self; - typedef std::pair flagged; - - /** path to the iterator, annotated with flag */ - std::stack path_; - - const sharedNode& current() const { - return path_.top().first; - } - - bool done() const { - return path_.top().second; - } - - // The idea is we already iterated through the left-subtree and current key-value. - // We now try pushing left subtree of right onto the stack. If there is no right - // sub-tree, we pop this node of the stack and the parent becomes the iterator. - // We avoid going down a right-subtree that was already visited by checking the flag. - void increment() { - if (path_.empty()) return; - sharedNode t = current()->right.root_; - if (!t || done()) { - // no right subtree, iterator becomes first parent with a non-visited right subtree - path_.pop(); - while (!path_.empty() && done()) - path_.pop(); - } else { - path_.top().second = true; // flag we visited right - // push right root and its left-most path onto the stack - while (t) { - path_.push(std::make_pair(t, false)); - t = t->left.root_; - } - } - } - - public: - - // traits for playing nice with STL - typedef ptrdiff_t difference_type; - typedef std::forward_iterator_tag iterator_category; - typedef std::pair value_type; - typedef const value_type* pointer; - typedef const value_type& reference; - - /** initialize end */ - const_iterator() { - } - - /** initialize from root */ - const_iterator(const sharedNode& root) { - sharedNode t = root; - while (t) { - path_.push(std::make_pair(t, false)); - t = t->left.root_; - } - } - - /** equality */ - bool operator==(const Self& __x) const { - return path_ == __x.path_; - } - - /** inequality */ - bool operator!=(const Self& __x) const { - return path_ != __x.path_; - } - - /** dereference */ - reference operator*() const { - if (path_.empty()) throw std::invalid_argument( - "operator*: tried to dereference end"); - return current()->keyValue_; - } - - /** dereference */ - pointer operator->() const { - if (path_.empty()) throw std::invalid_argument( - "operator->: tried to dereference end"); - return &(current()->keyValue_); - } - - /** pre-increment */ - Self& operator++() { - increment(); - return *this; - } - - /** post-increment */ - Self operator++(int) { - Self __tmp = *this; - increment(); - return __tmp; - } - - }; // const_iterator - - // to make BTree work with BOOST_FOREACH - // We do *not* want a non-const iterator - typedef const_iterator iterator; - - /** return iterator */ - const_iterator begin() const { - return const_iterator(root_); - } - - /** return iterator */ - const_iterator end() const { - return const_iterator(); - } - - }; // BTree - -} // namespace gtsam - diff --git a/gtsam/base/DSF.h b/gtsam/base/DSF.h deleted file mode 100644 index 9952f2e10..000000000 --- a/gtsam/base/DSF.h +++ /dev/null @@ -1,174 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 DSF.h - * @date Mar 26, 2010 - * @author Kai Ni - * @brief An implementation of Disjoint set forests (see CLR page 446 and up) - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * Disjoint Set Forest class - * - * Quoting from CLR: A disjoint-set data structure maintains a collection - * S = {S_1,S_2,...} of disjoint dynamic sets. Each set is identified by - * a representative, which is some member of the set. - * - * @ingroup base - */ - template - class DSF : protected BTree { - - public: - typedef KEY Label; // label can be different from key, but for now they are same - typedef DSF Self; - typedef std::set Set; - typedef BTree Tree; - typedef std::pair KeyLabel; - - // constructor - DSF() : Tree() { } - - // constructor - DSF(const Tree& tree) : Tree(tree) {} - - // constructor with a list of unconnected keys - DSF(const std::list& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } - - // constructor with a set of unconnected keys - DSF(const std::set& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } - - // create a new singleton, does nothing if already exists - Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); } - - // find the label of the set in which {key} lives - Label findSet(const KEY& key) const { - KEY parent = this->find(key); - return parent == key ? key : findSet(parent); } - - // return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed - Self makeUnion(const KEY& key1, const KEY& key2) { return this->add(findSet_(key2), findSet_(key1)); } - - // the in-place version of makeUnion - void makeUnionInPlace(const KEY& key1, const KEY& key2) { *this = this->add(findSet_(key2), findSet_(key1)); } - - // create a new singleton with two connected keys - Self makePair(const KEY& key1, const KEY& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); } - - // create a new singleton with a list of fully connected keys - Self makeList(const std::list& keys) const { - Self t = *this; - BOOST_FOREACH(const KEY& key, keys) - t = t.makePair(key, keys.front()); - return t; - } - - // return a dsf in which all find_set operations will be O(1) due to path compression. - DSF flatten() const { - DSF t = *this; - BOOST_FOREACH(const KeyLabel& pair, (Tree)t) - t.findSet_(pair.first); - return t; - } - - // maps f over all keys, must be invertible - DSF map(boost::function func) const { - DSF t; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - t = t.add(func(pair.first), func(pair.second)); - return t; - } - - // return the number of sets - size_t numSets() const { - size_t num = 0; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - if (pair.first == pair.second) num++; - return num; - } - - // return the numer of keys - size_t size() const { return Tree::size(); } - - // return all sets, i.e. a partition of all elements - std::map sets() const { - std::map sets; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - sets[findSet(pair.second)].insert(pair.first); - return sets; - } - - // return a partition of the given elements {keys} - std::map partition(const std::list& keys) const { - std::map partitions; - BOOST_FOREACH(const KEY& key, keys) - partitions[findSet(key)].insert(key); - return partitions; - } - - // get the nodes in the tree with the given label - Set set(const Label& label) const { - Set set; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) { - if (pair.second == label || findSet(pair.second) == label) - set.insert(pair.first); - } - return set; - } - - /** equality */ - bool operator==(const Self& t) const { return (Tree)*this == (Tree)t; } - - /** inequality */ - bool operator!=(const Self& t) const { return (Tree)*this != (Tree)t; } - - // print the object - void print(const std::string& name = "DSF") const { - std::cout << name << std::endl; - BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) - std::cout << (std::string)pair.first << " " << (std::string)pair.second << std::endl; - } - - protected: - - /** - * same as findSet except with path compression: After we have traversed the path to - * the root, each parent pointer is made to directly point to it - */ - KEY findSet_(const KEY& key) { - KEY parent = this->find(key); - if (parent == key) - return parent; - else { - KEY label = findSet_(parent); - *this = this->add(key, label); - return label; - } - } - - }; - - // shortcuts - typedef DSF DSFInt; - -} // namespace gtsam diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp deleted file mode 100644 index ceb33fc23..000000000 --- a/gtsam/base/DSFVector.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 DSFVector.cpp - * @date Jun 25, 2010 - * @author Kai Ni - * @brief a faster implementation for DSF, which uses vector rather than btree. - */ - -#include -#include -#include - -using namespace std; - -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); - } - } - - /* ************************************************************************* */ - 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; - } - - /* ************************************************************************* */ - 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; - } - - /* ************************************************************************* */ - 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::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 > 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; - } - - /* ************************************************************************* */ - void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) { - (*v_)[findSet(i2)] = findSet(i1); - } - -} // namespace - diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h deleted file mode 100644 index 17e897032..000000000 --- a/gtsam/base/DSFVector.h +++ /dev/null @@ -1,79 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 DSFVector.h - * @date Jun 25, 2010 - * @author Kai Ni - * @brief A faster implementation for DSF, which uses vector rather than btree. As a result, the size of the forest is prefixed. - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - - /** - * A fast implementation of disjoint set forests that uses vector as underly data structure. - * @ingroup 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 - - 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); - - /// 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 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; - - /// 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; - - /// the in-place version of makeUnion - void makeUnionInPlace(const size_t& i1, const size_t& i2); - - }; - -} diff --git a/gtsam/base/FixedVector.h b/gtsam/base/FixedVector.h deleted file mode 100644 index 4faa670b6..000000000 --- a/gtsam/base/FixedVector.h +++ /dev/null @@ -1,118 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 FixedVector.h - * @brief Extension of boost's bounded_vector to allow for fixed size operation - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * Fixed size vectors - compatible with boost vectors, but with compile-type - * size checking. - */ -template -class FixedVector : public Eigen::Matrix { -public: - typedef Eigen::Matrix Base; - - /** default constructor */ - FixedVector() {} - - /** copy constructors */ - FixedVector(const FixedVector& v) : Base(v) {} - - /** Convert from a variable-size vector to a fixed size vector */ - FixedVector(const Vector& v) : Base(v) {} - - /** Initialize with a C-style array */ - FixedVector(const double* values) { - std::copy(values, values+N, this->data()); - } - - /** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - * - * NOTE: this will throw warnings/explode if there is no argument - * before the variadic section, so there is a meaningless size argument. - */ - FixedVector(size_t n, ...) { - va_list ap; - va_start(ap, n); - for(size_t i = 0 ; i < N ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); - } - - /** - * Create vector initialized to a constant value - * @param constant value - */ - inline static FixedVector repeat(double value) { - return FixedVector(Base::Constant(value)); - } - - /** - * Create basis vector of - * with a constant in spot i - * @param index of the one - * @param value is the value to insert into the vector - * @return delta vector - */ - inline static FixedVector delta(size_t i, double value) { - return FixedVector(Base::Unit(i) * value); - } - - /** - * Create basis vector, - * with one in spot i - * @param index of the one - * @return basis vector - */ - inline static FixedVector basis(size_t i) { return FixedVector(Base::Unit(i)); } - - /** - * Create zero vector - */ - inline static FixedVector zero() { return FixedVector(Base::Zero());} - - /** - * Create vector initialized to ones - */ - inline static FixedVector ones() { return FixedVector(FixedVector::Ones());} - - static size_t dim() { return Base::max_size; } - - void print(const std::string& name="") const { gtsam::print(Vector(*this), name); } - - template - bool equals(const FixedVector& other, double tol=1e-9) const { - return false; - } - - bool equals(const FixedVector& other, double tol=1e-9) const { - return equal_with_abs_tol(*this,other,tol); - } - -}; - - -} // \namespace diff --git a/gtsam/base/tests/testBTree.cpp b/gtsam/base/tests/testBTree.cpp deleted file mode 100644 index da3003bb5..000000000 --- a/gtsam/base/tests/testBTree.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBTree.cpp - * @date Feb 3, 2010 - * @author Chris Beall - * @author Frank Dellaert - */ - -#include -#include -#include // for += -using namespace boost::assign; - -#include -#include - -using namespace std; -using namespace gtsam; - -typedef pair Range; -typedef BTree RangeTree; -typedef BTree IntTree; - -static std::stringstream ss; -static string x1("x1"), x2("x2"), x3("x3"), x4("x4"), x5("x5"); -typedef pair KeyInt; -KeyInt p1(x1, 1), p2(x2, 2), p3(x3, 3), p4(x4, 4), p5(x5, 5); - -/* ************************************************************************* */ -int f(const string& key, const Range& range) { - return range.first; -} - -void g(const string& key, int i) { - ss << (string) key; -} - -int add(const string& k, int v, int a) { - return v + a; -} - -/* ************************************************************************* */ -TEST( BTree, add ) -{ - RangeTree tree; - CHECK(tree.empty()) - LONGS_EQUAL(0,tree.height()) - - // check the height of tree after adding an element - RangeTree tree1 = tree.add(x1, Range(1, 1)); - LONGS_EQUAL(1,tree1.height()) - LONGS_EQUAL(1,tree1.size()) - CHECK(tree1.find(x1) == Range(1,1)) - - RangeTree tree2 = tree1.add(x5, Range(5, 2)); - RangeTree tree3 = tree2.add(x3, Range(3, 3)); - LONGS_EQUAL(3,tree3.size()) - CHECK(tree3.find(x5) == Range(5,2)) - CHECK(tree3.find(x3) == Range(3,3)) - - RangeTree tree4 = tree3.add(x2, Range(2, 4)); - RangeTree tree5 = tree4.add(x4, Range(4, 5)); - LONGS_EQUAL(5,tree5.size()) - CHECK(tree5.find(x4) == Range(4,5)) - - // Test functional nature: tree5 and tree6 have different values for x4 - RangeTree tree6 = tree5.add(x4, Range(6, 6)); - CHECK(tree5.find(x4) == Range(4,5)) - CHECK(tree6.find(x4) == Range(6,6)) - - // test assignment - RangeTree c5 = tree5; - LONGS_EQUAL(5,c5.size()) - CHECK(c5.find(x4) == Range(4,5)) - - // test map - // After (map f tree5) tree contains (x1,1), (x2,2), etc... - IntTree mapped = tree5.map (f); - LONGS_EQUAL(2,mapped.find(x2)); - LONGS_EQUAL(4,mapped.find(x4)); -} - -/* ************************************************************************* */ -TEST( BTree, equality ) -{ - IntTree tree1 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5); - CHECK(tree1==tree1) - CHECK(tree1.same(tree1)) - - IntTree tree2 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5); - CHECK(tree2==tree1) - CHECK(!tree2.same(tree1)) - - IntTree tree3 = IntTree().add(p1).add(p2).add(p3).add(p4); - CHECK(tree3!=tree1) - CHECK(tree3!=tree2) - CHECK(!tree3.same(tree1)) - CHECK(!tree3.same(tree2)) - - IntTree tree4 = tree3.add(p5); - CHECK(tree4==tree1) - CHECK(!tree4.same(tree1)) - - IntTree tree5 = tree1; - CHECK(tree5==tree1) - CHECK(tree5==tree2) - CHECK(tree5.same(tree1)) - CHECK(!tree5.same(tree2)) -} - -/* ************************************************************************* */ -TEST( BTree, iterating ) -{ - IntTree tree = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5); - - // test iter - tree.iter(g); - CHECK(ss.str() == string("x1x2x3x4x5")); - - // test fold - LONGS_EQUAL(25,tree.fold(add,10)) - - // test iterator - BTree::const_iterator it = tree.begin(), it2 = tree.begin(); - CHECK(it==it2) - CHECK(*it == p1) - CHECK(it->first == x1) - CHECK(it->second == 1) - CHECK(*(++it) == p2) - CHECK(it!=it2) - CHECK(it==(++it2)) - CHECK(*(++it) == p3) - CHECK(*(it++) == p3) - // post-increment, not so efficient - CHECK(*it == p4) - CHECK(*(++it) == p5) - CHECK((++it)==tree.end()) - - // acid iterator test: BOOST_FOREACH - int sum = 0; - BOOST_FOREACH(const KeyInt& p, tree) -sum += p.second; - LONGS_EQUAL(15,sum) - - // STL iterator test - list expected, actual; - expected += p1,p2,p3,p4,p5; - copy (tree.begin(),tree.end(),back_inserter(actual)); - CHECK(actual==expected) -} - -/* ************************************************************************* */ -TEST( BTree, remove ) -{ - IntTree tree5 = IntTree().add(p1).add(p2).add(p3).add(p4).add(p5); - LONGS_EQUAL(5,tree5.size()) - CHECK(tree5.mem(x3)) - IntTree tree4 = tree5.remove(x3); - LONGS_EQUAL(4,tree4.size()) - CHECK(!tree4.mem(x3)) -} - -/* ************************************************************************* */ -TEST( BTree, stress ) -{ - RangeTree tree; - list expected; - int N = 128; - for (int i = 1; i <= N; i++) { - string key('a', i); - Range value(i - 1, i); - tree = tree.add(key, value); - LONGS_EQUAL(i,tree.size()) - CHECK(tree.find(key) == value) - expected += make_pair(key, value); - } - - // Check height is log(N) - LONGS_EQUAL(8,tree.height()) - - // stress test iterator - list actual; - copy(tree.begin(), tree.end(), back_inserter(actual)); - CHECK(actual==expected) - - // deconstruct the tree - for (int i = N; i >= N; i--) { - string key('a', i); - tree = tree.remove(key); - LONGS_EQUAL(i-1,tree.size()) - CHECK(!tree.mem(key)) - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/base/tests/testDSF.cpp b/gtsam/base/tests/testDSF.cpp deleted file mode 100644 index fd5ca6e84..000000000 --- a/gtsam/base/tests/testDSF.cpp +++ /dev/null @@ -1,280 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testDSF.cpp - * @date Mar 26, 2010 - * @author nikai - * @brief unit tests for DSF - */ - -#include -#include -#include -using namespace boost::assign; -#include - -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(DSF, makeSet) { - DSFInt dsf; - dsf = dsf.makeSet(5); - LONGS_EQUAL(1, dsf.size()); -} - -/* ************************************************************************* */ -TEST(DSF, findSet) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - CHECK(dsf.findSet(5) != dsf.findSet(7)); -} - -/* ************************************************************************* */ -TEST(DSF, makeUnion) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); -} - -/* ************************************************************************* */ -TEST(DSF, makeUnion2) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(7,5); - CHECK(dsf.findSet(5) == dsf.findSet(7)); -} - -/* ************************************************************************* */ -TEST(DSF, makeUnion3) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - dsf = dsf.makeUnion(6,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); -} - -/* ************************************************************************* */ -TEST(DSF, makePair) { - DSFInt dsf; - dsf = dsf.makePair(0, 1); - dsf = dsf.makePair(1, 2); - dsf = dsf.makePair(3, 2); - CHECK(dsf.findSet(0) == dsf.findSet(3)); -} - -/* ************************************************************************* */ -TEST(DSF, makeList) { - DSFInt dsf; - list keys; keys += 5, 6, 7; - dsf = dsf.makeList(keys); - CHECK(dsf.findSet(5) == dsf.findSet(7)); -} - -/* ************************************************************************* */ -TEST(DSF, numSets) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - LONGS_EQUAL(2, dsf.numSets()); -} - -/* ************************************************************************* */ -TEST(DSF, sets) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeUnion(5,6); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); -} - -/* ************************************************************************* */ -TEST(DSF, sets2) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - dsf = dsf.makeUnion(6,7); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); - - set expected; expected += 5, 6, 7; - CHECK(expected == sets[dsf.findSet(5)]); -} - -/* ************************************************************************* */ -TEST(DSF, sets3) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - map > sets = dsf.sets(); - LONGS_EQUAL(2, sets.size()); - - set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); -} - -/* ************************************************************************* */ -TEST(DSF, partition) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeUnion(5,6); - - list keys; keys += 5; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(1, partitions.size()); - - set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); -} - -/* ************************************************************************* */ -TEST(DSF, partition2) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - - list keys; keys += 7; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(1, partitions.size()); - - set expected; expected += 7; - CHECK(expected == partitions[dsf.findSet(7)]); -} - -/* ************************************************************************* */ -TEST(DSF, partition3) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - - list keys; keys += 5, 7; - map > partitions = dsf.partition(keys); - LONGS_EQUAL(2, partitions.size()); - - set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); -} - -/* ************************************************************************* */ -TEST(DSF, set) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - set set = dsf.set(5); - LONGS_EQUAL(2, set.size()); - - std::set expected; expected += 5, 6; - CHECK(expected == set); -} - -/* ************************************************************************* */ -TEST(DSF, set2) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - dsf = dsf.makeUnion(6,7); - set set = dsf.set(5); - LONGS_EQUAL(3, set.size()); - - std::set expected; expected += 5, 6, 7; - CHECK(expected == set); -} - -/* ************************************************************************* */ -int func(const int& a) { return a + 10; } -TEST(DSF, map) { - DSFInt dsf; - dsf = dsf.makeSet(5); - dsf = dsf.makeSet(6); - dsf = dsf.makeSet(7); - dsf = dsf.makeUnion(5,6); - - DSFInt actual = dsf.map(&func); - DSFInt expected; - expected = expected.makeSet(15); - expected = expected.makeSet(16); - expected = expected.makeSet(17); - expected = expected.makeUnion(15,16); - CHECK(actual == expected); -} - -/* ************************************************************************* */ -TEST(DSF, flatten) { - DSFInt dsf; - dsf = dsf.makePair(1, 2); - dsf = dsf.makePair(2, 3); - dsf = dsf.makePair(5, 6); - dsf = dsf.makePair(6, 7); - dsf = dsf.makeUnion(2, 6); - - DSFInt actual = dsf.flatten(); - DSFInt expected; - expected = expected.makePair(1, 2); - expected = expected.makePair(1, 3); - expected = expected.makePair(1, 5); - expected = expected.makePair(1, 6); - expected = expected.makePair(1, 7); - CHECK(actual == expected); -} - -/* ************************************************************************* */ -TEST(DSF, flatten2) { - static string x1("x1"), x2("x2"), x3("x3"), x4("x4"); - list keys; keys += x1,x2,x3,x4; - DSF dsf(keys); - dsf = dsf.makeUnion(x1,x2); - dsf = dsf.makeUnion(x3,x4); - dsf = dsf.makeUnion(x1,x3); - - CHECK(dsf != dsf.flatten()); - - DSF expected2; - expected2 = expected2.makePair(x1, x2); - expected2 = expected2.makePair(x1, x3); - expected2 = expected2.makePair(x1, x4); - CHECK(expected2 == dsf.flatten()); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ - diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp deleted file mode 100644 index c0b72f1a0..000000000 --- a/gtsam/base/tests/testDSFVector.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testDSF.cpp - * @date June 25, 2010 - * @author nikai - * @brief unit tests for DSF - */ - -#include -#include -#include -#include -#include -using namespace boost::assign; -#include - -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(DSFVectorVector, findSet) { - 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)); -} - -/* ************************************************************************* */ -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)); -} - -/* ************************************************************************* */ -TEST(DSFVector, makeUnion2) { - 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)); -} - -/* ************************************************************************* */ -TEST(DSFVector, sets) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, arrays) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, sets2) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, arrays2) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, sets3) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, arrays3) { - 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)]); -} - -/* ************************************************************************* */ -TEST(DSFVector, set) { - 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); -} - -/* ************************************************************************* */ -TEST(DSFVector, set2) { - 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); -} - -/* ************************************************************************* */ -TEST(DSFVector, isSingleton) { - 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/testFixedVector.cpp b/gtsam/base/tests/testFixedVector.cpp deleted file mode 100644 index 85a963c89..000000000 --- a/gtsam/base/tests/testFixedVector.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testFixedVector.cpp - * @author Alex Cunningham - */ - -#include - -#include - -using namespace gtsam; - -typedef FixedVector<5> Vector5; -typedef FixedVector<3> Vector3; - -static const double tol = 1e-9; - -/* ************************************************************************* */ -TEST( testFixedVector, conversions ) { - double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = Vector_(3, data1); - Vector3 fv1(v1), fv2(data1); - - Vector actFv2(fv2); - CHECK(assert_equal(v1, actFv2)); -} - -/* ************************************************************************* */ -TEST( testFixedVector, variable_constructor ) { - Vector3 act(3, 1.0, 2.0, 3.0); - DOUBLES_EQUAL(1.0, act(0), tol); - DOUBLES_EQUAL(2.0, act(1), tol); - DOUBLES_EQUAL(3.0, act(2), tol); -} - -/* ************************************************************************* */ -TEST( testFixedVector, equals ) { - Vector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); - Vector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); - - CHECK(assert_equal(vec1, vec1, tol)); - CHECK(assert_equal(vec1, vec2, tol)); - CHECK(assert_equal(vec2, vec1, tol)); - CHECK(!vec1.equals(vec3, tol)); - CHECK(!vec3.equals(vec1, tol)); - CHECK(!vec1.equals(vec4, tol)); - CHECK(!vec4.equals(vec1, tol)); -} - -/* ************************************************************************* */ -TEST( testFixedVector, static_constructors ) { - Vector3 actZero = Vector3::zero(); - Vector3 expZero(3, 0.0, 0.0, 0.0); - CHECK(assert_equal(expZero, actZero, tol)); - - Vector3 actOnes = Vector3::ones(); - Vector3 expOnes(3, 1.0, 1.0, 1.0); - CHECK(assert_equal(expOnes, actOnes, tol)); - - Vector3 actRepeat = Vector3::repeat(2.3); - Vector3 expRepeat(3, 2.3, 2.3, 2.3); - CHECK(assert_equal(expRepeat, actRepeat, tol)); - - Vector3 actBasis = Vector3::basis(1); - Vector3 expBasis(3, 0.0, 1.0, 0.0); - CHECK(assert_equal(expBasis, actBasis, tol)); - - Vector3 actDelta = Vector3::delta(1, 2.3); - Vector3 expDelta(3, 0.0, 2.3, 0.0); - CHECK(assert_equal(expDelta, actDelta, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - diff --git a/gtsam/geometry/Tensor1.h b/gtsam/geometry/Tensor1.h deleted file mode 100644 index 685166b53..000000000 --- a/gtsam/geometry/Tensor1.h +++ /dev/null @@ -1,85 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor1.h - * @brief Rank 1 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * A rank 1 tensor. Actually stores data. - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor1 { - double T[N]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor1() { - } - - /** construct from data */ - Tensor1(const double* data) { - for (int i = 0; i < N; i++) - T[i] = data[i]; - } - - /** construct from expression */ - template - Tensor1(const Tensor1Expression >& a) { - for (int i = 0; i < N; i++) - T[i] = a(i); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** return data */ - inline int dim() const { - return N; - } - - /** return data */ - inline const double& operator()(int i) const { - return T[i]; - } - - /** return data */ - inline double& operator()(int i) { - return T[i]; - } - - /// return an expression associated with an index - template Tensor1Expression > operator()( - Index index) const { - return Tensor1Expression >(*this); - } - - /// @} - - }; -// Tensor1 - -}// namespace tensors diff --git a/gtsam/geometry/Tensor1Expression.h b/gtsam/geometry/Tensor1Expression.h deleted file mode 100644 index 4e43fff58..000000000 --- a/gtsam/geometry/Tensor1Expression.h +++ /dev/null @@ -1,181 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor1Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include - -namespace tensors { - - /** - * Templated class to provide a rank 1 tensor interface to a class. - * This class does not store any data but the result of an expression. - * It is associated with an index. - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor1Expression { - - private: - - A iter; - - typedef Tensor1Expression This; - - /** Helper class for multiplying with a double */ - class TimesDouble_ { - A iter; - const double s; - public: - /// Constructor - TimesDouble_(const A &a, double s_) : - iter(a), s(s_) { - } - /// Element access - inline double operator()(int i) const { - return iter(i) * s; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor1Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string s = "") const { - std::cout << s << "{"; - std::cout << (*this)(0); - for (int i = 1; i < I::dim; i++) - std::cout << ", "<< (*this)(i); - std::cout << "}" << std::endl; - } - - /// equality - template - bool equals(const Tensor1Expression & q, double tol) const { - for (int i = 0; i < I::dim; i++) - if (fabs((*this)(i) - q(i)) > tol) return false; - return true; - } - - /// @} - /// @name Standard Interface - /// @{ - - /** norm */ - double norm() const { - double sumsqr = 0.0; - for (int i = 0; i < I::dim; i++) - sumsqr += iter(i) * iter(i); - return sqrt(sumsqr); - } - - /// test equivalence - template - bool equivalent(const Tensor1Expression & q, double tol = 1e-9) const { - return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol) - || ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol); - } - - /** Check if two expressions are equal */ - template - bool operator==(const Tensor1Expression& e) const { - for (int i = 0; i < I::dim; i++) - if (iter(i) != e(i)) return false; - return true; - } - - /** element access */ - double operator()(int i) const { - return iter(i); - } - - /** mutliply with a double. */ - inline Tensor1Expression operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Class for contracting two rank 1 tensor expressions, yielding a double. */ - template - inline double operator*(const Tensor1Expression &b) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += (*this)(i) * b(i); - return sum; - } - - }; // Tensor1Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print a rank 1 expression */ - template - void print(const Tensor1Expression& T, const std::string s = "") { - T.print(s); - } - - /** norm */ - template - double norm(const Tensor1Expression& T) { - return T.norm(); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor1Expression& expected, - const Tensor1Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /** - * This template works for any two expressions - */ - template - bool assert_equivalent(const Tensor1Expression& expected, - const Tensor1Expression& actual, double tol = 1e-9) { - if (actual.equivalent(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor2.h b/gtsam/geometry/Tensor2.h deleted file mode 100644 index 12fd1509f..000000000 --- a/gtsam/geometry/Tensor2.h +++ /dev/null @@ -1,84 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor2.h - * @brief Rank 2 Tensor based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - -/** - * Rank 2 Tensor - * @ingroup tensors - * \nosubgrouping - */ -template -class Tensor2 { -protected: - Tensor1 T[N2]; ///< Storage - -public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor2() { - } - - /// construct from data - expressed in row major form - Tensor2(const double data[N2][N1]) { - for (int j = 0; j < N2; j++) - T[j] = Tensor1 (data[j]); - } - - /** construct from expression */ - template - Tensor2(const Tensor2Expression , Index >& a) { - for (int j = 0; j < N2; j++) - T[j] = a(j); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** dimension - TODO: is this right for anything other than 3x3? */ - size_t dim() const {return N1 * N2;} - - /// const element access - const double & operator()(int i, int j) const { - return T[j](i); - } - - /// element access - double & operator()(int i, int j) { - return T[j](i); - } - - /** convert to expression */ - template Tensor2Expression , Index< - N2, J> > operator()(Index i, Index j) const { - return Tensor2Expression , Index > (*this); - } - - /// @} - -}; - -} // namespace tensors - diff --git a/gtsam/geometry/Tensor2Expression.h b/gtsam/geometry/Tensor2Expression.h deleted file mode 100644 index 6c953a5aa..000000000 --- a/gtsam/geometry/Tensor2Expression.h +++ /dev/null @@ -1,310 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor2Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace tensors { - - /** - * Templated class to hold a rank 2 tensor expression. - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor2Expression { - - private: - - A iter; - - typedef Tensor2Expression This; - - /** Helper class for instantiating one index */ - class FixJ_ { - const int j; - const A iter; - public: - FixJ_(int j_, const A &a) : - j(j_), iter(a) { - } - double operator()(int i) const { - return iter(i, j); - } - }; - - /** Helper class for swapping indices */ - class Swap_ { - const A iter; - public: - /// Constructor - Swap_(const A &a) : - iter(a) { - } - /// Element access - double operator()(int j, int i) const { - return iter(i, j); - } - }; - - /** Helper class for multiplying with a double */ - class TimesDouble_ { - A iter; - const double s; - public: - /// Constructor - TimesDouble_(const A &a, double s_) : - iter(a), s(s_) { - } - /// Element access - inline double operator()(int i, int j) const { - return iter(i, j) * s; - } - }; - - /** Helper class for contracting index I with rank 1 tensor */ - template class ITimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - ITimesRank1_(const This &a_, const Tensor1Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int j) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += a(i, j) * b(i); - return sum; - } - }; - - /** Helper class for contracting index J with rank 1 tensor */ - template class JTimesRank1_ { - const This a; - const Tensor1Expression b; - public: - /// Constructor - JTimesRank1_(const This &a_, const Tensor1Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int i) const { - double sum = 0.0; - for (int j = 0; j < J::dim; j++) - sum += a(i, j) * b(j); - return sum; - } - }; - - /** Helper class for contracting index I with rank 2 tensor */ - template class ITimesRank2_ { - const This a; - const Tensor2Expression b; - public: - /// Constructor - ITimesRank2_(const This &a_, const Tensor2Expression &b_) : - a(a_), b(b_) { - } - /// Element access - double operator()(int j, int k) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += a(i, j) * b(i, k); - return sum; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor2Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor2:") const { - std::cout << s << "{"; - (*this)(0).print(); - for (int j = 1; j < J::dim; j++) { - std::cout << ","; - (*this)(j).print(""); - } - std::cout << "}" << std::endl; - } - - /// test equality - template - bool equals(const Tensor2Expression & q, double tol) const { - for (int j = 0; j < J::dim; j++) - if (!(*this)(j).equals(q(j), tol)) - return false; - return true; - } - - /// @} - /// @name Standard Interface - /// @{ - - /** norm */ - double norm() const { - double sumsqr = 0.0; - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - sumsqr += iter(i, j) * iter(i, j); - return sqrt(sumsqr); - } - - /// test equivalence - template - bool equivalent(const Tensor2Expression & q, double tol) const { - return ((*this) * (1.0 / norm())).equals(q * (1.0 / q.norm()), tol) - || ((*this) * (-1.0 / norm())).equals(q * (1.0 / q.norm()), tol); - } - - /** element access */ - double operator()(int i, int j) const { - return iter(i, j); - } - - /** swap indices */ - typedef Tensor2Expression Swapped; - /// Return Swap_ helper class - Swapped swap() { - return Swap_(iter); - } - - /** mutliply with a double. */ - inline Tensor2Expression operator*(double s) const { - return TimesDouble_(iter, s); - } - - /** Fix a single index */ - Tensor1Expression operator()(int j) const { - return FixJ_(j, iter); - } - - /** Check if two expressions are equal */ - template - bool operator==(const Tensor2Expression& T) const { - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - if (iter(i, j) != T(i, j)) - return false; - return true; - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** c(j) = a(i,j)*b(i) */ - template - inline Tensor1Expression, J> operator*( - const Tensor1Expression& p) { - return ITimesRank1_(*this, p); - } - - /** c(i) = a(i,j)*b(j) */ - template - inline Tensor1Expression, I> operator*( - const Tensor1Expression &p) { - return JTimesRank1_(*this, p); - } - - /** c(j,k) = a(i,j)*T(i,k) */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor2Expression& p) { - return ITimesRank2_(*this, p); - } - - }; - // Tensor2Expression - - /** Print */ - template - void print(const Tensor2Expression& T, const std::string& s = - "Tensor2:") { - T.print(s); - } - - /** Helper class for multiplying two covariant tensors */ - template class Rank1Rank1_ { - const Tensor1Expression iterA; - const Tensor1Expression iterB; - public: - /// Constructor - Rank1Rank1_(const Tensor1Expression &a, - const Tensor1Expression &b) : - iterA(a), iterB(b) { - } - /// element access - double operator()(int i, int j) const { - return iterA(i) * iterB(j); - } - }; - - /** Multiplying two different indices yields an outer product */ - template - inline Tensor2Expression , I, J> operator*( - const Tensor1Expression &a, const Tensor1Expression &b) { - return Rank1Rank1_(a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor2Expression& expected, - const Tensor2Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) - return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /** - * This template works for any two expressions - */ - template - bool assert_equivalent(const Tensor2Expression& expected, - const Tensor2Expression& actual, double tol = 1e-9) { - if (actual.equivalent(expected, tol)) - return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor3.h b/gtsam/geometry/Tensor3.h deleted file mode 100644 index 78cdcb9f2..000000000 --- a/gtsam/geometry/Tensor3.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor3.h - * @brief Rank 3 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author: Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 3 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor3 { - Tensor2 T[N3]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor3() { - } - - /** construct from data */ - Tensor3(const double data[N3][N2][N1]) { - for (int k = 0; k < N3; k++) - T[k] = data[k]; - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** construct from expression */ - template - Tensor3(const Tensor3Expression , Index , Index >& a) { - for (int k = 0; k < N3; k++) - T[k] = a(k); - } - - /// @} - /// @name Standard Interface - /// @{ - - /// element access - double operator()(int i, int j, int k) const { - return T[k](i, j); - } - - /** convert to expression */ - template Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) { - return Tensor3Expression , Index , Index > (*this); - } - - /** convert to expression */ - template Tensor3Expression , - Index , Index > operator()(const Index& i, - const Index& j, const Index& k) const { - return Tensor3Expression , Index , Index > (*this); - } - }; // Tensor3 - - /** Rank 3 permutation tensor */ - struct Eta3 { - - /** calculate value. TODO: wasteful to actually use this */ - double operator()(int i, int j, int k) const { - return ((j - i) * (k - i) * (k - j)) / 2; - } - - /** create expression */ - template Tensor3Expression , - Index<3, J> , Index<3, K> > operator()(const Index<3, I>& i, - const Index<3, J>& j, const Index<3, K>& k) const { - return Tensor3Expression , Index<3, J> , Index<3, K> > ( - *this); - } - - }; // Eta - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor3Expression.h b/gtsam/geometry/Tensor3Expression.h deleted file mode 100644 index 0356caa6f..000000000 --- a/gtsam/geometry/Tensor3Expression.h +++ /dev/null @@ -1,194 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor3Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 3 tensor - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor3Expression { - A iter; - - typedef Tensor3Expression This; - - /** Helper class for instantiating one index */ - class FixK_ { - const int k; - const A iter; - public: - FixK_(int k_, const A &a) : - k(k_), iter(a) { - } - double operator()(int i, int j) const { - return iter(i, j, k); - } - }; - - /** Helper class for contracting rank3 and rank1 tensor */ - template class TimesRank1_ { - typedef Tensor1Expression Rank1; - const This T; - const Rank1 t; - public: - TimesRank1_(const This &a, const Rank1 &b) : - T(a), t(b) { - } - double operator()(int j, int k) const { - double sum = 0.0; - for (int i = 0; i < I::dim; i++) - sum += T(i, j, k) * t(i); - return sum; - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor3Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor3:") const { - std::cout << s << "{"; - (*this)(0).print(""); - for (int k = 1; k < K::dim; k++) { - std::cout << ","; - (*this)(k).print(""); - } - std::cout << "}" << std::endl; - } - - /// test equality - template - bool equals(const Tensor3Expression & q, double tol) const { - for (int k = 0; k < K::dim; k++) - if (!(*this)(k).equals(q(k), tol)) return false; - return true; - } - - /** element access */ - double operator()(int i, int j, int k) const { - return iter(i, j, k); - } - - /** Fix a single index */ - Tensor2Expression operator()(int k) const { - return FixK_(k, iter); - } - - /** Contracting with rank1 tensor */ - template - inline Tensor2Expression , J, K> operator*( - const Tensor1Expression &b) const { - return TimesRank1_ (*this, b); - } - - }; // Tensor3Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print */ - template - void print(const Tensor3Expression& T, const std::string& s = - "Tensor3:") { - T.print(s); - } - - /** Helper class for outer product of rank2 and rank1 tensor */ - template - class Rank2Rank1_ { - typedef Tensor2Expression Rank2; - typedef Tensor1Expression Rank1; - const Rank2 iterA; - const Rank1 iterB; - public: - /// Constructor - Rank2Rank1_(const Rank2 &a, const Rank1 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k) const { - return iterA(i, j) * iterB(k); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor3Expression , I, J, K> operator*( - const Tensor2Expression& a, const Tensor1Expression &b) { - return Rank2Rank1_ (a, b); - } - - /** Helper class for outer product of rank1 and rank2 tensor */ - template - class Rank1Rank2_ { - typedef Tensor1Expression Rank1; - typedef Tensor2Expression Rank2; - const Rank1 iterA; - const Rank2 iterB; - public: - /// Constructor - Rank1Rank2_(const Rank1 &a, const Rank2 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k) const { - return iterA(i) * iterB(j, k); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor3Expression , I, J, K> operator*( - const Tensor1Expression& a, const Tensor2Expression &b) { - return Rank1Rank2_ (a, b); - } - - /** - * This template works for any two expressions - */ - template - bool assert_equality(const Tensor3Expression& expected, - const Tensor3Expression& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) return true; - std::cout << "Not equal:\n"; - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/Tensor4.h b/gtsam/geometry/Tensor4.h deleted file mode 100644 index 26a52750c..000000000 --- a/gtsam/geometry/Tensor4.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor4.h - * @brief Rank 4 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 4 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor4 { - - private: - - Tensor3 T[N4]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor4() { - } - - /// @} - /// @name Standard Interface - /// @{ - - /// element access - double operator()(int i, int j, int k, int l) const { - return T[l](i, j, k); - } - - /// @} - - }; // Tensor4 - -} // namespace tensors diff --git a/gtsam/geometry/Tensor5.h b/gtsam/geometry/Tensor5.h deleted file mode 100644 index e12f7d2fb..000000000 --- a/gtsam/geometry/Tensor5.h +++ /dev/null @@ -1,75 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor5.h - * @brief Rank 5 tensors based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once -#include - -namespace tensors { - - /** - * Rank 5 Tensor - * @ingroup tensors - * \nosubgrouping - */ - template - class Tensor5 { - - private: - - Tensor4 T[N5]; ///< Storage - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - Tensor5() { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** construct from expression */ - template - Tensor5(const Tensor5Expression , Index , Index , Index , Index >& a) { - for (int m = 0; m < N5; m++) - T[m] = a(m); - } - - /// element access - double operator()(int i, int j, int k, int l, int m) const { - return T[m](i, j, k, l); - } - - /** convert to expression */ - template Tensor5Expression , Index , Index , Index , - Index > operator()(Index i, Index j, - Index k, Index l, Index m) { - return Tensor5Expression , Index , Index , Index , Index > (*this); - } - - /// @} - - }; // Tensor5 - -} // namespace tensors diff --git a/gtsam/geometry/Tensor5Expression.h b/gtsam/geometry/Tensor5Expression.h deleted file mode 100644 index e9f0e852a..000000000 --- a/gtsam/geometry/Tensor5Expression.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Tensor5Expression.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace tensors { - - /** - * templated class to interface to an object A as a rank 5 tensor - * @ingroup tensors - * \nosubgrouping - */ - template class Tensor5Expression { - A iter; - - typedef Tensor5Expression This; - - /** Helper class for swapping indices 3 and 4 :-) */ - class Swap34_ { - const A iter; - public: - /// Constructor - Swap34_(const A &a) : - iter(a) { - } - /// swapping element access - double operator()(int i, int j, int k, int l, int m) const { - return iter(i, j, l, k, m); - } - }; - - public: - - /// @name Standard Constructors - /// @{ - - /** constructor */ - Tensor5Expression(const A &a) : - iter(a) { - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Print */ - void print(const std::string& s = "Tensor5:") const { - std::cout << s << std::endl; - for (int m = 0; m < M::dim; m++) - for (int l = 0; l < L::dim; l++) - for (int k = 0; k < K::dim; k++) { - std::cout << "(m,l,k) = (" << m << "," << l << "," << k << ")" - << std::endl; - for (int j = 0; j < J::dim; j++) { - for (int i = 0; i < I::dim; i++) - std::cout << " " << (*this)(i, j, k, l, m); - std::cout << std::endl; - } - } - std::cout << std::endl; - } - - /** swap indices */ - typedef Tensor5Expression Swapped; - /// create Swap34_ helper class - Swapped swap34() { - return Swap34_(iter); - } - - /** element access */ - double operator()(int i, int j, int k, int l, int m) const { - return iter(i, j, k, l, m); - } - - }; - // Tensor5Expression - - /// @} - /// @name Advanced Interface - /// @{ - - /** Print */ - template - void print(const Tensor5Expression& T, - const std::string& s = "Tensor5:") { - T.print(s); - } - - /** Helper class for outer product of rank3 and rank2 tensor */ - template - class Rank3Rank2_ { - typedef Tensor3Expression Rank3; - typedef Tensor2Expression Rank2; - const Rank3 iterA; - const Rank2 iterB; - public: - /// Constructor - Rank3Rank2_(const Rank3 &a, const Rank2 &b) : - iterA(a), iterB(b) { - } - /// Element access - double operator()(int i, int j, int k, int l, int m) const { - return iterA(i, j, k) * iterB(l, m); - } - }; - - /** outer product of rank2 and rank1 tensor */ - template - inline Tensor5Expression , I, J, K, L, M> operator*( - const Tensor3Expression& a, - const Tensor2Expression &b) { - return Rank3Rank2_(a, b); - } - - /// @} - -} // namespace tensors diff --git a/gtsam/geometry/projectiveGeometry.cpp b/gtsam/geometry/projectiveGeometry.cpp deleted file mode 100644 index 31c975c43..000000000 --- a/gtsam/geometry/projectiveGeometry.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 projectiveGeometry.cpp - * @brief Projective geometry, implemented using tensor library - * @date Feb 12, 2010 - * @author: Frank Dellaert - */ - -#include -#include - -#include -#include - -namespace gtsam { - - using namespace std; - using namespace tensors; - - /* ************************************************************************* */ - Point2h point2h(double x, double y, double w) { - double data[3]; - data[0] = x; - data[1] = y; - data[2] = w; - return data; - } - - /* ************************************************************************* */ - Line2h line2h(double a, double b, double c) { - double data[3]; - data[0] = a; - data[1] = b; - data[2] = c; - return data; - } - - /* ************************************************************************* */ - Point3h point3h(double X, double Y, double Z, double W) { - double data[4]; - data[0] = X; - data[1] = Y; - data[2] = Z; - data[3] = W; - return data; - } - - /* ************************************************************************* */ - Plane3h plane3h(double a, double b, double c, double d) { - double data[4]; - data[0] = a; - data[1] = b; - data[2] = c; - data[3] = d; - return data; - } - - - /* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/geometry/projectiveGeometry.h b/gtsam/geometry/projectiveGeometry.h deleted file mode 100644 index cfad515ac..000000000 --- a/gtsam/geometry/projectiveGeometry.h +++ /dev/null @@ -1,124 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 projectiveGeometry.h - * @brief Projective geometry, implemented using tensor library - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** - * 2D Point in homogeneous coordinates - * @ingroup geometry - */ - typedef tensors::Tensor1<3> Point2h; - Point2h point2h(double x, double y, double w); ///< create Point2h - - /** - * 2D Line in homogeneous coordinates - * @ingroup geometry - */ - typedef tensors::Tensor1<3> Line2h; - Line2h line2h(double a, double b, double c); ///< create Line2h - - /** - * 2D (homegeneous) Point correspondence - * @ingroup geometry - */ - struct Correspondence { - Point2h first; ///< First point - Point2h second; ///< Second point - - /// Create a correspondence pair - Correspondence(const Point2h &p1, const Point2h &p2) : - first(p1), second(p2) { - } - /// Swap points - Correspondence swap() const { - return Correspondence(second, first); - } - /// print - void print() { - tensors::Index<3, 'i'> i; - tensors::print(first(i), "first :"); - tensors::print(second(i), "second:"); - } - }; - - /** - * 2D-2D Homography - * @ingroup geometry - */ - typedef tensors::Tensor2<3, 3> Homography2; - - /** - * Fundamental Matrix - * @ingroup geometry - */ - typedef tensors::Tensor2<3, 3> FundamentalMatrix; - - /** - * Triplet of (homogeneous) 2D points - * @ingroup geometry - */ - struct Triplet { - Point2h first; ///< First point - Point2h second; ///< Second point - Point2h third; ///< Third point - - /// Create a Triplet correspondence - Triplet(const Point2h &p1, const Point2h &p2, const Point2h &p3) : - first(p1), second(p2), third(p3) { - } - /// print - void print() { - tensors::Index<3, 'i'> i; - tensors::print(first(i), "first :"); - tensors::print(second(i), "second:"); - tensors::print(third(i), "third :"); - } - }; - - /** - * Trifocal Tensor - * @ingroup geometry - */ - typedef tensors::Tensor3<3, 3, 3> TrifocalTensor; - - /** - * 3D Point in homogeneous coordinates - * @ingroup geometry - */ - typedef tensors::Tensor1<4> Point3h; - Point3h point3h(double X, double Y, double Z, double W); ///< create Point3h - - /** - * 3D Plane in homogeneous coordinates - * @ingroup geometry - */ - typedef tensors::Tensor1<4> Plane3h; - Plane3h plane3h(double a, double b, double c, double d); ///< create Plane3h - - /** - * 3D to 2D projective camera - * @ingroup geometry - */ - typedef tensors::Tensor2<3, 4> ProjectiveCamera; - -} // namespace gtsam diff --git a/gtsam/geometry/tensorInterface.cpp b/gtsam/geometry/tensorInterface.cpp deleted file mode 100644 index 683543e19..000000000 --- a/gtsam/geometry/tensorInterface.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 tensorInterface.cpp - * @brief Interfacing tensors template library and gtsam - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#include - -using namespace std; -using namespace tensors; - -namespace gtsam { - -} // namespace gtsam diff --git a/gtsam/geometry/tensorInterface.h b/gtsam/geometry/tensorInterface.h deleted file mode 100644 index 61212e2c9..000000000 --- a/gtsam/geometry/tensorInterface.h +++ /dev/null @@ -1,108 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 tensorInterface.h - * @brief Interfacing tensors template library and gtsam - * @date Feb 12, 2010 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Reshape rank 2 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor2Expression& T, int m, int n) { - if (m * n != I::dim * J::dim) throw std::invalid_argument( - "reshape: incompatible dimensions"); - MatrixRowMajor M(m, n); - size_t t = 0; - for (int j = 0; j < J::dim; j++) - for (int i = 0; i < I::dim; i++) - M.data()[t++] = T(i, j); - return Matrix(M); - } - - /** Reshape rank 2 tensor into Vector */ - template - Vector toVector(const tensors::Tensor2Expression& T) { - Vector v(I::dim * J::dim); - size_t t = 0; - for (int j = 0; j < J::dim; j++) - for (int i = 0; i < I::dim; i++) - v(t++) = T(i, j); - return v; - } - - /** Reshape Vector into rank 2 tensor */ - template - tensors::Tensor2 reshape2(const Vector& v) { - if (v.size() != N1 * N2) throw std::invalid_argument( - "reshape2: incompatible dimensions"); - double data[N2][N1]; - int t = 0; - for (int j = 0; j < N2; j++) - for (int i = 0; i < N1; i++) - data[j][i] = v(t++); - return tensors::Tensor2(data); - } - - /** Reshape rank 3 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor3Expression& T, int m, int n) { - if (m * n != I::dim * J::dim * K::dim) throw std::invalid_argument( - "reshape: incompatible dimensions"); - Matrix M(m, n); - int t = 0; - for (int k = 0; k < K::dim; k++) - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - M.data()[t++] = T(i, j, k); - return M; - } - - /** Reshape Vector into rank 3 tensor */ - template - tensors::Tensor3 reshape3(const Vector& v) { - if (v.size() != N1 * N2 * N3) throw std::invalid_argument( - "reshape3: incompatible dimensions"); - double data[N3][N2][N1]; - int t = 0; - for (int k = 0; k < N3; k++) - for (int j = 0; j < N2; j++) - for (int i = 0; i < N1; i++) - data[k][j][i] = v(t++); - return tensors::Tensor3(data); - } - - /** Reshape rank 5 tensor into Matrix */ - template - Matrix reshape(const tensors::Tensor5Expression& T, int m, - int n) { - if (m * n != I::dim * J::dim * K::dim * L::dim * M::dim) throw std::invalid_argument( - "reshape: incompatible dimensions"); - Matrix R(m, n); - int t = 0; - for (int m = 0; m < M::dim; m++) - for (int l = 0; l < L::dim; l++) - for (int k = 0; k < K::dim; k++) - for (int i = 0; i < I::dim; i++) - for (int j = 0; j < J::dim; j++) - R.data()[t++] = T(i, j, k, l, m); - return R; - } - -} // namespace gtsam diff --git a/gtsam/geometry/tensors.h b/gtsam/geometry/tensors.h deleted file mode 100644 index 1441a7823..000000000 --- a/gtsam/geometry/tensors.h +++ /dev/null @@ -1,45 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 tensors.h - * @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 10, 2010 - * @author Frank Dellaert - * @defgroup tensors - */ - -#pragma once - -namespace tensors { - - /** index */ - template struct Index { - static const int dim = Dim; ///< dimension - }; - -} // namespace tensors - -// Expression templates -#include -#include -#include -// Tensor4 not needed so far -#include - -// Actual tensor classes -#include -#include -#include -#include -#include - - diff --git a/gtsam/geometry/tests/testFundamental.cpp b/gtsam/geometry/tests/testFundamental.cpp deleted file mode 100644 index 297b36036..000000000 --- a/gtsam/geometry/tests/testFundamental.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testFundamental.cpp - * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 13, 2010 - * @author: Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -Index<3, 'a'> a; -Index<3, 'b'> b; - -Index<4, 'A'> A; -Index<4, 'B'> B; - -/* ************************************************************************* */ -TEST( Tensors, FundamentalMatrix) -{ - double f[3][3] = { { 1, 0, 0 }, { 1, 2, 3 }, { 1, 2, 3 } }; - FundamentalMatrix F(f); - - Point2h p = point2h(1, 2, 3); // point p in view one - Point2h q = point2h(14, -1, 0); // point q in view two - - // points p and q are in correspondence - CHECK(F(a,b)*p(a)*q(b) == 0) - - // in detail, l1(b)*q(b)==0 - Line2h l1 = line2h(1, 14, 14); - CHECK(F(a,b)*p(a) == l1(b)) - CHECK(l1(b)*q(b) == 0); // q is on line l1 - - // and l2(a)*p(a)==0 - Line2h l2 = line2h(13, -2, -3); - CHECK(F(a,b)*q(b) == l2(a)) - CHECK(l2(a)*p(a) == 0); // p is on line l2 -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp deleted file mode 100644 index 417d41977..000000000 --- a/gtsam/geometry/tests/testHomography2.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testHomography2.cpp - * @brief Test and estimate 2D homographies - * @date Feb 13, 2010 - * @author Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -Index<3, 'a'> a, _a; -Index<3, 'b'> b, _b; -Index<3, 'c'> c, _c; - -/* ************************************************************************* */ -TEST( Homography2, RealImages) -{ - // 4 point correspondences MATLAB from the floor of bt001.png and bt002.png - Correspondence p1(point2h(216.841, 443.220, 1), point2h(213.528, 414.671, 1)); - Correspondence p2(point2h(252.119, 363.481, 1), point2h(244.614, 348.842, 1)); - Correspondence p3(point2h(316.614, 414.768, 1), point2h(303.128, 390.000, 1)); - Correspondence p4(point2h(324.165, 465.463, 1), point2h(308.614, 431.129, 1)); - - // Homography obtained using MATLAB code - double h[3][3] = { { 0.9075, 0.0031, -0 }, { -0.1165, 0.8288, -0.0004 }, { - 30.8472, 16.0449, 1 } }; - Homography2 H(h); - - // CHECK whether they are equivalent - CHECK(assert_equivalent(p1.second(b),H(b,a)*p1.first(a),0.05)) - CHECK(assert_equivalent(p2.second(b),H(b,a)*p2.first(a),0.05)) - CHECK(assert_equivalent(p3.second(b),H(b,a)*p3.first(a),0.05)) - CHECK(assert_equivalent(p4.second(b),H(b,a)*p4.first(a),0.05)) -} - -/* ************************************************************************* */ -// Homography test case -// 4 trivial correspondences of a translating square -Correspondence p1(point2h(0, 0, 1), point2h(4, 5, 1)); -Correspondence p2(point2h(1, 0, 1), point2h(5, 5, 1)); -Correspondence p3(point2h(1, 1, 1), point2h(5, 6, 1)); -Correspondence p4(point2h(0, 1, 1), point2h(4, 6, 1)); - -double h[3][3] = { { 1, 0, 4 }, { 0, 1, 5 }, { 0, 0, 1 } }; -Homography2 H(h); - -/* ************************************************************************* */ -TEST( Homography2, TestCase) -{ - // Check homography - list correspondences; - correspondences += p1, p2, p3, p4; - BOOST_FOREACH(const Correspondence& p, correspondences) - CHECK(assert_equality(p.second(b),H(_a,b) * p.first(a))) - - // Check a line - Line2h l1 = line2h(1, 0, -1); // in a - Line2h l2 = line2h(1, 0, -5); // x==5 in b - CHECK(assert_equality(l1(a), H(a,b)*l2(b))) -} - -/* ************************************************************************* */ -/** - * Computes the homography H(I,_T) from template to image - * given the pose tEc of the camera in the template coordinate frame. - * Assumption is Z is normal to the template, template texture in X-Y plane. - */ -Homography2 patchH(const Pose3& tEc) { - Pose3 cEt = tEc.inverse(); - Rot3 cRt = cEt.rotation(); - Point3 r1 = cRt.r1(), r2 = cRt.r2(), t = cEt.translation(); - - // TODO cleanup !!!! - // column 1 - double H11 = r1.x(); - double H21 = r1.y(); - double H31 = r1.z(); - // column 2 - double H12 = r2.x(); - double H22 = r2.y(); - double H32 = r2.z(); - // column 3 - double H13 = t.x(); - double H23 = t.y(); - double H33 = t.z(); - double data2[3][3] = { { H11, H21, H31 }, { H12, H22, H32 }, - { H13, H23, H33 } }; - return Homography2(data2); -} - -/* ************************************************************************* */ -namespace gtsam { -// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;} - Vector toVector(const tensors::Tensor2<3, 3>& H) { - Index<3, 'T'> _T; // covariant 2D template - Index<3, 'C'> I; // contravariant 2D camera - return toVector(H(I,_T)); - } - Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { - return toVector(A)-toVector(B); // TODO correct order ? - } -} - -#include - -/* ************************************************************************* */ -TEST( Homography2, patchH) -{ - Index<3, 'T'> _T; // covariant 2D template - Index<3, 'C'> I; // contravariant 2D camera - - // data[_T][I] - double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; - Homography2 expected(data1); - - // camera rotation, looking in negative Z - Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1)); - Point3 gTc(0,0,10); // Camera location, out on the Z axis - Pose3 gEc(gRc,gTc); // Camera pose - - Homography2 actual = patchH(gEc); - -// GTSAM_PRINT(expected(I,_T)); -// GTSAM_PRINT(actual(I,_T)); - CHECK(assert_equality(expected(I,_T),actual(I,_T))); - - // FIXME: this doesn't appear to be tested, and requires that Tensor2 be a Lie object -// Matrix D = numericalDerivative11(patchH, gEc); -// print(D,"D"); -} - -/* ************************************************************************* */ -TEST( Homography2, patchH2) -{ - Index<3, 'T'> _T; // covariant 2D template - Index<3, 'C'> I; // contravariant 2D camera - - // data[_T][I] - double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; - Homography2 expected(data1); - - // camera rotation, looking in negative Z - Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1)); - Point3 gTc(0,0,10); // Camera location, out on the Z axis - Pose3 gEc(gRc,gTc); // Camera pose - - Homography2 actual = patchH(gEc); - -// GTSAM_PRINT(expected(I,_T)); -// GTSAM_PRINT(actual(I,_T)); - CHECK(assert_equality(expected(I,_T),actual(I,_T))); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testTensors.cpp b/gtsam/geometry/tests/testTensors.cpp deleted file mode 100644 index 755b274a6..000000000 --- a/gtsam/geometry/tests/testTensors.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testTensors.cpp - * @brief try tensor expressions based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf - * @date Feb 9, 2010 - * @author Frank Dellaert - */ - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace tensors; - -/* ************************************************************************* */ -// Indices - -Index<3, 'a'> a, _a; -Index<3, 'b'> b, _b; -Index<3, 'c'> c, _c; - -Index<4, 'A'> A; -Index<4, 'B'> B; - -/* ************************************************************************* */ -// Tensor1 -/* ************************************************************************* */ -TEST(Tensor1, Basics) -{ - // you can create 1-tensors corresponding to 2D homogeneous points - // using the function point2h in projectiveGeometry.* - Point2h p = point2h(1, 2, 3), q = point2h(2, 4, 6); - - // equality tests always take tensor expressions, not tensors themselves - // the difference is that a tensor expression has indices - CHECK(p(a)==p(a)) - CHECK(assert_equality(p(a),p(a))) - CHECK(assert_equality(p(a)*2,q(a))) - CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent - - // and you can take a norm, typically for normalization to the sphere - DOUBLES_EQUAL(sqrt(14),norm(p(a)),1e-9) -} - -/* ************************************************************************* */ -TEST( Tensor1, Incidence2D) -{ - // 2D lines are created with line2h - Line2h l = line2h(-13, 5, 1); - Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1); - - // Incidence between a line and a point is checked with simple contraction - // It does not matter which index you use, but it has to be of dimension 3 - DOUBLES_EQUAL(l(a)*p(a),0,1e-9) - DOUBLES_EQUAL(l(b)*q(b),0,1e-9) - DOUBLES_EQUAL(p(a)*l(a),0,1e-9) - DOUBLES_EQUAL(q(a)*l(a),0,1e-9) -} - -/* ************************************************************************* */ -TEST( Tensor1, Incidence3D) -{ - // similar constructs exist for 3D points and planes - Plane3h pi = plane3h(0, 1, 0, -2); - Point3h P = point3h(0, 2, 0, 1), Q = point3h(1, 2, 0, 1); - - // Incidence is checked similarly - DOUBLES_EQUAL(pi(A)*P(A),0,1e-9) - DOUBLES_EQUAL(pi(A)*Q(A),0,1e-9) - DOUBLES_EQUAL(P(A)*pi(A),0,1e-9) - DOUBLES_EQUAL(Q(A)*pi(A),0,1e-9) -} - -/* ************************************************************************* */ -// Tensor2 -/* ************************************************************************* */ -TEST( Tensor2, Outer33) -{ - Line2h l1 = line2h(1, 2, 3), l2 = line2h(1, 3, 5); - - // We can also create tensors directly from data - double data[3][3] = { { 1, 2, 3 }, { 3, 6, 9 }, {5, 10, 15} }; - Tensor2<3, 3> expected(data); - // in this case expected(0) == {1,2,3} - Line2h l0 = expected(a,b)(0); - CHECK(l0(a) == l1(a)) - - // And we create rank 2 tensors from the outer product of two rank 1 tensors - CHECK(expected(a,b) == l1(a) * l2(b)) - - // swap just swaps how you access a tensor, but note the data is the same - CHECK(assert_equality(expected(a,b).swap(), l2(b) * l1(a))); -} - -/* ************************************************************************* */ -TEST( Tensor2, AnotherOuter33) -{ - // first cube point from testFundamental, projected in left and right -// Point2h p = point2h(0, -1, 2), q = point2h(-2, -1, 2); -// print(p(a)*q(b)); -// print(p(b)*q(a)); -// print(q(a)*p(b)); -// print(q(b)*p(a)); -} - -/* ************************************************************************* */ -TEST( Tensor2, Outer34) -{ - Line2h l = line2h(1, 2, 3); - Plane3h pi = plane3h(1, 3, 5, 7); - double - data[4][3] = { { 1, 2, 3 }, { 3, 6, 9 }, { 5, 10, 15 }, { 7, 14, 21 } }; - Tensor2<3, 4> expected(data); - CHECK(assert_equality(expected(a,B),l(a) * pi(B))) - CHECK(assert_equality(expected(a,B).swap(),pi(B) * l(a))) -} - -/* ************************************************************************* */ -TEST( Tensor2, SpecialContract) -{ - double data[3][3] = { { 1, 2, 3 }, { 2, 4, 6 }, { 3, 6, 9 } }; - Tensor2<3, 3> S(data), T(data); - //print(S(a, b) * T(a, c)); // contract a -> b,c - // S(a,0)*T(a,0) = [1 2 3] . [1 2 3] = 14 - // S(a,0)*T(a,2) = [1 2 3] . [3 6 9] = 3+12+27 = 42 - double data2[3][3] = { { 14, 28, 42 }, { 28, 56, 84 }, { 42, 84, 126 } }; - Tensor2<3, 3> expected(data2); - CHECK(assert_equality(expected(b,c), S(a, b) * T(a, c))); -} - -/* ************************************************************************* */ -TEST( Tensor2, ProjectiveCamera) -{ - Point2h p = point2h(1 + 2, 2, 5); - Point3h P = point3h(1, 2, 5, 1); - double data[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { 2, 0, 0 } }; - ProjectiveCamera M(data); - CHECK(assert_equality(p(a),M(a,A)*P(A))) -} - -/* ************************************************************************* */ -namespace camera { - // to specify the tensor M(a,A), we need to give four 2D points - double data[4][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 }, { 10, 11, 12 } }; - ProjectiveCamera M(data); - Matrix matrix = Matrix_(4,3,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.); - Vector vector = Vector_( 12,1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,11.,12.); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape ) -{ - // it is annoying that a camera can only be reshaped to a 4*3 -// print(camera::M(a,A)); - Matrix actual = reshape(camera::M(a,A),4,3); - EQUALITY(camera::matrix,actual); -} - -/* ************************************************************************* */ -TEST( Tensor2, toVector ) -{ - // Vectors are created with the leftmost indices iterating the fastest - Vector actual = toVector(camera::M(a,A)); - CHECK(assert_equal(camera::vector,actual)); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape2 ) -{ - Tensor2<3,4> actual = reshape2<3,4>(camera::vector); - CHECK(assert_equality(camera::M(a,A),actual(a,A))); -} - -/* ************************************************************************* */ -TEST( Tensor2, reshape_33_to_9 ) -{ - double data[3][3] = { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } }; - FundamentalMatrix F(data); - Matrix matrix = Matrix_(1,9,1.,2.,3.,4.,5.,6.,7.,8.,9.); - Matrix actual = reshape(F(a,b),1,9); - EQUALITY(matrix,actual); - Vector v = Vector_( 9,1.,2.,3.,4.,5.,6.,7.,8.,9.); - CHECK(assert_equality(F(a,b),reshape2<3, 3> (v)(a,b))); -} - -/* ************************************************************************* */ -// Tensor3 -/* ************************************************************************* */ -TEST( Tensor3, Join) -{ - Line2h l = line2h(-13, 5, 1); - Point2h p = point2h(1, 2, 3), q = point2h(2, 5, 1); - - // join points into line - Eta3 e; - CHECK(assert_equality(e(a, b, c) * p(a) * q(b), l(c))) -} - -/* ************************************************************************* */ -TEST( Tensor5, Outer32) -{ - double t[3][3][3] = { { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0, - 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }, { { 0, 0, 3 }, { 0, 8, -125 }, - { -3, 125, 1 } } }; - TrifocalTensor T(t); - - double data[3][3] = { { 0, 0, 3 }, { 0, 8, -125 }, { -3, 125, 1 } }; - FundamentalMatrix F(data); - - //Index<3, 'd'> d, _d; - //Index<3, 'e'> e, _e; - //print(T(_a,b,c)*F(_d,_e)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 337b571c6..ea92cb791 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -23,7 +23,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/slam/simulated3D.cpp b/gtsam/slam/simulated3D.cpp deleted file mode 100644 index e2f6211a5..000000000 --- a/gtsam/slam/simulated3D.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated3D.cpp -* @brief measurement functions and derivatives for simulated 3D robot -* @author Alex Cunningham -**/ - -#include - -namespace gtsam { - -namespace simulated3D { - -Point3 prior (const Point3& x, boost::optional H) { - if (H) *H = eye(3); - return x; -} - -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return x2 - x1; -} - -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return l - x; -} - -}} // namespace gtsam::simulated3D diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h deleted file mode 100644 index 7b4dfce37..000000000 --- a/gtsam/slam/simulated3D.h +++ /dev/null @@ -1,126 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated3D.h - * @brief measurement functions and derivatives for simulated 3D robot - * @author Alex Cunningham - **/ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include - -// \namespace - -namespace gtsam { -namespace simulated3D { - -/** - * This is a linear SLAM domain where both poses and landmarks are - * 3D points, without rotation. The structure and use is based on - * the simulated2D domain. - */ - - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a pose key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - -/** - * Prior on a single pose - */ -Point3 prior(const Point3& x, boost::optional H = boost::none); - -/** - * odometry between two poses - */ -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * measurement between landmark and pose - */ -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * A prior factor on a single linear robot pose - */ -struct PointPrior3D: public NoiseModelFactor1 { - - Point3 measured_; ///< The prior pose value for the variable attached to this factor - - /** - * Constructor for a prior factor - * @param measured is the measured/prior position for the pose - * @param model is the measurement model for the factor (Dimension: 3) - * @param key is the key for the pose - */ - PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) : - NoiseModelFactor1 (model, key), measured_(measured) { - } - - /** - * Evaluates the error at a given value of x, - * with optional derivatives. - * @param x is the current value of the variable - * @param H is an optional Jacobian matrix (Dimension: 3x3) - * @return Vector error between prior value and x (Dimension: 3) - */ - Vector evaluateError(const Point3& x, boost::optional H = - boost::none) const { - return (prior(x, H) - measured_).vector(); - } -}; - -/** - * Models a linear 3D measurement between 3D points - */ -struct Simulated3DMeasurement: public NoiseModelFactor2 { - - Point3 measured_; ///< Linear displacement between a pose and landmark - - /** - * Creates a measurement factor with a given measurement - * @param measured is the measurement, a linear displacement between poses and landmarks - * @param model is a measurement model for the factor (Dimension: 3) - * @param poseKey is the pose key of the robot - * @param pointKey is the point key for the landmark - */ - Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey) : - NoiseModelFactor2(model, poseKey, pointKey), measured_(measured) {} - - /** - * Error function with optional derivatives - * @param x1 a robot pose value - * @param x2 a landmark point value - * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3) - * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) - * @return vector error between measurement and prediction (Dimension: 3) - */ - Vector evaluateError(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); - } -}; - -}} // namespace simulated3D diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp deleted file mode 100644 index a48e797e5..000000000 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSimulated3D.cpp - * @brief Unit tests for simulated 3D measurement functions - * @author Alex Cunningham - **/ - -#include -#include - -#include -#include -#include -#include - -using namespace gtsam; -using namespace simulated3D; - -/* ************************************************************************* */ -TEST( simulated3D, Values ) -{ - Values actual; - actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); - actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); - EXPECT(assert_equal(actual,actual,1e-9)); -} - -/* ************************************************************************* */ -TEST( simulated3D, Dprior ) -{ - Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(boost::bind(simulated3D::prior, _1, boost::none),x); - Matrix computed; - simulated3D::prior(x,computed); - EXPECT(assert_equal(numerical,computed,1e-9)); -} - -/* ************************************************************************* */ -TEST( simulated3D, DOdo ) -{ - Point3 x1(1,-9,7),x2(-5,6,7); - Matrix H1,H2; - simulated3D::odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A2,H2,1e-9)); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ From 4c58a00a6d61ea1badc219285c4641dc958f421b Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sat, 7 Apr 2012 04:40:24 +0000 Subject: [PATCH 15/72] revise parameter struct and clean file --- .../linear/IterativeOptimizationParameters.h | 218 +++++++++++++++--- gtsam/linear/IterativeSolver.h | 14 +- 2 files changed, 188 insertions(+), 44 deletions(-) diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index fa5c3a6ee..7bba870bb 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -7,57 +7,209 @@ #pragma once #include -#include +#include #include namespace gtsam { -// a container for all related parameters +/* parameters for combinatorial preconditioner */ +struct CombinatorialParameters { + + enum Group { /* group of variables */ + BLOCK = 0, /* utilize the inherent block structure */ + SCALAR /* break the block variables into scalars */ + } group_; + + enum Type { /* subgraph specification */ + JACOBI = 0, /* block diagonal */ + SPTREE, /* spanning tree */ + GSP, /* gspn, n = 0: all factors, n = 1: all unary factors, n = 2: spanning tree, etc .. */ + KOUTIS, /* implement Koutis2011Focs */ + } type_; + + enum Weight { /* how to weigh the graph edges */ + EQUAL = 0, /* 0: every block edge has equal weight */ + RHS_2NORM, /* 1: use the 2-norm of the rhs */ + LHS_FNORM, /* 2: use the frobenius norm of the lhs */ + RAW, /* 3: use raw scalar weight for the scalar case */ + } weight_ ; + + int complexity_;/* a parameter for the subgraph complexity */ + int hessian_; /* 0: do nothing, 1: use whole block hessian */ + + CombinatorialParameters(): group_(BLOCK), type_(JACOBI), weight_(EQUAL), complexity_(0), hessian_(1) {} + CombinatorialParameters(Group group, Type type, Weight weight, int complexity, int hessian) + : group_(group), type_(type), weight_(weight), complexity_(complexity), hessian_(hessian) {} + + inline Group group() const { return group_; } + inline Type type() const { return type_; } + inline Weight weight() const { return weight_; } + inline int complexity() const { return complexity_; } + inline int hessian() const { return hessian_; } + + inline bool isScalar() const { return group_ == SCALAR; } + inline bool isBlock() const { return group_ == BLOCK; } + inline bool isStatic() const { return (type_ == JACOBI || weight_ == EQUAL) ? true : false;} + inline bool isDynamic() const { return !isStatic(); } + + inline void print() const { + + const std::string groupStr[2] = {"block", "scalar"}; + const std::string typeStr[4] = {"jacobi", "sptree", "gsp", "koutis"}; + const std::string weightStr[4] = {"equal", "rhs-2norm", "lhs-form", "raw"}; + + std::cout << "CombinatorialParameters: " + << "group = " << groupStr[group_] + << ", type = " << typeStr[type_] + << ", weight = " << weightStr[weight_] + << ", complexity = " << complexity_ + << ", hessian = " << hessian_ << std::endl; + } +}; + +/* parameters for the preconditioner */ +struct PreconditionerParameters { + + typedef boost::shared_ptr shared_ptr; + + enum Kernel { /* Preconditioner Kernel */ + GTSAM = 0, /* Jacobian Factor Graph of GTSAM */ + CHOLMOD /* Cholmod Sparse */ + } kernel_ ; + + enum Type { /* preconditioner type */ + Combinatorial = 0 /* combinatorial preconditioner */ + } type_; + + CombinatorialParameters combinatorial_; + + PreconditionerParameters(): kernel_(CHOLMOD), type_(Combinatorial) {} + PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial) + : kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial) {} + + /* general interface */ + inline Kernel kernel() const { return kernel_; } + inline Type type() const { return type_; } + inline const CombinatorialParameters & combinatorial() const { return combinatorial_; } + + void print() const { + const std::string kernelStr[2] = {"gtsam", "cholmod"}; + const std::string typeStr[1] = {"combinatorial"}; + + std::cout << "PreconditionerParameters: " + << "kernel = " << kernelStr[kernel_] + << ", type = " << typeStr[type_] << std::endl; + combinatorial_.print(); + } +}; + +/* parameters for the conjugate gradient method */ +struct ConjugateGradientParameters { + + size_t minIterations_; /* minimum number of cg iterations */ + size_t maxIterations_; /* maximum number of cg iterations */ + size_t reset_; /* number of iterations before reset */ + double epsilon_rel_; /* threshold for relative error decrease */ + double epsilon_abs_; /* threshold for absolute error decrease */ + + enum BLASKernel { /* Matrix Operation Kernel */ + GTSAM = 0, /* Jacobian Factor Graph of GTSAM */ + SBM, /* Sparse Block Matrix */ + SM, /* Sparse Scalar Matrix */ + CHOLMOD /* Cholmod Sparse */ + } blas_kernel_; + + /* Specific for PCGPlus */ + size_t degree_; /* the maximum degree of the vertices to be eliminated before doing cg */ + + ConjugateGradientParameters() + : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3), + blas_kernel_(GTSAM), degree_(0) {} + ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, + double epsilon_rel, double epsilon_abs, BLASKernel blas = GTSAM, size_t degree = 0) + : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), + epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas), degree_(degree) {} + + /* general interface */ + inline size_t minIterations() const { return minIterations_; } + inline size_t maxIterations() const { return maxIterations_; } + inline size_t reset() const { return reset_; } + inline double epsilon() const { return epsilon_rel_; } + inline double epsilon_rel() const { return epsilon_rel_; } + inline double epsilon_abs() const { return epsilon_abs_; } + inline BLASKernel blas_kernel() const { return blas_kernel_; } + inline size_t degree() const { return degree_; } + + void print() const { + const std::string blasStr[4] = {"gtsam", "sbm", "sm", "cholmod"}; + std::cout << "ConjugateGradientParameters: " + << "blas = " << blasStr[blas_kernel_] + << ", minIter = " << minIterations_ + << ", maxIter = " << maxIterations_ + << ", resetIter = " << reset_ + << ", eps_rel = " << epsilon_rel_ + << ", eps_abs = " << epsilon_abs_ + << ", degree = " << degree_ + << std::endl; + } +}; + +/* parameters for iterative linear solvers */ struct IterativeOptimizationParameters { public: typedef boost::shared_ptr shared_ptr; - typedef enum { - SILENT = 0, - ERROR, - } verbosityLevel; + PreconditionerParameters preconditioner_; + ConjugateGradientParameters cg_; + enum Kernel { PCG = 0 /*, PCGPlus*/, LSPCG } kernel_ ; /* Iterative Method Kernel */ + enum Verbosity { SILENT = 0, ERROR } verbosity_ ; /* Verbosity */ public: - size_t minIterations_; - size_t maxIterations_; - size_t reset_; // number of iterations before reset, for cg and gmres - double epsilon_rel_; // relative error - double epsilon_abs_; // absolute error - verbosityLevel verbosity_; - bool est_cond_ ; - std::map sandbox_; -public: IterativeOptimizationParameters() - : minIterations_(1), maxIterations_(500), reset_(501), - epsilon_rel_(1e-3), epsilon_abs_(1e-3), verbosity_(SILENT), est_cond_(false) {} + : preconditioner_(), cg_(), kernel_(PCG), verbosity_(SILENT) {} + + IterativeOptimizationParameters(const IterativeOptimizationParameters &p) : + preconditioner_(p.preconditioner_), cg_(p.cg_), kernel_(p.kernel_), verbosity_(p.verbosity_) {} IterativeOptimizationParameters( - const IterativeOptimizationParameters &p) : - minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), - epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), verbosity_(p.verbosity_), - est_cond_(p.est_cond_){ } + const PreconditionerParameters &p, const ConjugateGradientParameters &c, Kernel kernel = PCG, Verbosity verbosity = SILENT) + : preconditioner_(p), cg_(c), kernel_(kernel), verbosity_(verbosity) {} - IterativeOptimizationParameters(size_t minIterations, size_t maxIterations, size_t reset, - double epsilon, double epsilon_abs, verbosityLevel verbosity = ERROR, bool est_cond = false) : - minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), - epsilon_rel_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity), est_cond_(est_cond) {} + /* general interface */ + inline Kernel kernel() const { return kernel_; } + inline Verbosity verbosity() const { return verbosity_; } + + /* utility */ + inline const PreconditionerParameters& preconditioner() const { return preconditioner_; } + inline const ConjugateGradientParameters& cg() const { return cg_; } + + /* interface to preconditioner parameters */ + inline PreconditionerParameters::Kernel preconditioner_kernel() const { return preconditioner_.kernel(); } + inline PreconditionerParameters::Type type() const { return preconditioner_.type(); } + + /* interface to cg parameters */ + inline size_t minIterations() const { return cg_.minIterations(); } + inline size_t maxIterations() const { return cg_.maxIterations(); } + inline size_t reset() const { return cg_.reset(); } + inline double epsilon() const { return cg_.epsilon_rel(); } + inline double epsilon_rel() const { return cg_.epsilon_rel(); } + inline double epsilon_abs() const { return cg_.epsilon_abs(); } + inline size_t degree() const { return cg_.degree(); } + inline ConjugateGradientParameters::BLASKernel blas_kernel() const { return cg_.blas_kernel(); } + + void print(const std::string &s="") const { + const std::string kernelStr[2] = {"pcg", "lspcg"}; + std::cout << s << std::endl + << "IterativeOptimizationParameters: " + << "kernel = " << kernelStr[kernel_] << std::endl; + cg_.print(); + preconditioner_.print(); + + } - size_t minIterations() const { return minIterations_; } - size_t maxIterations() const { return maxIterations_; } - size_t reset() const { return reset_; } - double epsilon() const { return epsilon_rel_; } - double epsilon_rel() const { return epsilon_rel_; } - double epsilon_abs() const { return epsilon_abs_; } - verbosityLevel verbosity() const { return verbosity_; } - bool est_cond() const { return est_cond_ ; } }; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 8de70bfdf..7b1c1e630 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -19,8 +19,6 @@ #pragma once #include -#include -#include #include #include @@ -40,15 +38,9 @@ protected: public: IterativeSolver(): parameters_(new Parameters()) {} - - IterativeSolver(const IterativeSolver &solver) - : parameters_(solver.parameters_) {} - - IterativeSolver(const Parameters::shared_ptr& parameters) - : parameters_(parameters) {} - - IterativeSolver(const Parameters ¶meters) - : parameters_(new Parameters(parameters)) {} + IterativeSolver(const IterativeSolver &solver) : parameters_(solver.parameters_) {} + IterativeSolver(const Parameters::shared_ptr& parameters) : parameters_(parameters) {} + IterativeSolver(const Parameters ¶meters) : parameters_(new Parameters(parameters)) {} virtual ~IterativeSolver() {} From 2ac1473a84f1785cefe1a12008cd370548e6cd5c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 03:02:11 +0000 Subject: [PATCH 16/72] Fixed iSAM2 deep copy of empty tree --- gtsam/inference/BayesTree-inl.h | 7 +++++++ gtsam/inference/BayesTree.h | 4 +--- tests/testGaussianISAM2.cpp | 5 +++++ 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 6881e9a69..95b070739 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -582,6 +582,13 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + void BayesTree::cloneTo(This& newTree) const { + if(root()) + cloneTo(newTree, root(), sharedClique()); + } + /* ************************************************************************* */ template void BayesTree::cloneTo( diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 3df697a48..aba7181ff 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -283,9 +283,7 @@ namespace gtsam { private: /** deep copy to another tree */ - void cloneTo(This& newTree) const { - cloneTo(newTree, root(), sharedClique()); - } + void cloneTo(This& newTree) const; /** deep copy to another tree */ void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 5ff24070c..9b1bd546f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -898,6 +898,11 @@ TEST(ISAM2, clone) { CHECK(assert_equal(createSlamlikeISAM2(), clone1)); CHECK(assert_equal(clone1, temp)); + + // Check clone empty + ISAM2 isam; + clone1 = isam; + CHECK(assert_equal(ISAM2(), clone1)); } /* ************************************************************************* */ From ed91c5c9ff5c5616992bb4fa208c7f738d68912e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 03:02:23 +0000 Subject: [PATCH 17/72] Tweak to dogleg evaluating error decrease --- gtsam/nonlinear/DoglegOptimizerImpl.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index fbfa06028..cf055293e 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -5,6 +5,8 @@ */ #pragma once +#include + #include #include // To get optimize(BayesTree) //#include @@ -180,17 +182,17 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const double new_M_error = jfg.error(result.dx_d); toc(6, "decrease in M"); - if(verbose) cout << "f error: " << f_error << " -> " << result.f_error << endl; - if(verbose) cout << "M error: " << M_error << " -> " << new_M_error << endl; + if(verbose) cout << setprecision(15) << "f error: " << f_error << " -> " << result.f_error << endl; + if(verbose) cout << setprecision(15) << "M error: " << M_error << " -> " << new_M_error << endl; tic(7, "adjust Delta"); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error - const double rho = fabs(M_error - new_M_error) < 1e-15 ? + const double rho = fabs(f_error - result.f_error) < 1e-15 || fabs(M_error - new_M_error) < 1e-15 ? 0.5 : (f_error - result.f_error) / (M_error - new_M_error); - if(verbose) cout << "rho = " << rho << endl; + if(verbose) cout << setprecision(15) << "rho = " << rho << endl; if(rho >= 0.75) { // M agrees very well with f, so try to increase lambda From 2f0b3cbe02dee40e54a6b0af3cd23324912befff Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 03:02:30 +0000 Subject: [PATCH 18/72] Format with g instead of f with printing doubles in CppUnitLite --- CppUnitLite/SimpleString.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CppUnitLite/SimpleString.cpp b/CppUnitLite/SimpleString.cpp index 90356183a..19dc7f258 100644 --- a/CppUnitLite/SimpleString.cpp +++ b/CppUnitLite/SimpleString.cpp @@ -102,7 +102,7 @@ SimpleString StringFrom (long value) SimpleString StringFrom (double value) { char buffer [DEFAULT_SIZE]; - sprintf (buffer, "%lf", value); + sprintf (buffer, "%lg", value); return SimpleString(buffer); } From 21e52f3ab10143bc533b5ddf3471e96eae1c2034 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 03:02:37 +0000 Subject: [PATCH 19/72] Added unit tests for badly-scaled matrices with Cholesky, LDL, and SVD --- gtsam/base/tests/testCholesky.cpp | 49 +++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 68b2bf660..5232fc6ba 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -160,6 +160,55 @@ TEST(cholesky, ldlPartial2) { EXPECT(assert_equal(IexpectedR, p.transpose()*I)); } +/* ************************************************************************* */ +TEST(cholesky, BadScalingCholesky) { + Matrix A = Matrix_(2,2, + 1e-40, 0.0, + 0.0, 1.0); + + Matrix R(A.transpose() * A); + choleskyPartial(R, 2); + + double expectedSqrtCondition = 1e-40; + double actualSqrtCondition = R(0,0) / R(1,1); + + DOUBLES_EQUAL(expectedSqrtCondition, actualSqrtCondition, 1e-41); +} + +/* ************************************************************************* */ +TEST(cholesky, BadScalingLDL) { + Matrix A = Matrix_(2,2, + 1.0, 0.0, + 0.0, 1e-40); + + Matrix R(A.transpose() * A); + Eigen::LDLT::TranspositionType permutation = ldlPartial(R, 2); + + EXPECT(permutation.indices()(0) == 0); + EXPECT(permutation.indices()(1) == 1); + + double expectedCondition = 1e40; + double actualCondition = R(0,0) / R(1,1); + + DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); +} + +/* ************************************************************************* */ +TEST(cholesky, BadScalingSVD) { + Matrix A = Matrix_(2,2, + 1.0, 0.0, + 0.0, 1e-40); + + Matrix U, V; + Vector S; + gtsam::svd(A, U, S, V); + + double expectedCondition = 1e40; + double actualCondition = S(0) / S(1); + + DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); +} + /* ************************************************************************* */ int main() { TestResult tr; From 08b87be4b51e5a485c283a5ff8ee9fedb1cb25f8 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 13:21:14 +0000 Subject: [PATCH 20/72] Fixed parenthesis typos --- gtsam/base/FastSet.h | 2 +- gtsam/linear/NoiseModel.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 05fb879a1..6abd7efb8 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -105,7 +105,7 @@ struct FastSetTestableHelper { typename Set::const_iterator it2 = set2.begin(); while (it1 != set1.end()) { if (it2 == set2.end() || - fabs((double)(*it1) - (double)(*it2) > tol)) + fabs((double)(*it1) - (double)(*it2)) > tol) return false; ++it1; ++it2; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index fd93d39fc..b6472061f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -59,7 +59,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) size_t i,j; for (i = 0; i < m; i++) for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j) > 1e-9)) goto full; + if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; Vector variances(n); for (j = 0; j < n; j++) variances(j) = covariance(j,j); return Diagonal::Variances(variances,true); From 7243a9870be26188c19a3d7b507f6ccb308ea47c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 9 Apr 2012 13:21:16 +0000 Subject: [PATCH 21/72] Using vector::assign instead of erase in JacobianFactor::eliminate, caused invalid iterator on some STL implementations --- gtsam/linear/JacobianFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1f9dca77b..c48a62431 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -474,7 +474,7 @@ namespace gtsam { tic(4, "remaining factor"); // Take lower-right block of Ab to get the new factor Ab_.rowEnd() = noiseModel->dim(); - keys_.assign(begin() + nrFrontals, end()); + keys_.erase(begin(), begin() + nrFrontals); // Set sigmas with the right model if (noiseModel->isConstrained()) model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); From da63e6757ced837bc46d224f698e57d1dd8d2260 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 10 Apr 2012 16:47:02 +0000 Subject: [PATCH 22/72] Fixed cpack install to work with tgz, deb and rpm --- CMakeLists.txt | 51 +++++++++++++++++++++++------------ gtsam/3rdparty/CMakeLists.txt | 8 +++--- gtsam/CMakeLists.txt | 20 +++++++------- wrap/CMakeLists.txt | 4 +-- 4 files changed, 50 insertions(+), 33 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 610d026d0..0b0bd9afa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ set (GTSAM_VERSION_MINOR 9) set (GTSAM_VERSION_PATCH 0) # Set the default install path to home -set (CMAKE_INSTALL_PREFIX ${HOME} CACHE DOCSTRING "Install prefix for library") +#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") # Use macros for creating tests/timing scripts set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) @@ -62,6 +62,8 @@ option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) +option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) +option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) @@ -69,6 +71,15 @@ option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) +# Flags for choosing default packaging tools +set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") +set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") + +# Sanity check building of libraries +if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY) + message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") +endif() + # Add the Quaternion Build Flag if requested if (GTSAM_USE_QUATERNIONS) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS") @@ -126,11 +137,25 @@ endif(GTSAM_BUILD_EXAMPLES) set(FIRST_PASS_DONE true CACHE BOOL "Internally used to mark whether cmake has been run multiple times") mark_as_advanced(FIRST_PASS_DONE) +# Set up CPack +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") +set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") +set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") +set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") +set(CPACK_INSTALLED_DIRECTORIES "doc" ".") # Include doc directory +set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makedoc.sh$") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs + # Record the root dir for gtsam - needed during external builds, e.g., ROS set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") - # print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") @@ -139,12 +164,18 @@ 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 ") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") +print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") +print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries") string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") +message(STATUS "Packaging flags ") +message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") +message(STATUS " CPack Generator : ${CPACK_GENERATOR}") + message(STATUS "GTSAM flags ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3") @@ -155,19 +186,5 @@ print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") message(STATUS "===============================================================") -# Set up CPack -set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") -set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") -set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) -set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) -set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) -set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -set(CPACK_INSTALLED_DIRECTORIES "doc" ".") # Include doc directory -set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makedoc.sh$") -set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") -#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs -set(CPACK_SOURCE_GENERATOR "TGZ") -set(CPACK_GENERATOR "TGZ") +# Include CPack *after* all flags include(CPack) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 673411a58..8ee371fe8 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -1,10 +1,10 @@ # install CCOLAMD headers -install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD) -install(FILES UFconfig/UFconfig.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/UFconfig) +install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) +install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) # install Eigen - only the headers install(DIRECTORY Eigen/Eigen - DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen + DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") @@ -12,6 +12,6 @@ file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") foreach(eigen_dir ${eigen_dir_headers_all}) get_filename_component(filename ${eigen_dir} NAME) if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) - install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/Eigen) + install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen) endif() endforeach(eigen_dir) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 0ca316b36..600ae2006 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -51,8 +51,6 @@ set(gtsam_srcs ${slam_srcs} ) -option (GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) - # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) @@ -60,14 +58,16 @@ message(STATUS "GTSAM Version: ${gtsam_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # build shared and static versions of the library -message(STATUS "Building GTSAM - static") -add_library(gtsam-static STATIC ${gtsam_srcs}) -set_target_properties(gtsam-static PROPERTIES - OUTPUT_NAME gtsam - CLEAN_DIRECT_OUTPUT 1 - VERSION ${gtsam_version} - SOVERSION ${gtsam_soversion}) -install(TARGETS gtsam-static ARCHIVE DESTINATION lib) +if (GTSAM_BUILD_STATIC_LIBRARY) + message(STATUS "Building GTSAM - static") + add_library(gtsam-static STATIC ${gtsam_srcs}) + set_target_properties(gtsam-static PROPERTIES + OUTPUT_NAME gtsam + CLEAN_DIRECT_OUTPUT 1 + VERSION ${gtsam_version} + SOVERSION ${gtsam_soversion}) + install(TARGETS gtsam-static ARCHIVE DESTINATION lib) +endif (GTSAM_BUILD_STATIC_LIBRARY) if (GTSAM_BUILD_SHARED_LIBRARY) message(STATUS "Building GTSAM - shared") diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index d753c5d76..8feb5779b 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -9,11 +9,11 @@ target_link_libraries(wrap wrap_lib) # Install wrap binary if (GTSAM_INSTALL_WRAP) - install(TARGETS wrap DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) + install(TARGETS wrap DESTINATION bin) endif(GTSAM_INSTALL_WRAP) # Install matlab header -install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/wrap) +install(FILES matlab.h DESTINATION include/wrap) # Build tests if (GTSAM_BUILD_TESTS) From 08bacd544f7f41ebc7208b56ecfe2e7241db92f6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 10 Apr 2012 17:13:59 +0000 Subject: [PATCH 23/72] Added make targets for creating binary/source packages for gtsam --- .cproject | 359 ++++++++++++++++++++++++++----------------------- CMakeLists.txt | 2 +- 2 files changed, 195 insertions(+), 166 deletions(-) diff --git a/.cproject b/.cproject index 00c33f6be..f241acfb8 100644 --- a/.cproject +++ b/.cproject @@ -311,14 +311,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -345,6 +337,7 @@ make + tests/testBayesTree.run true false @@ -352,6 +345,7 @@ make + testBinaryBayesNet.run true false @@ -399,6 +393,7 @@ make + testSymbolicBayesNet.run true false @@ -406,6 +401,7 @@ make + tests/testSymbolicFactor.run true false @@ -413,6 +409,7 @@ make + testSymbolicFactorGraph.run true false @@ -428,11 +425,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -459,7 +465,6 @@ make - testGraph.run true false @@ -531,7 +536,6 @@ make - testInference.run true false @@ -539,7 +543,6 @@ make - testGaussianFactor.run true false @@ -547,7 +550,6 @@ make - testJunctionTree.run true false @@ -555,7 +557,6 @@ make - testSymbolicBayesNet.run true false @@ -563,7 +564,6 @@ make - testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -689,18 +689,26 @@ true true - + make - -j5 - nonlinear.testValues.run + -j2 + all true true true - + make - -j5 - nonlinear.testOrdering.run + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -737,26 +745,18 @@ true true - + make - -j2 - all + -j5 + nonlinear.testValues.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -1067,6 +1067,7 @@ make + testErrors.run true false @@ -1522,7 +1523,6 @@ make - testSimulated2DOriented.run true false @@ -1562,7 +1562,6 @@ make - testSimulated2D.run true false @@ -1570,7 +1569,6 @@ make - testSimulated3D.run true false @@ -1754,7 +1752,6 @@ make - tests/testGaussianISAM2 true false @@ -1776,102 +1773,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 -j2 @@ -2071,42 +1972,130 @@ false true - + + cpack + + -G DEB + true + false + true + + + cpack + + -G RPM + true + false + true + + + cpack + + -G TGZ + true + false + true + + + cpack + + --config CPackSourceConfig.cmake + true + false + true + + make - -j5 - wrap.testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - wrap.testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap_gtsam + -j2 + timeRot3.run true true true - + make - -j5 - wrap + -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 @@ -2150,6 +2139,46 @@ false true + + make + -j5 + wrap.testSpirit.run + true + true + true + + + make + -j5 + wrap.testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b0bd9afa..335d25173 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,7 +147,7 @@ set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -set(CPACK_INSTALLED_DIRECTORIES "doc" ".") # Include doc directory +set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makedoc.sh$") set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") #set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs From 2365ade34ca2c432b467afd8a217a17799d4517d Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 06:37:26 +0000 Subject: [PATCH 24/72] fixed typo --- examples/Pose2SLAMExample_advanced.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 1c196488f..432b6a050 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -74,7 +74,7 @@ int main(int argc, char** argv) { /* Get covariances */ Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); - Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1)); + Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2)); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); From 1623b8ce12adfb64cfdf68e191180867b5296f58 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 06:46:19 +0000 Subject: [PATCH 25/72] converted Rot3M to fixed-size Matrix, and changed some methods elsewhere to return fixed-size Vector3 to avoid heap allocations for speedup. --- gtsam/base/Matrix.h | 2 + gtsam/base/Vector.h | 4 + gtsam/geometry/Point3.h | 9 +-- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Rot3.h | 40 ++++++---- gtsam/geometry/Rot3M.cpp | 163 ++++++++++++++++----------------------- gtsam/geometry/Rot3Q.cpp | 32 ++++---- 7 files changed, 122 insertions(+), 130 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fee61f62b..4fe3a56c4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,6 +37,8 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +typedef Eigen::Matrix3d Matrix3; + // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index abfa44364..2dd038e42 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -33,6 +33,10 @@ namespace gtsam { // Typedef arbitary length vector typedef Eigen::VectorXd Vector; +// Commonly used fixed size vectors +typedef Eigen::Vector3d Vector3; +typedef Eigen::Matrix Vector6; + typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index ad66a845c..9eeb748db 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -126,7 +126,7 @@ namespace gtsam { inline Point3 retract(const Vector& v) const { return Point3(*this + v); } /// Returns inverse retraction - inline Vector localCoordinates(const Point3& q) const { return (q -*this).vector(); } + inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } /// @} /// @name Lie Group @@ -136,7 +136,7 @@ namespace gtsam { static inline Point3 Expmap(const Vector& v) { return Point3(v); } /** Log map at identity - return the x,y,z of this point */ - static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } + static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// @} /// @name Vector Space @@ -170,9 +170,8 @@ namespace gtsam { bool operator ==(const Point3& q) const; /** return vectorized form (column-wise)*/ - Vector vector() const { - Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; - return v; + Vector3 vector() const { + return Vector3(x_,y_,z_); } /// get x diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e12ad28e1..9f343cbe4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -136,7 +136,7 @@ namespace gtsam { return Logmap(between(T)); } else if(mode == Pose3::FIRST_ORDER) { // R is always done exactly in all three retract versions below - Vector omega = R_.localCoordinates(T.rotation()); + Vector3 omega = R_.localCoordinates(T.rotation()); // Incorrect version // Independently computes the logmap of the translation and rotation diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 599e6a8e9..a948fd5d9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -33,6 +33,7 @@ #endif #include +#include #include #include @@ -59,8 +60,7 @@ namespace gtsam { /** Internal Eigen Quaternion */ Quaternion quaternion_; #else - /** We store columns ! */ - Point3 r1_, r2_, r3_; + Matrix3 rot_; #endif public: @@ -87,6 +87,9 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix& R); +// /** constructor from a fixed size rotation matrix */ +// Rot3(const Matrix3& R); + /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion @@ -183,7 +186,7 @@ namespace gtsam { /// @{ /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + void print(const std::string& s="R") const { gtsam::print((Matrix)matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3& p, double tol = 1e-9) const; @@ -250,7 +253,7 @@ namespace gtsam { Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// Returns local retract coordinates in neighborhood around current rotation - Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; + Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group @@ -268,7 +271,7 @@ namespace gtsam { /** * Log map at identity - return the canonical coordinates of this rotation */ - static Vector Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R); /// @} /// @name Group Action on Point3 @@ -294,10 +297,10 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - Matrix matrix() const; + Matrix3 matrix() const; /** return 3*3 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix3 transpose() const; /** returns column vector specified by index */ Point3 column(int index) const; @@ -309,19 +312,19 @@ namespace gtsam { * Use RQ to calculate xyz angle representation * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) */ - Vector xyz() const; + Vector3 xyz() const; /** * Use RQ to calculate yaw-pitch-roll angle representation * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) */ - Vector ypr() const; + Vector3 ypr() const; /** * Use RQ to calculate roll-pitch-yaw angle representation * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) */ - Vector rpy() const; + Vector3 rpy() const; /** * Accessor to get to component of angle representations @@ -365,9 +368,18 @@ namespace gtsam { ar & boost::serialization::make_nvp("Rot3", boost::serialization::base_object(*this)); #ifndef GTSAM_DEFAULT_QUATERNIONS - ar & BOOST_SERIALIZATION_NVP(r1_); - ar & BOOST_SERIALIZATION_NVP(r2_); - ar & BOOST_SERIALIZATION_NVP(r3_); + ar & boost::serialization::make_nvp("rot11", rot_(0,0)); + ar & boost::serialization::make_nvp("rot12", rot_(0,1)); + ar & boost::serialization::make_nvp("rot13", rot_(0,2)); + ar & boost::serialization::make_nvp("rot21", rot_(1,0)); + ar & boost::serialization::make_nvp("rot22", rot_(1,1)); + ar & boost::serialization::make_nvp("rot23", rot_(1,2)); + ar & boost::serialization::make_nvp("rot31", rot_(2,0)); + ar & boost::serialization::make_nvp("rot32", rot_(2,1)); + ar & boost::serialization::make_nvp("rot33", rot_(2,2)); +// ar & BOOST_SERIALIZATION_NVP(r1_); +// ar & BOOST_SERIALIZATION_NVP(r2_); +// ar & BOOST_SERIALIZATION_NVP(r3_); #else ar & boost::serialization::make_nvp("w", quaternion_.w()); ar & boost::serialization::make_nvp("x", quaternion_.x()); @@ -389,5 +401,5 @@ namespace gtsam { * @return an upper triangular matrix R * @return a vector [thetax, thetay, thetaz] in radians. */ - std::pair RQ(const Matrix& A); + std::pair RQ(const Matrix& A); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index be7c8c196..b698a77a5 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -28,42 +28,39 @@ using namespace std; namespace gtsam { -static const Matrix I3 = eye(3); +static const Matrix3 I3 = Matrix3::Identity(); /* ************************************************************************* */ -Rot3::Rot3() : - r1_(Point3(1.0,0.0,0.0)), - r2_(Point3(0.0,1.0,0.0)), - r3_(Point3(0.0,0.0,1.0)) {} +Rot3::Rot3() : rot_(Matrix3::Identity()) {} /* ************************************************************************* */ -Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) {} +Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) { + rot_.col(0) = r1.vector(); + rot_.col(1) = r2.vector(); + rot_.col(2) = r3.vector(); +} /* ************************************************************************* */ Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, - double R31, double R32, double R33) : - r1_(Point3(R11, R21, R31)), - r2_(Point3(R12, R22, R32)), - r3_(Point3(R13, R23, R33)) {} + double R31, double R32, double R33) { + rot_ << R11, R12, R13, + R21, R22, R23, + R31, R32, R33; +} /* ************************************************************************* */ Rot3::Rot3(const Matrix& R) { if (R.rows()!=3 || R.cols()!=3) throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - r1_ = Point3(R(0, 0), R(1, 0), R(2, 0)); - r2_ = Point3(R(0, 1), R(1, 1), R(2, 1)); - r3_ = Point3(R(0, 2), R(1, 2), R(2, 2)); + rot_ = R; } +///* ************************************************************************* */ +//Rot3::Rot3(const Matrix3& R) : rot_(R) {} + /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) { - Eigen::Matrix3d R = q.toRotationMatrix(); - r1_ = Point3(R.col(0)); - r2_ = Point3(R.col(1)); - r3_ = Point3(R.col(2)); -} +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { @@ -164,46 +161,39 @@ Point3 Rot3::operator*(const Point3& p) const { return rotate(p); } /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -matrix(); - return Rot3( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); + if (H1) *H1 = -rot_; + return Rot3(rot_.transpose()); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); + if (H1) *H1 = -(R2.transpose()*rot_); if (H2) *H2 = I3; - return between_default(*this, R2); + return Rot3(rot_.transpose()*R2.rot_); + //return between_default(*this, R2); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, boost::optional H1, boost::optional H2) const { if (H1 || H2) { - const Matrix R(matrix()); - if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = R; + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; } - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); + return Point3(rot_ * p.vector()); } /* ************************************************************************* */ // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Point3 q( - r1_.x()*p.x()+r1_.y()*p.y()+r1_.z()*p.z(), - r2_.x()*p.x()+r2_.y()*p.y()+r2_.z()*p.z(), - r3_.x()*p.x()+r3_.y()*p.y()+r3_.z()*p.z() - ); // q = Rt*p + Point3 q(rot_.transpose()*p.vector()); // q = Rt*p if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); if (H2) *H2 = transpose(); return q; @@ -211,25 +201,26 @@ Point3 Rot3::unrotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R) { static const double PI = boost::math::constants::pi(); + const Matrix3& rot = R.rot_; // Get trace(R) - double tr = R.r1_.x()+R.r2_.y()+R.r3_.z(); + double tr = rot.trace(); // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(R.r3_.z()+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*R.r3_.z())) * - Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z()); - else if(std::abs(R.r2_.y()+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*R.r2_.y())) * - Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z()); + if(std::abs(rot(2,2)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(2,2) )) * + Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); + else if(std::abs(rot(1,1)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(1,1))) * + Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*R.r1_.x())) * - Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z()); + return (PI / sqrt(2.0+2.0*rot(0,0))) * + Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; double tr_3 = tr-3.0; // always negative @@ -241,10 +232,10 @@ Vector Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector_(3, - R.r2_.z()-R.r3_.y(), - R.r3_.x()-R.r1_.z(), - R.r1_.y()-R.r2_.x()); + return magnitude*Vector3( + rot(2,1)-rot(1,2), + rot(0,2)-rot(2,0), + rot(1,0)-rot(0,1)); } } @@ -276,7 +267,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } /* ************************************************************************* */ -Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { +Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { if(mode == Rot3::EXPMAP) { return Logmap(between(T)); } else if(mode == Rot3::CAYLEY) { @@ -292,13 +283,13 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { const double x = (a * f - cd + f) * K; const double y = (b * f - ce - c) * K; const double z = (fg - di - d) * K; - return -2 * Vector_(3, x, y, z); + return -2 * Vector3(x, y, z); } else if(mode == Rot3::SLOW_CAYLEY) { // Create a fixed-size matrix Eigen::Matrix3d A(between(T).matrix()); // using templated version of Cayley Matrix Omega = Cayley<3>(A); - return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); + return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); } else { assert(false); exit(1); @@ -306,89 +297,69 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { } /* ************************************************************************* */ -Matrix Rot3::matrix() const { - Matrix R(3,3); - R << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z(); - return R; +Matrix3 Rot3::matrix() const { + return rot_; } /* ************************************************************************* */ -Matrix Rot3::transpose() const { - Matrix Rt(3,3); - Rt << - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z(); - return Rt; +Matrix3 Rot3::transpose() const { + return rot_.transpose(); } /* ************************************************************************* */ Point3 Rot3::column(int index) const{ - if(index == 3) - return r3_; - else if(index == 2) - return r2_; - else if(index == 1) - return r1_; // default returns r1 - else - throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); + return Point3(rot_.col(index)); } /* ************************************************************************* */ -Point3 Rot3::r1() const { return r1_; } +Point3 Rot3::r1() const { return Point3(rot_.col(0)); } /* ************************************************************************* */ -Point3 Rot3::r2() const { return r2_; } +Point3 Rot3::r2() const { return Point3(rot_.col(1)); } /* ************************************************************************* */ -Point3 Rot3::r3() const { return r3_; } +Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Vector Rot3::xyz() const { - Matrix I;Vector q; +Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ -Vector Rot3::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); +Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); } /* ************************************************************************* */ -Vector Rot3::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); +Vector3 Rot3::rpy() const { + Vector3 q = xyz(); + return Vector3(q(0),q(1),q(2)); } /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { - return Quaternion((Eigen::Matrix3d() << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z()).finished()); + return Quaternion(rot_); } /* ************************************************************************* */ -pair RQ(const Matrix& A) { +pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); - Matrix B = A * Qx.matrix(); + Matrix3 B = A * Qx.matrix(); double y = -atan2(B(2, 0), B(2, 2)); Rot3 Qy = Rot3::Ry(-y); - Matrix C = B * Qy.matrix(); + Matrix3 C = B * Qy.matrix(); double z = -atan2(-C(1, 0), C(1, 1)); Rot3 Qz = Rot3::Rz(-z); - Matrix R = C * Qz.matrix(); + Matrix3 R = C * Qz.matrix(); - Vector xyz = Vector_(3, x, y, z); + Vector xyz = Vector3(x, y, z); return make_pair(R, xyz); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d86c37a30..c67834be3 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -49,6 +49,10 @@ namespace gtsam { Rot3::Rot3(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {} +// /* ************************************************************************* */ +// Rot3::Rot3(const Matrix3& R) : +// quaternion_(R) {} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} @@ -140,7 +144,7 @@ namespace gtsam { /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R) { Eigen::AngleAxisd angleAxis(R.quaternion_); if(angleAxis.angle() > M_PI) // Important: use the smallest possible angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep @@ -155,15 +159,15 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { + Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { return Logmap(between(t2)); } /* ************************************************************************* */ - Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); } + Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); } /* ************************************************************************* */ - Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } + Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } /* ************************************************************************* */ Point3 Rot3::column(int index) const{ @@ -187,29 +191,29 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Vector Rot3::xyz() const { - Matrix I;Vector q; + Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ - Vector Rot3::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); + Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); } /* ************************************************************************* */ - Vector Rot3::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); + Vector3 Rot3::rpy() const { + Vector3 q = xyz(); + return Vector3(q(0),q(1),q(2)); } /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ - pair RQ(const Matrix& A) { + pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); @@ -223,7 +227,7 @@ namespace gtsam { Rot3 Qz = Rot3::Rz(-z); Matrix R = C * Qz.matrix(); - Vector xyz = Vector_(3, x, y, z); + Vector xyz = Vector3(x, y, z); return make_pair(R, xyz); } From 2c62712be61b2e9f00c10a8e479fc9cc7ddb3b31 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 14:13:49 +0000 Subject: [PATCH 26/72] fix for quaternion version --- gtsam/geometry/Rot3Q.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index c67834be3..dfa2d546d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -213,19 +213,19 @@ namespace gtsam { Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ - pair RQ(const Matrix& A) { + pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); - Matrix B = A * Qx.matrix(); + Matrix3 B = A * Qx.matrix(); double y = -atan2(B(2, 0), B(2, 2)); Rot3 Qy = Rot3::Ry(-y); - Matrix C = B * Qy.matrix(); + Matrix3 C = B * Qy.matrix(); double z = -atan2(-C(1, 0), C(1, 1)); Rot3 Qz = Rot3::Rz(-z); - Matrix R = C * Qz.matrix(); + Matrix3 R = C * Qz.matrix(); Vector xyz = Vector3(x, y, z); return make_pair(R, xyz); From 91e7dc5882937636e7b6b129ae9c6aa17bc8dcbc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 11 Apr 2012 14:17:59 +0000 Subject: [PATCH 27/72] Caching linearized factors in iSAM2, improves speed when linearization is expensive Merge remote-tracking branch 'svn/branches/iSAM2_cache_linearized' into trunk Conflicts: .cproject --- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 3 +- gtsam/linear/HessianFactor.h | 8 -- gtsam/linear/JacobianFactor.cpp | 37 -------- gtsam/linear/JacobianFactor.h | 7 -- .../linear/tests/testGaussianFactorGraph.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 3 + gtsam/nonlinear/ISAM2.cpp | 87 +++++++++++++------ gtsam/nonlinear/ISAM2.h | 19 +++- 9 files changed, 81 insertions(+), 87 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 2d889ee64..3fdad19c6 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -96,7 +96,7 @@ namespace gtsam { * to already be inverted. This acts just as a change-of-name for each * variable. The order of the variables within the factor is not changed. */ - virtual void permuteWithInverse(const Permutation& inversePermutation) = 0; + virtual void permuteWithInverse(const Permutation& inversePermutation) { IndexFactor::permuteWithInverse(inversePermutation); } private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 56a377c61..0241bc3b3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -51,7 +51,8 @@ namespace gtsam { void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->permuteWithInverse(inversePermutation); + if(factor) + factor->permuteWithInverse(inversePermutation); } } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 7cba0c744..91e048b3c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -270,14 +270,6 @@ namespace gtsam { * @return The linear term \f$ g \f$ */ constColumn linearTerm() const; - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation) { - IndexFactor::permuteWithInverse(inversePermutation); } - // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; friend class ::Constructor1HessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c48a62431..688e23fe4 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -250,43 +250,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - void JacobianFactor::permuteWithInverse(const Permutation& inversePermutation) { - - // Build a map from the new variable indices to the old slot positions. - typedef FastMap SourceSlots; - SourceSlots sourceSlots; - for(size_t j=0; j dimensions(size() + 1); - size_t j = 0; - BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { - dimensions[j++] = Ab_(sourceSlot.second).cols(); - } - assert(j == size()); - dimensions.back() = 1; - - // Copy the variables and matrix into the new order - vector oldKeys(size()); - keys_.swap(oldKeys); - AbMatrix oldMatrix; - BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows()); - Ab_.swap(oldAb); - j = 0; - BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { - keys_[j] = sourceSlot.first; - Ab_(j++).noalias() = oldAb(sourceSlot.second); - } - Ab_(j).noalias() = oldAb(j); - - // Since we're permuting the variables, ensure that entire rows from this - // factor are copied when Combine is called - BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; } - assertInvariants(); - } - /* ************************************************************************* */ Vector JacobianFactor::unweighted_error(const VectorValues& c) const { Vector e = -getb(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ec661066d..5a756e43b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -169,13 +169,6 @@ namespace gtsam { */ virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation); - /** * return the number of rows in the corresponding linear system */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ade5e65ce..34009f856 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -552,7 +552,7 @@ TEST(GaussianFactor, permuteWithInverse) actual.permuteWithInverse(inversePermutation); // actualIndex.permute(*inversePermutation.inverse()); - JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); + JacobianFactor expected(4, A1, 2, A2, 0, A3, b, sharedSigma(2, 1.0)); GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); // GaussianVariableIndex expectedIndex(expectedFG); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index e4cb2abd3..ec3b4edfd 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -257,6 +257,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, cout << "Full var-ordered eliminated BT:\n"; result.bayesTree->printTree(""); } + // Undo permutation on our subset of cached factors, we must later permute *all* of the cached factors + factors.permuteWithInverse(*affectedColamd); + factors.permuteWithInverse(affectedKeysSelector); toc(8,"permute eliminated"); return result; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6bb74dd8a..d8138366b 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -78,6 +78,12 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) { deltaUptodate_ = rhs.deltaUptodate_; deltaReplacedMask_ = rhs.deltaReplacedMask_; nonlinearFactors_ = rhs.nonlinearFactors_; + + linearFactors_ = GaussianFactorGraph(); + linearFactors_.reserve(rhs.linearFactors_.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) { + linearFactors_.push_back(linearFactor->clone()); } + ordering_ = rhs.ordering_; params_ = rhs.params_; doglegDelta_ = rhs.doglegDelta_; @@ -125,7 +131,7 @@ FastList ISAM2::getAffectedFactors(const FastList& keys) const { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); @@ -139,24 +145,37 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); toc(2,"affectedKeysSet"); - tic(3,"check candidates"); + tic(3,"check candidates and linearize"); + FactorGraph::shared_ptr linearized = boost::make_shared >(); BOOST_FOREACH(size_t idx, candidates) { bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { Index var = ordering_[key]; - if (affectedKeysSet.find(var) == affectedKeysSet.end()) { + if(affectedKeysSet.find(var) == affectedKeysSet.end()) { inside = false; break; } + if(useCachedLinear && relinKeys.find(var) != relinKeys.end()) + useCachedLinear = false; + } + if(inside) { + if(useCachedLinear) { + assert(linearFactors_[idx]); + linearized->push_back(linearFactors_[idx]); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys()); + } else { + GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); + linearized->push_back(linearFactor); + if(params_.cacheLinearizedFactors) { + assert(linearFactors_[idx]->keys() == linearFactor->keys()); + linearFactors_[idx] = linearFactor; + } + } } - if (inside) - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - toc(3,"check candidates"); + toc(3,"check candidates and linearize"); - tic(4,"linearize"); - FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); - toc(4,"linearize"); return linearized; } @@ -187,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& newKeys, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -300,11 +319,11 @@ boost::shared_ptr > ISAM2::recalculate( toc(1,"reorder"); tic(2,"linearize"); - GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); + linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_); toc(2,"linearize"); tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); + JunctionTree jt(linearFactors_, variableIndex_); sharedClique newRoot; if(params_.factorization == ISAM2Params::LDL) newRoot = jt.eliminate(EliminatePreferLDL); @@ -325,7 +344,7 @@ boost::shared_ptr > ISAM2::recalculate( lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = factors.size(); + lastAffectedFactorCount = linearFactors_.size(); return affectedKeysSet; @@ -338,7 +357,7 @@ boost::shared_ptr > ISAM2::recalculate( affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); tic(1,"relinearizeAffected"); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); if(debug) factors.print("Relinearized factors: "); toc(1,"relinearizeAffected"); @@ -360,11 +379,7 @@ boost::shared_ptr > ISAM2::recalculate( // add the cached intermediate results from the boundary of the orphans ... GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); if(debug) cachedBoundary.print("Boundary factors: "); - factors.reserve(factors.size() + cachedBoundary.size()); - // Copy so that we can later permute factors - BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) { - factors.push_back(cached->clone()); - } + factors.push_back(cachedBoundary); toc(2,"cached"); // END OF COPIED CODE @@ -410,6 +425,15 @@ boost::shared_ptr > ISAM2::recalculate( tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); toc(5,"permute ordering"); + if(params_.cacheLinearizedFactors) { + tic(6,"permute cached linear"); + //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); + BOOST_FOREACH(size_t idx, permuteLinearIndices) { + linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse); + } + toc(6,"permute cached linear"); + } toc(4,"reorder and eliminate"); @@ -526,11 +550,12 @@ ISAM2Result ISAM2::update( toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + FastSet relinKeys; if (relinearizeThisStep) { tic(5,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); + relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging // Add the variables being relinearized to the marked keys @@ -563,15 +588,21 @@ ISAM2Result ISAM2::update( } tic(8,"linearize new"); - tic(1,"linearize"); // 7. Linearize new factors - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); - toc(1,"linearize"); + if(params_.cacheLinearizedFactors) { + tic(1,"linearize"); + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + linearFactors_.push_back(*linearFactors); + assert(nonlinearFactors_.size() == linearFactors_.size()); + toc(1,"linearize"); - tic(2,"augment VI"); - // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); - toc(2,"augment VI"); + tic(2,"augment VI"); + // Augment the variable index with the new factors + variableIndex_.augment(*linearFactors); + toc(2,"augment VI"); + } else { + variableIndex_.augment(*newFactors.symbolic(ordering_)); + } toc(8,"linearize new"); tic(9,"recalculate"); @@ -587,7 +618,7 @@ ISAM2Result ISAM2::update( } boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); + replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5e7adc19d..21f929ed9 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -118,6 +118,13 @@ struct ISAM2Params { */ Factorization factorization; + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) /** Specify parameters as constructor arguments */ @@ -128,11 +135,12 @@ struct ISAM2Params { bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization + bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - keyFormatter(_keyFormatter) {} + cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {} }; /** @@ -341,6 +349,9 @@ protected: /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; + /** The current linear factors, which are only updated as needed */ + mutable GaussianFactorGraph linearFactors_; + /** The current elimination ordering Symbols to Index (integer) keys. * * We keep it up to date as we add and reorder variables. @@ -453,11 +464,11 @@ public: private: FastList getAffectedFactors(const FastList& keys) const; - FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys) const; + FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - boost::shared_ptr > recalculate(const FastSet& markedKeys, - const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const FastVector& newKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; From 5839d1bfaa7adf3a382f14fc1a6e093a511bf9cc Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 15:12:39 +0000 Subject: [PATCH 28/72] minor improvements to RQ and cleanup --- gtsam/geometry/Rot3.h | 5 +---- gtsam/geometry/Rot3M.cpp | 9 ++++----- gtsam/geometry/Rot3Q.cpp | 2 +- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index a948fd5d9..93e276de7 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -377,9 +377,6 @@ namespace gtsam { ar & boost::serialization::make_nvp("rot31", rot_(2,0)); ar & boost::serialization::make_nvp("rot32", rot_(2,1)); ar & boost::serialization::make_nvp("rot33", rot_(2,2)); -// ar & BOOST_SERIALIZATION_NVP(r1_); -// ar & BOOST_SERIALIZATION_NVP(r2_); -// ar & BOOST_SERIALIZATION_NVP(r3_); #else ar & boost::serialization::make_nvp("w", quaternion_.w()); ar & boost::serialization::make_nvp("x", quaternion_.x()); @@ -401,5 +398,5 @@ namespace gtsam { * @return an upper triangular matrix R * @return a vector [thetax, thetay, thetaz] in radians. */ - std::pair RQ(const Matrix& A); + std::pair RQ(const Matrix3& A); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b698a77a5..aba684a95 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -322,8 +322,8 @@ Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; - boost::tie(I,q)=RQ(matrix()); + Matrix3 I;Vector3 q; + boost::tie(I,q)=RQ(rot_); return q; } @@ -335,8 +335,7 @@ Vector3 Rot3::ypr() const { /* ************************************************************************* */ Vector3 Rot3::rpy() const { - Vector3 q = xyz(); - return Vector3(q(0),q(1),q(2)); + return xyz(); } /* ************************************************************************* */ @@ -345,7 +344,7 @@ Quaternion Rot3::toQuaternion() const { } /* ************************************************************************* */ -pair RQ(const Matrix& A) { +pair RQ(const Matrix3& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index dfa2d546d..3a68e1a92 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -213,7 +213,7 @@ namespace gtsam { Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ - pair RQ(const Matrix& A) { + pair RQ(const Matrix3& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); From 6d28e0a039d5ca1c9f11f3a65f67ae2adcca474b Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 15:15:12 +0000 Subject: [PATCH 29/72] fixed size skewSymmetric --- gtsam/base/Matrix.cpp | 6 +++--- gtsam/base/Matrix.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index b3c51325d..849dc3f5a 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -541,12 +541,12 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) { } /* ************************************************************************* */ -Matrix skewSymmetric(double wx, double wy, double wz) +Matrix3 skewSymmetric(double wx, double wy, double wz) { - return Matrix_(3,3, + return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, - -wy, +wx, 0.0); + -wy, +wx, 0.0).finished(); } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 4fe3a56c4..131c5f958 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -395,7 +395,7 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // * @param wz * @return a 3*3 skew symmetric matrix */ -Matrix skewSymmetric(double wx, double wy, double wz); +Matrix3 skewSymmetric(double wx, double wy, double wz); inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));} /** Use SVD to calculate inverse square root of a matrix */ From 40bc02cca84f128bbf39154ddfd18969dc7cf1bd Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 11 Apr 2012 15:40:55 +0000 Subject: [PATCH 30/72] added constructor to convert fixed size vectors to a LieVector --- gtsam/base/LieVector.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 9a8e5480f..82455e173 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -34,6 +34,10 @@ struct LieVector : public Vector, public DerivedValue { /** 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) {} + /** wrap a double */ LieVector(double d) : Vector(Vector_(1, d)) {} From 35255dc0845cad18ecc9deeed25086677f66004b Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Wed, 11 Apr 2012 15:50:03 +0000 Subject: [PATCH 31/72] add verbosity --- .../linear/IterativeOptimizationParameters.h | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index 7bba870bb..d3d5e8c76 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -83,14 +83,18 @@ struct PreconditionerParameters { CombinatorialParameters combinatorial_; - PreconditionerParameters(): kernel_(CHOLMOD), type_(Combinatorial) {} - PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial) - : kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial) {} + enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ + + PreconditionerParameters(): kernel_(CHOLMOD), type_(Combinatorial), verbosity_(SILENT) {} + PreconditionerParameters(Kernel kernel, const CombinatorialParameters &combinatorial, Verbosity verbosity) + : kernel_(kernel), type_(Combinatorial), combinatorial_(combinatorial), verbosity_(verbosity) {} /* general interface */ inline Kernel kernel() const { return kernel_; } inline Type type() const { return type_; } inline const CombinatorialParameters & combinatorial() const { return combinatorial_; } + inline Verbosity verbosity() const { return verbosity_; } + void print() const { const std::string kernelStr[2] = {"gtsam", "cholmod"}; @@ -119,16 +123,18 @@ struct ConjugateGradientParameters { CHOLMOD /* Cholmod Sparse */ } blas_kernel_; - /* Specific for PCGPlus */ size_t degree_; /* the maximum degree of the vertices to be eliminated before doing cg */ + enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ + ConjugateGradientParameters() : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3), - blas_kernel_(GTSAM), degree_(0) {} + blas_kernel_(GTSAM), degree_(0), verbosity_(SILENT) {} + ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, - double epsilon_rel, double epsilon_abs, BLASKernel blas = GTSAM, size_t degree = 0) + double epsilon_rel, double epsilon_abs, BLASKernel blas = GTSAM, size_t degree = 0, Verbosity verbosity = SILENT) : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), - epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas), degree_(degree) {} + epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas), degree_(degree), verbosity_(verbosity) {} /* general interface */ inline size_t minIterations() const { return minIterations_; } @@ -139,6 +145,7 @@ struct ConjugateGradientParameters { inline double epsilon_abs() const { return epsilon_abs_; } inline BLASKernel blas_kernel() const { return blas_kernel_; } inline size_t degree() const { return degree_; } + inline Verbosity verbosity() const { return verbosity_; } void print() const { const std::string blasStr[4] = {"gtsam", "sbm", "sm", "cholmod"}; @@ -164,7 +171,7 @@ public: PreconditionerParameters preconditioner_; ConjugateGradientParameters cg_; enum Kernel { PCG = 0 /*, PCGPlus*/, LSPCG } kernel_ ; /* Iterative Method Kernel */ - enum Verbosity { SILENT = 0, ERROR } verbosity_ ; /* Verbosity */ + enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ public: From fbef6ce63f39ad643cb836e5057e36a843004cd7 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 17:01:16 +0000 Subject: [PATCH 32/72] tiny inline fix --- gtsam/base/Matrix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 131c5f958..a73c85c1b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -396,7 +396,7 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // * @return a 3*3 skew symmetric matrix */ Matrix3 skewSymmetric(double wx, double wy, double wz); -inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));} +inline Matrix3 skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));} /** Use SVD to calculate inverse square root of a matrix */ Matrix inverse_square_root(const Matrix& A); From 792c8ee55a67249e9f777b7e37491dc966df04d0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 12 Apr 2012 03:04:32 +0000 Subject: [PATCH 33/72] Detailed results optionally returned by ISAM2::update, with the status of each variable --- gtsam/nonlinear/ISAM2.cpp | 76 +++++++++++++++++++++++++++++---------- gtsam/nonlinear/ISAM2.h | 39 ++++++++++++++++++-- 2 files changed, 94 insertions(+), 21 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d8138366b..e00c93cae 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -206,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& newKeys, + const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -237,8 +237,8 @@ boost::shared_ptr > ISAM2::recalculate( cout << "markedKeys: "; BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } cout << endl; - cout << "newKeys: "; - BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } + cout << "observedKeys: "; + BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; } cout << endl; } @@ -270,12 +270,13 @@ boost::shared_ptr > ISAM2::recalculate( FastList affectedKeys = affectedBayesNet.ordering(); toc(2,"affectedKeys"); + boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + if(affectedKeys.size() >= theta_.size() * batchThreshold) { tic(3,"batch"); tic(0,"add keys"); - boost::shared_ptr > affectedKeysSet(new FastSet()); BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } toc(0,"add keys"); @@ -296,8 +297,8 @@ boost::shared_ptr > ISAM2::recalculate( } } } else { - if(theta_.size() > newKeys.size()) { // Only if some variables are unconstrained - BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } + if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained + BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; } } } Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); @@ -338,15 +339,17 @@ boost::shared_ptr > ISAM2::recalculate( this->insert(newRoot); toc(6,"insert"); - toc(3,"batch"); - result.variablesReeliminated = affectedKeysSet->size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedFactorCount = linearFactors_.size(); - return affectedKeysSet; + // Reeliminated keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } + + toc(3,"batch"); } else { @@ -355,7 +358,7 @@ boost::shared_ptr > ISAM2::recalculate( // 2. Add the new factors \Factors' into the resulting factor graph FastList affectedAndNewKeys; affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); tic(1,"relinearizeAffected"); GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); if(debug) factors.print("Relinearized factors: "); @@ -363,6 +366,10 @@ boost::shared_ptr > ISAM2::recalculate( if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + // Reeliminated keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; } + result.variablesReeliminated = affectedAndNewKeys.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); @@ -391,7 +398,7 @@ boost::shared_ptr > ISAM2::recalculate( tic(1,"list to set"); // create a partial reordering for the new and contaminated factors // markedKeys are passed in: those variables will be forced to the end in the ordering - boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); + affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); toc(1,"list to set"); @@ -404,7 +411,7 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.constrainedKeys = *constrainKeys; } else { reorderingMode.constrainedKeys = FastMap(); - BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } + BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } } Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR)); @@ -467,9 +474,13 @@ boost::shared_ptr > ISAM2::recalculate( toc(7,"orphans"); toc(4,"incremental"); - - return affectedKeysSet; } + + // Root clique variables for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; } + + return affectedKeysSet; } /* ************************************************************************* */ @@ -490,6 +501,8 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; + if(params_.enableDetailedResults) + result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); if(verbose) { @@ -528,6 +541,10 @@ ISAM2Result ISAM2::update( tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); + // New keys for detailed results + if(params_.enableDetailedResults) { + inverseOrdering_ = ordering_.invert(); + BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } toc(2,"add new variables"); tic(3,"evaluate error before"); @@ -543,10 +560,13 @@ ISAM2Result ISAM2::update( FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } + // Observed keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. - FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold @@ -558,6 +578,12 @@ ISAM2Result ISAM2::update( relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + // Above relin threshold keys for detailed results + if(params_.enableDetailedResults) { + BOOST_FOREACH(Index index, relinKeys) { + result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true; + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } markedKeys.insert(relinKeys.begin(), relinKeys.end()); @@ -565,8 +591,20 @@ ISAM2Result ISAM2::update( tic(6,"fluid find_all"); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) - Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator + if (!relinKeys.empty() && this->root()) { + // add other cliques that have the marked ones in the separator + Impl::FindAll(this->root(), markedKeys, markedRelinMask); + + // Relin involved keys for detailed results + if(params_.enableDetailedResults) { + FastSet involvedRelinKeys; + Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); + BOOST_FOREACH(Index index, involvedRelinKeys) { + if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) { + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true; + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + } + } toc(6,"fluid find_all"); tic(7,"expmap"); @@ -617,8 +655,8 @@ ISAM2Result ISAM2::update( } } boost::shared_ptr > replacedKeys; - if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result); + if(!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 21f929ed9..2d576ec5d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -127,6 +127,8 @@ struct ISAM2Params { KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) + bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -140,7 +142,8 @@ struct ISAM2Params { ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {} + cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), + enableDetailedResults(false) {} }; /** @@ -201,6 +204,35 @@ struct ISAM2Result { * used later to refer to the factors in order to remove them. */ FastVector newFactorsIndices; + + /** A struct holding detailed results, which must be enabled with + * ISAM2Params::enableDetailedResults. + */ + struct DetailedResults { + /** The status of a single variable, this struct is stored in + * DetailedResults::variableStatus */ + struct VariableStatus { + /** Whether the variable was just reeliminated, due to being relinearized, + * observed, new, or on the path up to the root clique from another + * reeliminated variable. */ + bool isReeliminated; + bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement. + bool isObserved; ///< Whether the variable was just involved in new factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus(): isReeliminated(false), isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} + }; + + /** The status of each variable during this update, see VariableStatus. + */ + FastMap variableStatus; + }; + + /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See + * Detail for information about the results data stored here. */ + boost::optional detail; }; struct ISAM2Clique : public BayesTreeCliqueBase { @@ -364,6 +396,9 @@ protected: /** The current Dogleg Delta (trust region radius) */ mutable boost::optional doglegDelta_; + /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */ + boost::optional inverseOrdering_; + private: #ifndef NDEBUG std::vector lastRelinVariables_; @@ -468,7 +503,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& newKeys, + const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; From 317a0ce832ce114ce6b94c5947f7edc0794cf29f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 12 Apr 2012 16:13:19 +0000 Subject: [PATCH 34/72] Updated version number to match CMake files --- Doxyfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Doxyfile b/Doxyfile index 2ecf956f5..07a715a3c 100644 --- a/Doxyfile +++ b/Doxyfile @@ -32,7 +32,7 @@ PROJECT_NAME = gtsam # This could be handy for archiving the generated documentation or # if some version control system is used. -PROJECT_NUMBER = 0.9.3 +PROJECT_NUMBER = 1.9.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer From 6fb80c983dc058dd598adfac50ea7e0250b81b3b Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 12 Apr 2012 18:46:55 +0000 Subject: [PATCH 35/72] Added missing default values for detailed results --- gtsam/nonlinear/ISAM2.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2d576ec5d..c03bb9eef 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -222,7 +222,8 @@ struct ISAM2Result { bool isObserved; ///< Whether the variable was just involved in new factors bool isNew; ///< Whether the variable itself was just added bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus(): isReeliminated(false), isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} + VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false), + isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} }; /** The status of each variable during this update, see VariableStatus. From 995fda8e21d060bbfaa3a81ac3955e8b5cdb4b72 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 12 Apr 2012 19:55:11 +0000 Subject: [PATCH 36/72] Fixed VariableIndex deep copy --- gtsam/inference/VariableIndex.cpp | 14 +++++++++ gtsam/inference/VariableIndex.h | 11 ++++++- gtsam/inference/tests/testVariableIndex.cpp | 34 +++++++++++++++++++++ 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 32780c220..c7ef90e9f 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -23,6 +23,20 @@ namespace gtsam { using namespace std; +/* ************************************************************************* */ +VariableIndex::VariableIndex(const VariableIndex& other) : + index_(indexUnpermuted_) { + *this = other; +} + +/* ************************************************************************* */ +VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) { + index_ = rhs.index_; + nFactors_ = rhs.nFactors_; + nEntries_ = rhs.nEntries_; + return *this; +} + /* ************************************************************************* */ void VariableIndex::permute(const Permutation& permutation) { #ifndef NDEBUG diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 878f6a85a..c9efc6b22 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -70,6 +70,16 @@ public: */ template VariableIndex(const FactorGraph& factorGraph); + /** + * Copy constructor + */ + VariableIndex(const VariableIndex& other); + + /** + * Assignment operator + */ + VariableIndex& operator=(const VariableIndex& rhs); + /// @} /// @name Standard Interface /// @{ @@ -127,7 +137,6 @@ public: */ template void remove(const CONTAINER& indices, const FactorGraph& factors); - /** * Apply a variable permutation. Does not rearrange data, just permutes * future lookups by variable. diff --git a/gtsam/inference/tests/testVariableIndex.cpp b/gtsam/inference/tests/testVariableIndex.cpp index 2ffc03a97..a990a5a87 100644 --- a/gtsam/inference/tests/testVariableIndex.cpp +++ b/gtsam/inference/tests/testVariableIndex.cpp @@ -75,7 +75,41 @@ TEST(VariableIndex, remove) { actual.remove(indices, fg1); CHECK(assert_equal(expected, actual)); +} +/* ************************************************************************* */ +TEST(VariableIndex, deep_copy) { + + SymbolicFactorGraph fg1, fg2; + fg1.push_factor(0, 1); + fg1.push_factor(0, 2); + fg1.push_factor(5, 9); + fg1.push_factor(2, 3); + fg2.push_factor(1, 3); + fg2.push_factor(2, 4); + fg2.push_factor(3, 5); + fg2.push_factor(5, 6); + + // Create original graph and VariableIndex + SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); + VariableIndex original(fgOriginal); + VariableIndex expectedOriginal(fgOriginal); + + // Create a factor graph containing only the factors from fg2 and with null + // factors in the place of those of fg1, so that the factor indices are correct. + SymbolicFactorGraph fg2removed(fgOriginal); + fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); + VariableIndex expectedRemoved(fg2removed); + + // Create a clone and modify the clone - the original should not change + VariableIndex clone(original); + vector indices; + indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); + clone.remove(indices, fg1); + + // When modifying the clone, the original should have stayed the same + EXPECT(assert_equal(expectedOriginal, original)); + EXPECT(assert_equal(expectedRemoved, clone)); } /* ************************************************************************* */ From c4f31ea273ce9823d21679f35ad4aa3c9b3503d3 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 13 Apr 2012 15:42:12 +0000 Subject: [PATCH 37/72] Added function to set random seed for static RNGf --- .cproject | 387 ++++++++++++++++---------------- gtsam/base/Vector.cpp | 11 + gtsam/base/Vector.h | 35 ++- gtsam/base/tests/testVector.cpp | 21 ++ 4 files changed, 258 insertions(+), 196 deletions(-) diff --git a/.cproject b/.cproject index f241acfb8..e7bb2cd93 100644 --- a/.cproject +++ b/.cproject @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -465,6 +459,7 @@ make + testGraph.run true false @@ -536,6 +531,7 @@ make + testInference.run true false @@ -543,6 +539,7 @@ make + testGaussianFactor.run true false @@ -550,6 +547,7 @@ make + testJunctionTree.run true false @@ -557,6 +555,7 @@ make + testSymbolicBayesNet.run true false @@ -564,6 +563,7 @@ make + testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -689,26 +689,18 @@ true true - + make - -j2 - all + -j5 + nonlinear.testValues.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -745,18 +737,26 @@ true true - + make - -j5 - nonlinear.testValues.run + -j2 + all true true true - + make - -j5 - nonlinear.testOrdering.run + -j2 + check + true + true + true + + + make + -j2 + clean true true true @@ -1067,7 +1067,6 @@ make - testErrors.run true false @@ -1523,6 +1522,7 @@ make + testSimulated2DOriented.run true false @@ -1562,6 +1562,7 @@ make + testSimulated2D.run true false @@ -1569,6 +1570,7 @@ make + testSimulated3D.run true false @@ -1582,6 +1584,14 @@ true true + + make + -j5 + testVector.run + true + true + true + make -j2 @@ -1752,6 +1762,7 @@ make + tests/testGaussianISAM2 true false @@ -1773,6 +1784,102 @@ 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 -j2 @@ -1974,7 +2081,6 @@ cpack - -G DEB true false @@ -1982,7 +2088,6 @@ cpack - -G RPM true false @@ -1990,7 +2095,6 @@ cpack - -G TGZ true false @@ -1998,147 +2102,11 @@ cpack - --config CPackSourceConfig.cmake true false 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 - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - make -j5 @@ -2179,6 +2147,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 5367cec09..27b56830c 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -38,6 +38,8 @@ using namespace std; +boost::minstd_rand generator(42u); + namespace gtsam { /* ************************************************************************* */ @@ -197,6 +199,15 @@ bool assert_equal(const Vector& expected, const Vector& actual, double tol) { 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; +} + /* ************************************************************************* */ bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) { if (equal_with_abs_tol(expected,actual,tol)) return true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2dd038e42..84fd506cb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -25,11 +25,20 @@ #include #include -// Vector is just a typedef of the Eigen dynamic vector type -// TODO: make a version that works for matlab wrapping +/** + * Static random number generator - needs to maintain a state + * over time, hence the static generator. Be careful in + * cases where multiple processes (as is frequently the case with + * multi-robot scenarios) are using the sample() facilities + * in NoiseModel, as they will each have the same seed. + */ +// FIXME: make this go away - use the Sampler class instead +extern boost::minstd_rand generator; namespace gtsam { +// Vector is just a typedef of the Eigen dynamic vector type + // Typedef arbitary length vector typedef Eigen::VectorXd Vector; @@ -160,6 +169,15 @@ inline bool equal(const Vector& vec1, const Vector& vec2) { */ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9); +/** + * Not the same, prints if error + * @param vec1 Vector + * @param vec2 Vector + * @param tol 1e-9 + * @return bool + */ +bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9); + /** * Same, prints if error * @param vec1 Vector @@ -344,12 +362,17 @@ Vector concatVectors(size_t nrVectors, ...); */ Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1); +/** + * Sets the generator to use a different seed value. + * Default argument resets the RNG + * @param seed is the new seed + */ +inline void seedRNG(unsigned int seed = 42u) { + generator.seed(seed); +} + } // namespace gtsam -// FIXME: make this go away - use the Sampler class instead -static boost::minstd_rand generator(42u); - - #include #include diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 7506043c3..8b8757027 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -297,6 +297,27 @@ TEST( TestVector, linear_dependent3 ) EXPECT(!linear_dependent(v1, v2)); } +/* ************************************************************************* */ +TEST( TestVector, random ) +{ + // Assumes seed not previously reset during this test + seedRNG(); + Vector v1_42 = rand_vector_norm(5); + + // verify that resetting the RNG produces the same value + seedRNG(); + Vector v2_42 = rand_vector_norm(5); + + EXPECT(assert_equal(v1_42, v2_42, 1e-6)); + + // verify that different seed produces a different value + seedRNG(41u); + + Vector v3_41 = rand_vector_norm(5); + + EXPECT(assert_inequal(v1_42, v3_41, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 444c7fccf1c5cd337d2364187638418d14c2e59f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 15 Apr 2012 22:35:28 +0000 Subject: [PATCH 38/72] Moving discrete files into gtsam --- gtsam/discrete/AlgebraicDecisionTree.h | 134 +++ gtsam/discrete/AllDiff.cpp | 110 ++ gtsam/discrete/AllDiff.h | 64 ++ gtsam/discrete/Assignment.h | 36 + gtsam/discrete/BinaryAllDiff.h | 87 ++ gtsam/discrete/CMakeLists.txt | 35 + gtsam/discrete/CSP.cpp | 94 ++ gtsam/discrete/CSP.h | 71 ++ gtsam/discrete/DecisionTree-inl.h | 667 ++++++++++++ gtsam/discrete/DecisionTree.h | 218 ++++ gtsam/discrete/DecisionTreeFactor.cpp | 91 ++ gtsam/discrete/DecisionTreeFactor.h | 148 +++ gtsam/discrete/DiscreteBayesNet.cpp | 48 + gtsam/discrete/DiscreteBayesNet.h | 33 + gtsam/discrete/DiscreteConditional.cpp | 152 +++ gtsam/discrete/DiscreteConditional.h | 110 ++ gtsam/discrete/DiscreteFactor.cpp | 21 + gtsam/discrete/DiscreteFactor.h | 108 ++ gtsam/discrete/DiscreteFactorGraph.cpp | 82 ++ gtsam/discrete/DiscreteFactorGraph.h | 87 ++ gtsam/discrete/DiscreteKey.cpp | 57 + gtsam/discrete/DiscreteKey.h | 133 +++ gtsam/discrete/DiscreteSequentialSolver.cpp | 47 + gtsam/discrete/DiscreteSequentialSolver.h | 97 ++ gtsam/discrete/Domain.cpp | 95 ++ gtsam/discrete/Domain.h | 107 ++ gtsam/discrete/PotentialTable.cpp | 162 +++ gtsam/discrete/PotentialTable.h | 95 ++ gtsam/discrete/Potentials.cpp | 53 + gtsam/discrete/Potentials.h | 62 ++ gtsam/discrete/RefCounted.cpp | 9 + gtsam/discrete/RefCounted.h | 86 ++ gtsam/discrete/Scheduler.cpp | 297 ++++++ gtsam/discrete/Scheduler.h | 171 +++ gtsam/discrete/Signature.cpp | 217 ++++ gtsam/discrete/Signature.h | 129 +++ gtsam/discrete/SingleValue.cpp | 78 ++ gtsam/discrete/SingleValue.h | 72 ++ gtsam/discrete/TypedDiscreteFactor.cpp | 117 ++ gtsam/discrete/TypedDiscreteFactor.h | 68 ++ gtsam/discrete/TypedDiscreteFactorGraph.cpp | 68 ++ gtsam/discrete/TypedDiscreteFactorGraph.h | 50 + gtsam/discrete/examples/Doodle.csv | 1 + gtsam/discrete/examples/Doodle.xls | Bin 0 -> 13824 bytes gtsam/discrete/examples/Doodle2012.csv | 1 + gtsam/discrete/examples/Doodle2012.xls | Bin 0 -> 13312 bytes gtsam/discrete/examples/intrusive.xlsx | Bin 0 -> 43718 bytes gtsam/discrete/examples/schedulingExample.cpp | 344 ++++++ gtsam/discrete/examples/schedulingQuals12.cpp | 264 +++++ gtsam/discrete/examples/small.csv | 1 + gtsam/discrete/label_traits.h | 41 + gtsam/discrete/parseUAI.cpp | 157 +++ gtsam/discrete/parseUAI.h | 22 + gtsam/discrete/tests/data/FG/alarm.fg | 935 ++++++++++++++++ .../discrete/tests/data/UAI/sampleMARKOV.uai | 18 + .../tests/data/UAI/sampleMARKOV.uai.evid | 3 + gtsam/discrete/tests/data/UAI/uai08_test1.uai | 996 ++++++++++++++++++ .../tests/data/UAI/uai08_test1.uai.evid | 11 + .../tests/data/UAI/uai08_test1.uai.output | 3 + gtsam/discrete/tests/data/UAI/uai08_test2.uai | 269 +++++ .../tests/data/UAI/uai08_test2.uai.evid | 13 + .../tests/data/UAI/uai08_test2.uai.output | 3 + gtsam/discrete/tests/data/UAI/uai08_test3.uai | 94 ++ .../tests/data/UAI/uai08_test3.uai.evid | 1 + .../tests/data/UAI/uai08_test3.uai.output | 3 + .../tests/testAlgebraicDecisionTree.cpp | 524 +++++++++ gtsam/discrete/tests/testCSP.cpp | 224 ++++ gtsam/discrete/tests/testDecisionTree.cpp | 226 ++++ .../discrete/tests/testDecisionTreeFactor.cpp | 96 ++ gtsam/discrete/tests/testDiscreteBayesNet.cpp | 128 +++ .../tests/testDiscreteConditional.cpp | 86 ++ gtsam/discrete/tests/testDiscreteFactor.cpp | 23 + .../tests/testDiscreteFactorGraph.cpp | 359 +++++++ gtsam/discrete/tests/testPotentialTable.cpp | 153 +++ gtsam/discrete/tests/testScheduler.cpp | 183 ++++ gtsam/discrete/tests/testSignature.cpp | 61 ++ gtsam/discrete/tests/testSudoku.cpp | 215 ++++ .../tests/testTypedDiscreteFactor.cpp | 54 + .../tests/testTypedDiscreteFactorGraph.cpp | 200 ++++ .../tests/testTypedDiscreteVariable.cpp | 69 ++ 80 files changed, 10447 insertions(+) create mode 100644 gtsam/discrete/AlgebraicDecisionTree.h create mode 100644 gtsam/discrete/AllDiff.cpp create mode 100644 gtsam/discrete/AllDiff.h create mode 100644 gtsam/discrete/Assignment.h create mode 100644 gtsam/discrete/BinaryAllDiff.h create mode 100644 gtsam/discrete/CMakeLists.txt create mode 100644 gtsam/discrete/CSP.cpp create mode 100644 gtsam/discrete/CSP.h create mode 100644 gtsam/discrete/DecisionTree-inl.h create mode 100644 gtsam/discrete/DecisionTree.h create mode 100644 gtsam/discrete/DecisionTreeFactor.cpp create mode 100644 gtsam/discrete/DecisionTreeFactor.h create mode 100644 gtsam/discrete/DiscreteBayesNet.cpp create mode 100644 gtsam/discrete/DiscreteBayesNet.h create mode 100644 gtsam/discrete/DiscreteConditional.cpp create mode 100644 gtsam/discrete/DiscreteConditional.h create mode 100644 gtsam/discrete/DiscreteFactor.cpp create mode 100644 gtsam/discrete/DiscreteFactor.h create mode 100644 gtsam/discrete/DiscreteFactorGraph.cpp create mode 100644 gtsam/discrete/DiscreteFactorGraph.h create mode 100644 gtsam/discrete/DiscreteKey.cpp create mode 100644 gtsam/discrete/DiscreteKey.h create mode 100644 gtsam/discrete/DiscreteSequentialSolver.cpp create mode 100644 gtsam/discrete/DiscreteSequentialSolver.h create mode 100644 gtsam/discrete/Domain.cpp create mode 100644 gtsam/discrete/Domain.h create mode 100644 gtsam/discrete/PotentialTable.cpp create mode 100644 gtsam/discrete/PotentialTable.h create mode 100644 gtsam/discrete/Potentials.cpp create mode 100644 gtsam/discrete/Potentials.h create mode 100644 gtsam/discrete/RefCounted.cpp create mode 100644 gtsam/discrete/RefCounted.h create mode 100644 gtsam/discrete/Scheduler.cpp create mode 100644 gtsam/discrete/Scheduler.h create mode 100644 gtsam/discrete/Signature.cpp create mode 100644 gtsam/discrete/Signature.h create mode 100644 gtsam/discrete/SingleValue.cpp create mode 100644 gtsam/discrete/SingleValue.h create mode 100644 gtsam/discrete/TypedDiscreteFactor.cpp create mode 100644 gtsam/discrete/TypedDiscreteFactor.h create mode 100644 gtsam/discrete/TypedDiscreteFactorGraph.cpp create mode 100644 gtsam/discrete/TypedDiscreteFactorGraph.h create mode 100644 gtsam/discrete/examples/Doodle.csv create mode 100644 gtsam/discrete/examples/Doodle.xls create mode 100644 gtsam/discrete/examples/Doodle2012.csv create mode 100644 gtsam/discrete/examples/Doodle2012.xls create mode 100644 gtsam/discrete/examples/intrusive.xlsx create mode 100644 gtsam/discrete/examples/schedulingExample.cpp create mode 100644 gtsam/discrete/examples/schedulingQuals12.cpp create mode 100644 gtsam/discrete/examples/small.csv create mode 100644 gtsam/discrete/label_traits.h create mode 100644 gtsam/discrete/parseUAI.cpp create mode 100644 gtsam/discrete/parseUAI.h create mode 100644 gtsam/discrete/tests/data/FG/alarm.fg create mode 100644 gtsam/discrete/tests/data/UAI/sampleMARKOV.uai create mode 100644 gtsam/discrete/tests/data/UAI/sampleMARKOV.uai.evid create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test1.uai create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test1.uai.evid create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test1.uai.output create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test2.uai create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test2.uai.evid create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test2.uai.output create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test3.uai create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test3.uai.evid create mode 100644 gtsam/discrete/tests/data/UAI/uai08_test3.uai.output create mode 100644 gtsam/discrete/tests/testAlgebraicDecisionTree.cpp create mode 100644 gtsam/discrete/tests/testCSP.cpp create mode 100644 gtsam/discrete/tests/testDecisionTree.cpp create mode 100644 gtsam/discrete/tests/testDecisionTreeFactor.cpp create mode 100644 gtsam/discrete/tests/testDiscreteBayesNet.cpp create mode 100644 gtsam/discrete/tests/testDiscreteConditional.cpp create mode 100644 gtsam/discrete/tests/testDiscreteFactor.cpp create mode 100644 gtsam/discrete/tests/testDiscreteFactorGraph.cpp create mode 100644 gtsam/discrete/tests/testPotentialTable.cpp create mode 100644 gtsam/discrete/tests/testScheduler.cpp create mode 100644 gtsam/discrete/tests/testSignature.cpp create mode 100644 gtsam/discrete/tests/testSudoku.cpp create mode 100644 gtsam/discrete/tests/testTypedDiscreteFactor.cpp create mode 100644 gtsam/discrete/tests/testTypedDiscreteFactorGraph.cpp create mode 100644 gtsam/discrete/tests/testTypedDiscreteVariable.cpp diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h new file mode 100644 index 000000000..7c182f387 --- /dev/null +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -0,0 +1,134 @@ +/* + * @file AlgebraicDecisionTree.h + * @brief Algebraic Decision Trees + * @author Frank Dellaert + * @date Mar 14, 2011 + */ + +#pragma once + +#include + +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 { + + public: + + 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; + } + }; + + AlgebraicDecisionTree() : + Super(1.0) { + } + + 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 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 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()); + } + + /** 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); + } + + /** 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); + } + + /** 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 typename Super::LabelC& labelC) const { + return this->combine(labelC, &Ring::add); + } + + }; +// AlgebraicDecisionTree + +} +// namespace gtsam diff --git a/gtsam/discrete/AllDiff.cpp b/gtsam/discrete/AllDiff.cpp new file mode 100644 index 000000000..b06dbae14 --- /dev/null +++ b/gtsam/discrete/AllDiff.cpp @@ -0,0 +1,110 @@ +/* + * AllDiff.cpp + * @brief General "all-different" constraint + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + AllDiff::AllDiff(const DiscreteKeys& dkeys) : + DiscreteFactor(dkeys.indices()) { + BOOST_FOREACH(const DiscreteKey& dkey, dkeys) + cardinalities_.insert(dkey); + } + + /* ************************************************************************* */ + void AllDiff::print(const std::string& s) const { + std::cout << s << ": AllDiff on "; + BOOST_FOREACH (Index dkey, keys_) + std::cout << dkey << " "; + std::cout << std::endl; + } + + /* ************************************************************************* */ + double AllDiff::operator()(const Values& values) const { + std::set < size_t > taken; // record values taken by keys + BOOST_FOREACH(Index dkey, keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0;// check if value alreday taken + taken.insert(value);// if not, record it as taken and keep checking + } + return 1.0; + } + + /* ************************************************************************* */ + AllDiff::operator DecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); + converted = converted * binary12; + } + return converted; + } + + /* ************************************************************************* */ + DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return DecisionTreeFactor(*this) * f; + } + + /* ************************************************************************* */ + bool AllDiff::ensureArcConsistency(size_t j, std::vector& domains) const { + // Though strictly not part of allDiff, we check for + // a value in domains[j] that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + Domain& Dj = domains[j]; + if (Dj.checkAllDiff(keys_, domains)) return true; + + // Check all other domains for singletons and erase corresponding values + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + BOOST_FOREACH(Index k, keys_) + if (k != j) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; + } + } + } + return changed; + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + BOOST_FOREACH(Index k, keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); + } + return boost::make_shared(newKeys); + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr AllDiff::partiallyApply( + const std::vector& domains) const { + DiscreteFactor::Values known; + BOOST_FOREACH(Index k, keys_) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) + known[k] = Dk.firstValue(); + } + return partiallyApply(known); + } + + /* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/discrete/AllDiff.h b/gtsam/discrete/AllDiff.h new file mode 100644 index 000000000..57ed3f9d3 --- /dev/null +++ b/gtsam/discrete/AllDiff.h @@ -0,0 +1,64 @@ +/* + * AllDiff.h + * @brief General "all-different" constraint + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * General AllDiff constraint + * Returns 1 if values for all keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Index and an Index. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ + class AllDiff: public DiscreteFactor { + + std::map cardinalities_; + + DiscreteKey discreteKey(size_t i) const { + Index j = keys_[i]; + return DiscreteKey(j,cardinalities_.at(j)); + } + + public: + + /// Constructor + AllDiff(const DiscreteKeys& dkeys); + + // print + virtual void print(const std::string& s = "") const; + + /// Calculate value = expensive ! + virtual double operator()(const Values& values) const; + + /// Convert into a decisiontree, can be *very* expensive ! + virtual operator DecisionTreeFactor() const; + + /// Multiply into a decisiontree + virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + + /* + * Ensure Arc-consistency + * Arc-consistency involves creating binaryAllDiff constraints + * In which case the combinatorial hyper-arc explosion disappears. + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, std::vector& domains) const; + + /// Partially apply known values + virtual DiscreteFactor::shared_ptr partiallyApply(const Values&) const; + + /// Partially apply known values, domain version + virtual DiscreteFactor::shared_ptr partiallyApply(const std::vector&) const; + }; + +} // namespace gtsam diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h new file mode 100644 index 000000000..0150f6ff9 --- /dev/null +++ b/gtsam/discrete/Assignment.h @@ -0,0 +1,36 @@ +/* + * @file Assignment.h + * @brief An assignment from labels to a discrete value index (size_t) + * @author Frank Dellaert + * @date Feb 5, 2012 + */ + +#pragma once + +#include +#include +#include + +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; + } + + bool equals(const Assignment& other, double tol = 1e-9) const { + return (*this == other); + } + }; + +} // namespace gtsam diff --git a/gtsam/discrete/BinaryAllDiff.h b/gtsam/discrete/BinaryAllDiff.h new file mode 100644 index 000000000..a97adb2e3 --- /dev/null +++ b/gtsam/discrete/BinaryAllDiff.h @@ -0,0 +1,87 @@ +/* + * BinaryAllDiff.h + * @brief Binary "all-different" constraint + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + + /** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Index and an Index. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ + class BinaryAllDiff: public DiscreteFactor { + + size_t cardinality0_, cardinality1_; /// cardinality + + public: + + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : + DiscreteFactor(key1.first, key2.first), + cardinality0_(key1.second), cardinality1_(key2.second) { + } + + // print + virtual void print(const std::string& s = "") const { + std::cout << s << ": BinaryAllDiff on " << keys_[0] << " and " << keys_[1] + << std::endl; + } + + /// Calculate value + virtual double operator()(const Values& values) const { + return (double) (values.at(keys_[0]) != values.at(keys_[1])); + } + + /// Convert into a decisiontree + virtual operator DecisionTreeFactor() const { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0],cardinality0_)); + keys.push_back(DiscreteKey(keys_[1],cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) + table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /// Multiply into a decisiontree + virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return DecisionTreeFactor(*this) * f; + } + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + /// + bool ensureArcConsistency(size_t j, std::vector& domains) const { +// throw std::runtime_error( +// "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + virtual DiscreteFactor::shared_ptr partiallyApply(const Values&) const { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + virtual DiscreteFactor::shared_ptr partiallyApply( + const std::vector&) const { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + }; + +} // namespace gtsam diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt new file mode 100644 index 000000000..42917e59b --- /dev/null +++ b/gtsam/discrete/CMakeLists.txt @@ -0,0 +1,35 @@ +# Install headers +set(subdir discrete) +file(GLOB discrete_headers "*.h") +# FIXME: exclude headers +install(FILES ${discrete_headers} DESTINATION include/gtsam2/discrete) + +# Set up library dependencies +set (discrete_local_libs + discrete) + +set (discrete_full_libs + gtsam2-static) + +# Exclude tests that don't work +set (discrete_excluded_tests +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactorGraph.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testPotentialTable.cpp") + +# Add all tests +gtsam_add_subdir_tests(discrete "${discrete_local_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}") + +# add examples +foreach(example schedulingExample schedulingQuals12) + add_executable(${example} "examples/${example}.cpp") + add_dependencies(${example} gtsam2-static) + target_link_libraries(${example} ${Boost_LIBRARIES} gtsam2-static) + add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN}) +endforeach(example) + +# Build timing scripts +#if (GTSAM_BUILD_TIMING) +# gtsam_add_timing(discrete "${discrete_local_libs}") +#endif(GTSAM_BUILD_TIMING) + diff --git a/gtsam/discrete/CSP.cpp b/gtsam/discrete/CSP.cpp new file mode 100644 index 000000000..903daccf4 --- /dev/null +++ b/gtsam/discrete/CSP.cpp @@ -0,0 +1,94 @@ +/* + * CSP.cpp + * @brief Constraint Satisfaction Problem class + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + + /// Find the best total assignment - can be expensive + CSP::sharedValues CSP::optimalAssignment() const { + DiscreteSequentialSolver solver(*this); + DiscreteBayesNet::shared_ptr chordal = solver.eliminate(); + sharedValues mpe = optimize(*chordal); + return mpe; + } + + void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const { + // Create VariableIndex + VariableIndex index(*this); + // index.print(); + + size_t n = index.size(); + + // Initialize domains + std::vector < Domain > domains; + for (size_t j = 0; j < n; j++) + domains.push_back(Domain(DiscreteKey(j,cardinality))); + + // Create array of flags indicating a domain changed or not + std::vector changed(n); + + // iterate nrIterations over entire grid + for (size_t it = 0; it < nrIterations; it++) { + bool anyChange = false; + // iterate over all cells + for (size_t v = 0; v < n; v++) { + // keep track of which domains changed + changed[v] = false; + // loop over all factors/constraints for variable v + const VariableIndex::Factors& factors = index[v]; + BOOST_FOREACH(size_t f,factors) { + // if not already a singleton + if (!domains[v].isSingleton()) { + // get the constraint and call its ensureArcConsistency method + DiscreteFactor::shared_ptr factor = (*this)[f]; + changed[v] = factor->ensureArcConsistency(v,domains) || changed[v]; + } + } // f + if (changed[v]) anyChange = true; + } // v + if (!anyChange) break; + // TODO: Sudoku specific hack + if (print) { + if (cardinality == 9 && n == 81) { + for (size_t i = 0, v = 0; i < sqrt(n); i++) { + for (size_t j = 0; j < sqrt(n); j++, v++) { + if (changed[v]) cout << "*"; + domains[v].print(); + cout << "\t"; + } // i + cout << endl; + } // j + } else { + for (size_t v = 0; v < n; v++) { + if (changed[v]) cout << "*"; + domains[v].print(); + cout << "\t"; + } // v + } + cout << endl; + } // print + } // it + +#ifndef INPROGRESS + // Now create new problem with all singleton variables removed + // We do this by adding simplifying all factors using parial application + // TODO: create a new ordering as we go, to ensure a connected graph + // KeyOrdering ordering; + // vector dkeys; + BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors_) { + DiscreteFactor::shared_ptr reduced = factor->partiallyApply(domains); + if (print) reduced->print(); + } +#endif + } +} // gtsam + diff --git a/gtsam/discrete/CSP.h b/gtsam/discrete/CSP.h new file mode 100644 index 000000000..973d104fa --- /dev/null +++ b/gtsam/discrete/CSP.h @@ -0,0 +1,71 @@ +/* + * CSP.h + * @brief Constraint Satisfaction Problem class + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Constraint Satisfaction Problem class + * A specialization of a DiscreteFactorGraph. + * It knows about CSP-specific constraints and algorithms + */ + class CSP: public DiscreteFactorGraph { + + public: + /// Constructor + CSP() { + } + + /// Add a unary constraint, allowing only a single value + void addSingleValue(const DiscreteKey& dkey, size_t value) { + boost::shared_ptr factor(new SingleValue(dkey, value)); + push_back(factor); + } + + /// Add a binary AllDiff constraint + void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { + boost::shared_ptr factor( + new BinaryAllDiff(key1, key2)); + push_back(factor); + } + + /// Add a general AllDiff constraint + void addAllDiff(const DiscreteKeys& dkeys) { + boost::shared_ptr factor(new AllDiff(dkeys)); + push_back(factor); + } + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment() const; + + /* + * Perform loopy belief propagation + * True belief propagation would check for each value in domain + * whether any satisfying separator assignment can be found. + * This corresponds to hyper-arc consistency in CSP speak. + * This can be done by creating a mini-factor graph and search. + * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep. + * It will be very expensive to exclude values that way. + */ + // void applyBeliefPropagation(size_t nrIterations = 10) const; + /* + * Apply arc-consistency ~ Approximate loopy belief propagation + * We need to give the domains to a constraint, and it returns + * a domain whose values don't conflict in the arc-consistency way. + * TODO: should get cardinality from Indices + */ + void runArcConsistency(size_t cardinality, size_t nrIterations = 10, + bool print = false) const; + }; + +} // gtsam + diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h new file mode 100644 index 000000000..6a6519d83 --- /dev/null +++ b/gtsam/discrete/DecisionTree-inl.h @@ -0,0 +1,667 @@ +/* + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + + using namespace boost::assign; + + /*********************************************************************************/ + // Node + /*********************************************************************************/ +#ifdef DT_DEBUG_MEMORY + template + int DecisionTree::Node::nrNodes = 0; +#endif + + /*********************************************************************************/ + // Leaf + /*********************************************************************************/ + template + class DecisionTree::Leaf: public DecisionTree::Node { + + /** constant stored in this leaf */ + Y constant_; + + public: + + /** Constructor from constant */ + Leaf(const Y& constant) : + constant_(constant) {} + + /** return the constant */ + const Y& constant() const { + return 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)); + } + + /** equality up to tolerance */ + bool equals(const Node& q, double tol) const { + const Leaf* other = dynamic_cast (&q); + if (!other) return false; + return fabs(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; + } + + /** 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_; + } + + /** 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); + } + + // 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 + } + + /** 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; } + + }; // Leaf + + /*********************************************************************************/ + // Choice + /*********************************************************************************/ + template + class DecisionTree::Choice: public DecisionTree::Node { + + /** the label of the variable on which we split */ + L label_; + + /** The children of this Choice node. */ + std::vector branches_; + + private: + /** incremental allSame */ + size_t allSame_; + + typedef boost::shared_ptr ChoicePtr; + + public: + + virtual ~Choice() { +#ifdef DT_DEBUG_MEMORY + 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) { +#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 +#endif + return f; + } + + 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); + } + + /** + * 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)); + } + } + + const L& label() const { + return label_; + } + + size_t nrChoices() const { + return branches_.size(); + } + + 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); + } + + /** 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()); + } + + /** 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; + } + + 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; + } + + /// 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; + } + + /** 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"); + } +#endif + 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) { + + 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 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 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); + } + + /** 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 + + /*********************************************************************************/ + // DecisionTree + /*********************************************************************************/ + template + DecisionTree::DecisionTree() { + } + + template + DecisionTree::DecisionTree(const NodePtr& root) : + root_(root) { + } + + /*********************************************************************************/ + 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 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::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)); + + // 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 + 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); + } + + /*********************************************************************************/ + // 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()); + } + } + + // 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); + } + + /*********************************************************************************/ + // "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; + + // 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); + } + + /*********************************************************************************/ + 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; + + // 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"); + + // 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); + } + + /*********************************************************************************/ + 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 + 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 + 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 + NodePtr h = root_->apply_f_op_g(*g.root_, op); + // create a new class with the resulting root "h" + 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; + } + + /*********************************************************************************/ + 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()); + } + +/*********************************************************************************/ + +} // namespace gtsam + + diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h new file mode 100644 index 000000000..75263475d --- /dev/null +++ b/gtsam/discrete/DecisionTree.h @@ -0,0 +1,218 @@ +/* + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * Algebraic Decision Trees + * L = label for variables + * Y = function range (any algebra), e.g., bool, int, double + */ + template + class DecisionTree { + + public: + + /** 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; + + /** 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; + +#ifdef DT_DEBUG_MEMORY + static int nrNodes; +#endif + + // Constructor + Node() { +#ifdef DT_DEBUG_MEMORY + std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); + +#endif + } + + // Destructor + virtual ~Node() { +#ifdef DT_DEBUG_MEMORY + std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); + +#endif + } + + // 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 --------------------------- */ + + public: + + /** 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_; + + protected: + + /** 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); + + /** Default constructor */ + DecisionTree(); + + public: + + /// @name Standard Constructors + /// @{ + + /** 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); + + /** 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::string& table); + + /** 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); + + /** Convert from a different type */ + template + DecisionTree(const DecisionTree& other, + const std::map& map, boost::function op); + + /// @} + /// @name Testable + /// @{ + + /** GTSAM-style print */ + void print(const std::string& s = "DecisionTree") const; + + // Testable + bool equals(const DecisionTree& other, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** Make virtual */ + virtual ~DecisionTree() { + } + + /** equality */ + bool operator==(const DecisionTree& q) const; + + /** evaluate */ + const Y& operator()(const Assignment& x) 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; + + /** 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 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, open a file */ + void dot(const std::string& name, bool showZero = true) const; + + /// @name Advanced Interface + /// @{ + + // internal use only + DecisionTree(const NodePtr& root); + + // internal use only + template NodePtr + compose(Iterator begin, Iterator end, const L& label) const; + + /// @} + + }; // DecisionTree + + /** 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 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 new file mode 100644 index 000000000..f883a6026 --- /dev/null +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -0,0 +1,91 @@ +/* + * DecisionTreeFactor.cpp + * @brief: discrete factor + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor() { + } + + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, + const ADT& potentials) : + DiscreteFactor(keys.indices()), Potentials(keys, potentials) { + } + + /* *************************************************************************/ + 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); + } + + /* ************************************************************************* */ + void DecisionTreeFactor::print(const string& s) const { + cout << s << ":\n"; + IndexFactor::print("IndexFactor:"); + Potentials::print("Potentials:"); + } + + /* ************************************************************************* */ + 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 { + + 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); + } + + // 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))); + } + shared_ptr f(new DecisionTreeFactor(dkeys, result)); + return f; + } + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h new file mode 100644 index 000000000..b68730e34 --- /dev/null +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -0,0 +1,148 @@ +/* + * DecisionTreeFactor.h + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace gtsam { + + class DiscreteConditional; + + /** + * A discrete probabilistic factor + */ + class DecisionTreeFactor: public DiscreteFactor, public Potentials { + + public: + + // typedefs needed to play nice with gtsam + typedef DecisionTreeFactor This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; + + /// Index label and cardinality + typedef std::pair IndexC; + + public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor for I/O */ + DecisionTreeFactor(); + + /** 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) { + } + + /** Construct from a DiscreteConditional type */ + DecisionTreeFactor(const DiscreteConditional& c); + + /// @} + /// @name Testable + /// @{ + + /// equality + bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; + + // print + void print(const std::string& s = "DecisionTreeFactor: ") const; + + /// @} + /// @name Standard Interface + /// @{ + + /// 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); + } + + /// divide by factor f (safely) + DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { + return apply(f, safe_div); + } + + /// Convert into a decisiontree + virtual operator DecisionTreeFactor() 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 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 + /// @{ + + /** + * 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; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + /// + bool ensureArcConsistency(size_t j, std::vector& domains) const { +// throw std::runtime_error( +// "DecisionTreeFactor::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + virtual DiscreteFactor::shared_ptr partiallyApply(const Values&) const { + throw std::runtime_error("DecisionTreeFactor::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + virtual DiscreteFactor::shared_ptr partiallyApply( + const std::vector&) const { + throw std::runtime_error("DecisionTreeFactor::partiallyApply not implemented"); + } + /// @} + }; +// DecisionTreeFactor + +}// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp new file mode 100644 index 000000000..e233743eb --- /dev/null +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -0,0 +1,48 @@ +/* + * DiscreteBayesNet.cpp + * + * @date Feb 15, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtsam { + + // 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(DiscreteBayesNet& bayesNet, const Signature& s) { + bayesNet.push_back(boost::make_shared(s)); + } + + /* ************************************************************************* */ + 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; + } + +/* ************************************************************************* */ +} // namespace diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h new file mode 100644 index 000000000..c0bba84eb --- /dev/null +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -0,0 +1,33 @@ +/* + * DiscreteBayesNet.h + * + * @date Feb 15, 2011 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + + typedef BayesNet DiscreteBayesNet; + + /** 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); + + /** Optimize function for back-substitution. */ + DiscreteFactor::sharedValues optimize(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 new file mode 100644 index 000000000..00e41ce63 --- /dev/null +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -0,0 +1,152 @@ +/* + * DiscreteConditional.cpp + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +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 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; + } + + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const Signature& signature) : + IndexConditional(signature.indices(), 1), Potentials( + signature.discreteKeysParentsFirst(), signature.cpt()) { + } + + /* ******************************************************************************** */ + 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& e) { + throw runtime_error( + "DiscreteConditional::choose: parent value missing"); + }; + return pFS; + } + + /* ******************************************************************************** */ + void DiscreteConditional::solveInPlace(Values& values) const { + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + size_t mpe = solve(values); // Solve for variable + values[j] = mpe; // 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 { + + // 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; + } + + /* ******************************************************************************** */ + size_t DiscreteConditional::sample(const Values& parentsValues) const { + + using boost::uniform_real; + static boost::mt19937 gen(2); // random number generator + + bool debug = ISDEBUG("DiscreteConditional::sample"); + + // 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; + } + + // 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 0; + } + +/* ******************************************************************************** */ + +} // namespace diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h new file mode 100644 index 000000000..5a7881dd9 --- /dev/null +++ b/gtsam/discrete/DiscreteConditional.h @@ -0,0 +1,110 @@ +/* + * DiscreteConditional.h + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * 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; + + /** A map from keys to values */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; + + /// @name Standard Constructors + /// @{ + + /** default constructor needed for serialization */ + DiscreteConditional() { + } + + /** constructor from factor */ + DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); + + /** 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); + + /// @} + /// @name Testable + /// @{ + + /** GTSAM-style print */ + void print(const std::string& s = "Discrete Conditional: ") const { + std::cout << s << std::endl; + IndexConditional::print(s); + Potentials::print(s); + } + + /** GTSAM-style equals */ + bool equals(const DiscreteConditional& other, double tol = 1e-9) const { + return IndexConditional::equals(other, tol) + && Potentials::equals(other, tol); + } + + /// @} + /// @name Standard Interface + /// @{ + + /** 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; + + /** + * solve a conditional + * @param parentsAssignment Known values of the parents + * @return MPE value of the child (1 frontal variable). + */ + size_t solve(const Values& parentsValues) const; + + /** + * sample + * @param parentsAssignment Known values of the parents + * @return sample from conditional + */ + size_t sample(const Values& parentsValues) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /// solve a conditional, in place + void solveInPlace(Values& parentsValues) const; + + /// sample in place, stores result in partial solution + void sampleInPlace(Values& parentsValues) const; + + /// @} + + }; +// DiscreteConditional + +}// gtsam + diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp new file mode 100644 index 000000000..6ed7b70ce --- /dev/null +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -0,0 +1,21 @@ +/* + * DiscreteFactor.cpp + * @brief: discrete factor + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include +#include + +using namespace std; + +namespace gtsam { + + /* ******************************************************************************** */ + DiscreteFactor::DiscreteFactor() { + } + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h new file mode 100644 index 000000000..8b20b3f18 --- /dev/null +++ b/gtsam/discrete/DiscreteFactor.h @@ -0,0 +1,108 @@ +/* + * DiscreteFactor.h + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include + +namespace gtsam { + + class DecisionTreeFactor; + class DiscreteConditional; + class Domain; + + /** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ + class DiscreteFactor: public IndexFactor { + + public: + + // 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; + + protected: + + /// Construct n-way factor + DiscreteFactor(const std::vector& js) : + IndexFactor(js) { + } + + /// Construct unary factor + DiscreteFactor(Index j) : + IndexFactor(j) { + } + + /// Construct binary factor + DiscreteFactor(Index j1, Index j2) : + IndexFactor(j1, j2) { + } + + /// construct from container + template + DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : + IndexFactor(beginKey, endKey) { + } + + public: + + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + DiscreteFactor(); + + /// Virtual destructor + virtual ~DiscreteFactor() {} + + /// @} + /// @name Testable + /// @{ + + // print + virtual void print(const std::string& s = "DiscreteFactor") const { + IndexFactor::print(s); + } + + /// @} + /// @name Standard Interface + /// @{ + + /// 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; + + virtual operator DecisionTreeFactor() const = 0; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + virtual bool ensureArcConsistency(size_t j, std::vector& domains) const = 0; + + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; + + + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const std::vector&) const = 0; + /// @} + }; +// DiscreteFactor + +}// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp new file mode 100644 index 000000000..3c9c42689 --- /dev/null +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -0,0 +1,82 @@ +/* + * DiscreteFactorGraph.cpp + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + */ + +//#define ENABLE_TIMING +#include +#include +#include +#include + +namespace gtsam { + +// Explicitly instantiate so we don't have to include everywhere +template class FactorGraph ; +template class EliminationTree ; + +/* ************************************************************************* */ +DiscreteFactorGraph::DiscreteFactorGraph() { +} + +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +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; + 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"); + + // now divide product/sum to get conditional + tic(3, "divide"); + DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); + toc(3, "divide"); + tictoc_finishedIteration(); + + return make_pair(cond, sum); +} + +/* ************************************************************************* */ +} // namespace + diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h new file mode 100644 index 000000000..8afacc51b --- /dev/null +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -0,0 +1,87 @@ +/* + * DiscreteFactorGraph.h + * + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include +#include +#include +#include + +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; + + /** Construct empty factor graph */ + DiscreteFactorGraph(); + + /** 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); + + 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)); + } + + /** 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 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; + + /// print + void print(const std::string& s = "DiscreteFactorGraph") const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str()); + } + } + +}; +// DiscreteFactorGraph + +/** Main elimination function for DiscreteFactorGraph */ +std::pair, DecisionTreeFactor::shared_ptr> +EliminateDiscrete(const FactorGraph& factors, + size_t nrFrontals = 1); + +} // namespace gtsam diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp new file mode 100644 index 000000000..9ab4207bb --- /dev/null +++ b/gtsam/discrete/DiscreteKey.cpp @@ -0,0 +1,57 @@ +/* + * DiscreteKey.h + * @brief specialized key for discrete variables + * @author Frank Dellaert + * @date Feb 28, 2011 + */ + +#include +#include // for key names +#include // FOREACH +#include "DiscreteKey.h" + +namespace gtsam { + + using namespace std; + + bool OldDiscreteKey::equals(const OldDiscreteKey& other, double tol) const { + return (*this == other); + } + + void OldDiscreteKey::print(const string& s) const { + cout << s << *this; + } + + ostream& operator <<(ostream &os, const OldDiscreteKey &key) { + os << key.name_; + return os; + } + + 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; + } + + 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; + } + +} diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h new file mode 100644 index 000000000..23deaf9d8 --- /dev/null +++ b/gtsam/discrete/DiscreteKey.h @@ -0,0 +1,133 @@ +/* + * DiscreteKey.h + * @brief specialized key for discrete variables + * @author Frank Dellaert + * @date Feb 28, 2011 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + + typedef std::pair DiscreteKey; + + /** + * Key type for discrete conditionals + * Includes name and cardinality + */ + class OldDiscreteKey : std::pair { + + private: + + std::string name_; + + public: + + /** Default constructor */ + OldDiscreteKey() : + std::pair(0,0), name_("default") { + } + + /** Constructor, defaults to binary */ + OldDiscreteKey(Index j, const std::string& name, size_t cardinality = 2) : + std::pair(j,cardinality), name_(name) { + } + + virtual ~OldDiscreteKey() { + } + + // Testable + bool equals(const OldDiscreteKey& other, double tol = 1e-9) const; + void print(const std::string& s = "") const; + + operator Index() const { return first; } + + const std::string& name() const { + return name_; + } + + size_t cardinality() const { + return second; + } + + /** compare 2 keys by their name */ + bool operator <(const OldDiscreteKey& other) const { + return name_ < other.name_; + } + + /** equality */ + bool operator==(const OldDiscreteKey& other) const { + return (first == other.first) && (second == other.second) && (name_ == other.name_); + } + + bool operator!=(const OldDiscreteKey& other) const { + return !(*this == other); + } + + /** provide streaming */ + friend std::ostream& operator <<(std::ostream &os, const OldDiscreteKey &key); + + }; // OldDiscreteKey + + /// DiscreteKeys is a set of keys that can be assembled using the & operator + struct DiscreteKeys: public std::vector { + + /// Default constructor + DiscreteKeys() { + } + + /// 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 cardinalities with default names + DiscreteKeys(const std::vector& cs); + + /// Return a vector of indices + std::vector indices() 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 + + /// Create a list from two keys + DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); + + /// traits class for DiscreteKey for use with DecisionTree/DecisionDiagram + template<> + struct label_traits { + /** get cardinality from type */ + static size_t cardinality(const OldDiscreteKey& key) { + return key.cardinality(); + } + /** compare 2 keys by their name */ + static bool higher(const OldDiscreteKey& a, const OldDiscreteKey& b) { + return a.name() < b.name(); + } + /** hash function */ + static size_t hash_value(const OldDiscreteKey& a) { + boost::hash hasher; + return hasher(a.name()); + } + }; + +} diff --git a/gtsam/discrete/DiscreteSequentialSolver.cpp b/gtsam/discrete/DiscreteSequentialSolver.cpp new file mode 100644 index 000000000..86cea7ffb --- /dev/null +++ b/gtsam/discrete/DiscreteSequentialSolver.cpp @@ -0,0 +1,47 @@ +/* + * DiscreteSequentialSolver.cpp + * + * @date Feb 16, 2011 + * @author Duy-Nguyen Ta + */ + +//#define ENABLE_TIMING +#include +#include +#include +#include + +namespace gtsam { + + template class GenericSequentialSolver ; + + /* ************************************************************************* */ + DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { + + static const bool debug = false; + + 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"); + + if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); + + // Allocate the solution vector if it is not already allocated + + // Back-substitute + tic(2, "optimize"); + DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); + toc(2, "optimize"); + + if (debug) solution->print("DiscreteSequentialSolver, solution "); + + return solution; + } +/* ************************************************************************* */ + +} diff --git a/gtsam/discrete/DiscreteSequentialSolver.h b/gtsam/discrete/DiscreteSequentialSolver.h new file mode 100644 index 000000000..0be149646 --- /dev/null +++ b/gtsam/discrete/DiscreteSequentialSolver.h @@ -0,0 +1,97 @@ +/* + * DiscreteSequentialSolver.h + * + * @date Feb 16, 2011 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + // The base class provides all of the needed functionality + + class DiscreteSequentialSolver: public GenericSequentialSolver { + + protected: + typedef GenericSequentialSolver Base; + typedef boost::shared_ptr shared_ptr; + + 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; + + /** + * 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) { + } + + 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); + } + +#ifdef BROKEN + /** + * 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); + } +#endif + + /** + * 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/Domain.cpp b/gtsam/discrete/Domain.cpp new file mode 100644 index 000000000..ff71fa39f --- /dev/null +++ b/gtsam/discrete/Domain.cpp @@ -0,0 +1,95 @@ +/* + * Domain.cpp + * @brief Domain restriction constraint + * @date Feb 13, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtsam { + + using namespace std; + + /* ************************************************************************* */ + void Domain::print(const string& s) const { +// cout << s << ": Domain on " << keys_[0] << " (j=" << keys_[0] +// << ") with values"; +// BOOST_FOREACH (size_t v,values_) cout << " " << v; +// cout << endl; + BOOST_FOREACH (size_t v,values_) cout << v; + } + + /* ************************************************************************* */ + double Domain::operator()(const Values& values) const { + return contains(values.at(keys_[0])); + } + + /* ************************************************************************* */ + Domain::operator DecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0],cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) + table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /* ************************************************************************* */ + DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return DecisionTreeFactor(*this) * f; + } + + /* ************************************************************************* */ + bool Domain::ensureArcConsistency(size_t j, vector& domains) const { + if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains[j]; + BOOST_FOREACH(size_t value, values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; + } + + /* ************************************************************************* */ + bool Domain::checkAllDiff(const vector keys, vector& domains) { + Index j = keys_[0]; + // for all values in this domain + BOOST_FOREACH(size_t value, values_) { + // for all connected domains + BOOST_FOREACH(Index k, keys) + // if any domain contains the value we cannot make this domain singleton + if (k!=j && domains[k].contains(value)) + goto found; + values_.clear(); + values_.insert(value); + return true; // we changed it + found:; + } + return false; // we did not change it + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr Domain::partiallyApply( + const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && !contains(it->second)) throw runtime_error( + "Domain::partiallyApply: unsatisfiable"); + return boost::make_shared < Domain > (*this); + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr Domain::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( + "Domain::partiallyApply: unsatisfiable"); + return boost::make_shared < Domain > (Dk); + } + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/discrete/Domain.h b/gtsam/discrete/Domain.h new file mode 100644 index 000000000..f06e9a1da --- /dev/null +++ b/gtsam/discrete/Domain.h @@ -0,0 +1,107 @@ +/* + * Domain.h + * @brief Domain restriction constraint + * @date Feb 13, 2012 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Domain restriction constraint + */ + class Domain: public DiscreteFactor { + + size_t cardinality_; /// Cardinality + std::set values_; /// allowed values + + public: + + typedef boost::shared_ptr shared_ptr; + + // Constructor on Discrete Key initializes an "all-allowed" domain + Domain(const DiscreteKey& dkey) : + DiscreteFactor(dkey.first), cardinality_(dkey.second) { + for (size_t v = 0; v < cardinality_; v++) + values_.insert(v); + } + + // Constructor on Discrete Key with single allowed value + // Consider SingleValue constraint + Domain(const DiscreteKey& dkey, size_t v) : + DiscreteFactor(dkey.first), cardinality_(dkey.second) { + values_.insert(v); + } + + /// Constructor + Domain(const Domain& other) : + DiscreteFactor(other.keys_[0]), values_(other.values_) { + } + + /// insert a value, non const :-( + void insert(size_t value) { + values_.insert(value); + } + + /// erase a value, non const :-( + void erase(size_t value) { + values_.erase(value); + } + + size_t nrValues() const { + return values_.size(); + } + + bool isSingleton() const { + return nrValues() == 1; + } + + size_t firstValue() const { + return *values_.begin(); + } + + // print + virtual void print(const std::string& s = "") const; + + bool contains(size_t value) const { + return values_.count(value)>0; + } + + /// Calculate value + virtual double operator()(const Values& values) const; + + /// Convert into a decisiontree + virtual operator DecisionTreeFactor() const; + + /// Multiply into a decisiontree + virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, std::vector& domains) const; + + /** + * Check for a value in domain that does not occur in any other connected domain. + * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency + * @param keys connected domains through alldiff + */ + bool checkAllDiff(const std::vector keys, std::vector& domains); + + /// Partially apply known values + virtual DiscreteFactor::shared_ptr partiallyApply( + const Values& values) const; + + /// Partially apply known values, domain version + virtual DiscreteFactor::shared_ptr partiallyApply( + const std::vector& domains) const; + }; + +} // namespace gtsam diff --git a/gtsam/discrete/PotentialTable.cpp b/gtsam/discrete/PotentialTable.cpp new file mode 100644 index 000000000..d0388e052 --- /dev/null +++ b/gtsam/discrete/PotentialTable.cpp @@ -0,0 +1,162 @@ +/* + * Potentials.cpp + * + * @date Feb 21, 2011 + * @author Duy-Nguyen Ta + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + void PotentialTable::Iterator::operator++() { + // note size_t is unsigned and i>=0 is always true, so strange-looking loop: + for (size_t i = size(); i--; ) { + if (++at(i) < cardinalities_[i]) + return; + else + at(i) = 0; + } + } + + /* ************************************************************************* */ + size_t PotentialTable::computeTableSize( + const std::vector& cardinalities) { + size_t tableSize = 1; + BOOST_FOREACH(const size_t c, cardinalities) + tableSize *= c; + return tableSize; + } + + /* ************************************************************************* */ + PotentialTable::PotentialTable(const std::vector& cs) : + cardinalities_(cs), table_(computeTableSize(cs)) { + generateKeyFactors(); + } + + /* ************************************************************************* */ + PotentialTable::PotentialTable(const std::vector& cardinalities, + const Table& table) : cardinalities_(cardinalities),table_(table) { + generateKeyFactors(); + } + + /* ************************************************************************* */ + PotentialTable::PotentialTable(const std::vector& cardinalities, + const std::string& tableString) : cardinalities_(cardinalities) { + parse(tableString); + generateKeyFactors(); + } + + /* ************************************************************************* */ + bool PotentialTable::equals(const PotentialTable& other, double tol) const { + //TODO: compare potentials in a more general sense with arbitrary order of keys??? + if ((cardinalities_ == other.cardinalities_) && (table_.size() + == other.table_.size()) && (keyFactors_ == other.keyFactors_)) { + for (size_t i = 0; i < table_.size(); i++) { + if (fabs(table_[i] - other.table_[i]) > tol) { + return false; + } + return true; + } + } + return false; + } + + /* ************************************************************************* */ + void PotentialTable::print(const std::string& s) const { + cout << s << endl; + for (size_t i = 0; i < cardinalities_.size(); i++) + cout << boost::format("[%d,%d]") % cardinalities_[i] % keyFactors_[i] << " "; + cout << endl; + Iterator assignment(cardinalities_); + for (size_t idx = 0; idx < table_.size(); ++idx, ++assignment) { + for (size_t k = 0; k < assignment.size(); k++) + cout << assignment[k] << "\t\t"; + cout << table_[idx] << endl; + } + } + + /* ************************************************************************* */ + const double& PotentialTable::operator()(const Assignment& var) const { + return table_[tableIndexFromAssignment(var)]; + } + + /* ************************************************************************* */ + const double& PotentialTable::operator[](const size_t index) const { + return table_.at(index); + } + + + /* ************************************************************************* */ + void PotentialTable::setPotential(const PotentialTable::Assignment& asg, const double potential) { + size_t idx = tableIndexFromAssignment(asg); + assert(idx (iss), istream_iterator (), + back_inserter(table_)); + +#ifndef NDEBUG + size_t expectedSize = computeTableSize(cardinalities_); + if (table_.size() != expectedSize) throw invalid_argument( + boost::str( + boost::format( + "String specification \"%s\" for table only contains %d doubles instead of %d") + % tableString % table_.size() % expectedSize)); +#endif + } + +} // namespace diff --git a/gtsam/discrete/PotentialTable.h b/gtsam/discrete/PotentialTable.h new file mode 100644 index 000000000..b7741ba1e --- /dev/null +++ b/gtsam/discrete/PotentialTable.h @@ -0,0 +1,95 @@ +/* + * Potentials.h + * + * @date Feb 21, 2011 + * @author Duy-Nguyen Ta + */ + +#ifndef POTENTIALS_H_ +#define POTENTIALS_H_ + +#include +#include +#include +#include +#include +#include + +namespace gtsam +{ +/** + * PotentialTable holds the real-valued potentials for Factors or Conditionals + */ +class PotentialTable { +public: + typedef std::vector Table; // container type for potentials f(x1,x2,..) + typedef std::vector Cardinalities; // just a typedef + typedef std::vector Assignment; // just a typedef + + /** + * An assignment that can be incemented + */ + struct Iterator: std::vector { + Cardinalities cardinalities_; + Iterator(const Cardinalities& cs):cardinalities_(cs) { + for(size_t i=0;i cardinalities_; // cardinalities of variables + Table table_; // Potential values of all instantiations of the variables, following the variables' order in vector Keys. + std::vector keyFactors_; // factors to multiply a key's assignment with, to access the potential table + + void generateKeyFactors(); + void parse(const std::string& tableString); + +public: + + /** compute table size from variable cardinalities */ + static size_t computeTableSize(const std::vector& cardinalities); + + /** construct an empty potential */ + PotentialTable() {} + + /** Dangerous empty n-ary potential. */ + PotentialTable(const std::vector& cardinalities); + + /** n-ary potential. */ + PotentialTable(const std::vector& cardinalities, + const Table& table); + + /** n-ary potential. */ + PotentialTable(const std::vector& cardinalities, + const std::string& tableString); + + /** return iterator to first element */ + Iterator begin() const { return Iterator(cardinalities_);} + + /** equality */ + bool equals(const PotentialTable& other, double tol = 1e-9) const; + + /** print */ + void print(const std::string& s = "Potential Table: ") const; + + /** return cardinality of a variable */ + size_t cardinality(size_t var) const { return cardinalities_[var]; } + size_t tableSize() const { return table_.size(); } + + /** accessors to potential values in the table given the assignment */ + const double& operator()(const Assignment& var) const; + const double& operator[](const size_t index) const; + + void setPotential(const Assignment& asg, const double potential); + void setPotential(const size_t tableIndex, const double potential); + + /** convert between assignment and where it is in the table */ + size_t tableIndexFromAssignment(const Assignment& var) const; + Assignment assignmentFromTableIndex(const size_t i) const; +}; + + +} // namespace + +#endif /* POTENTIALS_H_ */ diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp new file mode 100644 index 000000000..f2d6760eb --- /dev/null +++ b/gtsam/discrete/Potentials.cpp @@ -0,0 +1,53 @@ +/* + * Potentials.cpp + * @date March 24, 2011 + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + + // 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); + } + + /* ******************************************************************************** */ + Potentials::Potentials() : + ADT(1.0) { + } + + /* ******************************************************************************** */ + 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); + } + + /* ************************************************************************* */ + void Potentials::print(const string&s) const { + cout << s << "\n Cardinalities: "; + BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + cout << key.first << "=" << key.second << " "; + cout << endl; + ADT::print(" "); + } + + /* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h new file mode 100644 index 000000000..cfcf58400 --- /dev/null +++ b/gtsam/discrete/Potentials.h @@ -0,0 +1,62 @@ +/* + * Potentials.h + * @date March 24, 2011 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + + /** + * A base class for both DiscreteFactor and DiscreteConditional + */ + class Potentials: public AlgebraicDecisionTree { + + public: + + typedef AlgebraicDecisionTree ADT; + + protected: + + /// Cardinality for each key, used in combine + std::map cardinalities_; + + /** 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); + + public: + + /** Default constructor for I/O */ + Potentials(); + + /** 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()) { + } + + // Testable + bool equals(const Potentials& other, double tol = 1e-9) const; + void print(const std::string& s = "Potentials: ") const; + + size_t cardinality(Index j) const { return cardinalities_.at(j);} + + }; // Potentials + +} // namespace gtsam diff --git a/gtsam/discrete/RefCounted.cpp b/gtsam/discrete/RefCounted.cpp new file mode 100644 index 000000000..6440b5028 --- /dev/null +++ b/gtsam/discrete/RefCounted.cpp @@ -0,0 +1,9 @@ +/* + * @file RefCounted.cpp + * @brief Simple reference-counted base class + * @author Frank Dellaert + * @date Mar 29, 2011 + */ + +#include + diff --git a/gtsam/discrete/RefCounted.h b/gtsam/discrete/RefCounted.h new file mode 100644 index 000000000..03d086ab6 --- /dev/null +++ b/gtsam/discrete/RefCounted.h @@ -0,0 +1,86 @@ +/* + * @file RefCounted.h + * @brief Simple reference-counted base class + * @author Frank Dellaert + * @date Mar 29, 2011 + */ + +#include + +// Forward Declarations +namespace gtsam { + struct RefCounted; +} + +namespace boost { + void intrusive_ptr_add_ref(const gtsam::RefCounted * p); + void intrusive_ptr_release(const gtsam::RefCounted * p); +} + +namespace gtsam { + + /** + * Simple reference counted class inspired by + * http://www.codeproject.com/KB/stl/boostsmartptr.aspx + */ + struct RefCounted { + private: + mutable long references_; + friend void ::boost::intrusive_ptr_add_ref(const RefCounted * p); + friend void ::boost::intrusive_ptr_release(const RefCounted * p); + public: + RefCounted() : + references_(0) { + } + virtual ~RefCounted() { + } + }; + +} // namespace gtsam + +// Intrusive Pointer free functions +#ifndef DEBUG_REFCOUNT + +namespace boost { + + // increment reference count of object *p + inline void intrusive_ptr_add_ref(const gtsam::RefCounted * p) { + ++(p->references_); + } + + // decrement reference count, and delete object when reference count reaches 0 + inline void intrusive_ptr_release(const gtsam::RefCounted * p) { + if (--(p->references_) == 0) + delete p; + } + +} // namespace boost + +#else + +#include + + namespace gtsam { + static long GlobalRefCount = 0; + } + + namespace boost { + inline void intrusive_ptr_add_ref(const gtsam::RefCounted * p) { + ++(p->references_); + gtsam::GlobalRefCount++; + std::cout << "add_ref " << p << " " << p->references_ << // + " " << gtsam::GlobalRefCount << std::endl; + } + + inline void intrusive_ptr_release(const gtsam::RefCounted * p) { + gtsam::GlobalRefCount--; + std::cout << "release " << p << " " << (p->references_ - 1) << // + " " << gtsam::GlobalRefCount << std::endl; + if (--(p->references_) == 0) + delete p; + } + + } // namespace boost + +#endif + diff --git a/gtsam/discrete/Scheduler.cpp b/gtsam/discrete/Scheduler.cpp new file mode 100644 index 000000000..574e276a9 --- /dev/null +++ b/gtsam/discrete/Scheduler.cpp @@ -0,0 +1,297 @@ +/* + * Scheduler.h + * @brief an example how inference can be used for scheduling qualifiers + * @date Mar 26, 2011 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace gtsam { + + using namespace std; + + Scheduler::Scheduler(size_t maxNrStudents, const string& filename): + maxNrStudents_(maxNrStudents) + { + typedef boost::tokenizer > Tokenizer; + + // open file + ifstream is(filename.c_str()); + + string line; // buffer + + // process first line with faculty + if (getline(is, line, '\r')) { + Tokenizer tok(line); + Tokenizer::iterator it = tok.begin(); + for (++it; it != tok.end(); ++it) + addFaculty(*it); + } + + // for all remaining lines + size_t count = 0; + while (getline(is, line, '\r')) { + if (count++ > 100) throw runtime_error("reached 100 lines, exiting"); + Tokenizer tok(line); + Tokenizer::iterator it = tok.begin(); + addSlot(*it++); // add slot + // add availability + for (; it != tok.end(); ++it) + available_ += (it->empty()) ? "0 " : "1 "; + available_ += '\n'; + } + } // constructor + + /** addStudent has to be called after adding slots and faculty */ + void Scheduler::addStudent(const string& studentName, + const string& area1, const string& area2, + const string& area3, const string& advisor) { + assert(nrStudents() area) const { + return area ? students_[s].keys_[*area] : students_[s].key_; + } + + const string& Scheduler::studentName(size_t i) const { + assert(i slot) { + bool debug = ISDEBUG("Scheduler::buildGraph"); + + assert(iat(j); + cout << studentName(s) << " slot: " << slotName_[slot] << endl; + Index base = 3*s; + for (size_t area = 0; area < 3; area++) { + size_t faculty = assignment->at(base+area); + cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty] + << endl; + } + cout << endl; + } + } + + /** Special print for single-student case */ + void Scheduler::printSpecial(sharedValues assignment) const { + Values::const_iterator it = assignment->begin(); + for (size_t area = 0; area < 3; area++, it++) { + size_t f = it->second; + cout << setw(12) << it->first << ": " << facultyName_[f] << endl; + } + cout << endl; + } + + /** Accumulate faculty stats */ + void Scheduler::accumulateStats(sharedValues assignment, vector< + size_t>& stats) const { + for (size_t s = 0; s < nrStudents(); s++) { + Index base = 3*s; + for (size_t area = 0; area < 3; area++) { + size_t f = assignment->at(base+area); + assert(frbegin(); + const Student & student = students_.front(); + cout << endl; + (*it)->print(student.name_); + } + + tic(3, "my_optimize"); + sharedValues mpe = optimize(*chordal); + toc(3, "my_optimize"); + return mpe; + } + + /** find the assignment of students to slots with most possible committees */ + Scheduler::sharedValues Scheduler::bestSchedule() const { + sharedValues best; + throw runtime_error("bestSchedule not implemented"); + return best; + } + + /** find the corresponding most desirable committee assignment */ + Scheduler::sharedValues Scheduler::bestAssignment( + sharedValues bestSchedule) const { + sharedValues best; + throw runtime_error("bestAssignment not implemented"); + return best; + } + +} // gtsam + + diff --git a/gtsam/discrete/Scheduler.h b/gtsam/discrete/Scheduler.h new file mode 100644 index 000000000..8b91bce61 --- /dev/null +++ b/gtsam/discrete/Scheduler.h @@ -0,0 +1,171 @@ +/* + * Scheduler.h + * @brief an example how inference can be used for scheduling qualifiers + * @date Mar 26, 2011 + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + + /** + * Scheduler class + * Creates one variable for each student, and three variables for each + * of the student's areas, for a total of 4*nrStudents variables. + * The "student" variable will determine when the student takes the qual. + * The "area" variables determine which faculty are on his/her committee. + */ + class Scheduler : public CSP { + + private: + + /** Internal data structure for students */ + struct Student { + std::string name_; + DiscreteKey key_; // key for student + std::vector keys_; // key for areas + std::vector areaName_; + std::vector advisor_; + Student(size_t nrFaculty, size_t advisorIndex) : + keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { + advisor_[advisorIndex] = 0.0; + } + void print() const { + using std::cout; + cout << name_ << ": "; + for (size_t area = 0; area < 3; area++) + cout << areaName_[area] << " "; + cout << std::endl; + } + }; + + /** Maximum number of students */ + size_t maxNrStudents_; + + /** discrete keys, indexed by student and area index */ + std::vector students_; + + /** faculty identifiers */ + std::map facultyIndex_; + std::vector facultyName_, slotName_, areaName_; + + /** area constraints */ + typedef std::map > FacultyInArea; + FacultyInArea facultyInArea_; + + /** nrTimeSlots * nrFaculty availability constraints */ + std::string available_; + + /** which slots are good */ + std::vector slotsAvailable_; + + public: + + /** + * Constructor + * WE need to know the number of students in advance for ordering keys. + * then add faculty, slots, areas, availability, students, in that order + */ + Scheduler(size_t maxNrStudents):maxNrStudents_(maxNrStudents) { + } + + void addFaculty(const std::string& facultyName) { + facultyIndex_[facultyName] = nrFaculty(); + facultyName_.push_back(facultyName); + } + + size_t nrFaculty() const { + return facultyName_.size(); + } + + /** boolean std::string of nrTimeSlots * nrFaculty */ + void setAvailability(const std::string& available) { + available_ = available; + } + + void addSlot(const std::string& slotName) { + slotName_.push_back(slotName); + } + + size_t nrTimeSlots() const { + return slotName_.size(); + } + + const std::string& slotName(size_t s) const { + return slotName_[s]; + } + + /** slots available, boolean */ + void setSlotsAvailable(const std::vector& slotsAvailable) { + slotsAvailable_ = slotsAvailable; + } + + void addArea(const std::string& facultyName, const std::string& areaName) { + areaName_.push_back(areaName); + std::vector& table = facultyInArea_[areaName]; // will create if needed + if (table.empty()) table.resize(nrFaculty(), 0); + table[facultyIndex_[facultyName]] = 1; + } + + /** + * Constructor that reads in faculty, slots, availibility. + * Still need to add areas and students after this + */ + Scheduler(size_t maxNrStudents, const std::string& filename); + + /** get key for student and area, 0 is time slot itself */ + const DiscreteKey& key(size_t s, boost::optional area = boost::none) const; + + /** addStudent has to be called after adding slots and faculty */ + void addStudent(const std::string& studentName, const std::string& area1, + const std::string& area2, const std::string& area3, + const std::string& advisor); + + /// current number of students + size_t nrStudents() const { + return students_.size(); + } + + const std::string& studentName(size_t i) const; + const DiscreteKey& studentKey(size_t i) const; + const std::string& studentArea(size_t i, size_t area) const; + + /** Add student-specific constraints to the graph */ + void addStudentSpecificConstraints(size_t i, boost::optional slot = boost::none); + + /** Main routine that builds factor graph */ + void buildGraph(size_t mutexBound = 7); + + /** print */ + void print(const std::string& s = "Scheduler") const; + + /** Print readable form of assignment */ + void printAssignment(sharedValues assignment) const; + + /** Special print for single-student case */ + void printSpecial(sharedValues assignment) const; + + /** Accumulate faculty stats */ + void accumulateStats(sharedValues assignment, + std::vector& stats) const; + + /** Eliminate, return a Bayes net */ + DiscreteBayesNet::shared_ptr eliminate() const; + + /** Find the best total assignment - can be expensive */ + sharedValues optimalAssignment() const; + + /** find the assignment of students to slots with most possible committees */ + sharedValues bestSchedule() const; + + /** find the corresponding most desirable committee assignment */ + sharedValues bestAssignment(sharedValues bestSchedule) const; + + }; // Scheduler + +} // gtsam + + diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp new file mode 100644 index 000000000..4d808543a --- /dev/null +++ b/gtsam/discrete/Signature.cpp @@ -0,0 +1,217 @@ +/* + * Signature.cpp + * @brief: signatures for conditional densities + * @author: Frank dellaert + * @date Feb 27, 2011 + */ + +#include +#include + +#include "Signature.h" + +#ifdef BOOST_HAVE_PARSER +#include // for parsing +#include // for qi::_val +#endif + +namespace gtsam { + + using namespace std; + + +#ifdef BOOST_HAVE_PARSER + namespace qi = boost::spirit::qi; + + // 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 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; + + // 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")[ref(table) = logic(false, true, true, true)]) || + qi::parse(f, l, + qi::lit("AND")[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(ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ref(values), qi::_1)]) | + qi::lit("T")[ref(values) = T] | + qi::lit("F")[ref(values) = F] ); + if (!r) + return false; + table.push_back(values); + } + + return true; + } + } // \namespace parser +#endif + + 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; + } + + 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; + } + + 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; + } + + 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; + } + + Signature& Signature::operator=(const string& spec) { + spec_.reset(spec); +#ifdef BOOST_HAVE_PARSER + 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); + } +#endif + 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; + } + + 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 Signature::Table& parent) { + Signature s(key); + return s = parent; + } + +} // namespace gtsam diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h new file mode 100644 index 000000000..25e66860e --- /dev/null +++ b/gtsam/discrete/Signature.h @@ -0,0 +1,129 @@ +/* + * Signature.h + * @brief: signatures for conditional densities + * @author: Frank dellaert + * @date Feb 27, 2011 + */ + +#pragma once +#include +#include +#include +#include + +#include // for checking whether we are using boost 1.40 +#if BOOST_VERSION >= 104200 +#define BOOST_HAVE_PARSER +#endif + +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 { + + public: + + /** Data type for the CPT */ + typedef std::vector Row; + typedef std::vector Table; + + private: + + /** the variable key */ + DiscreteKey key_; + + /** the parent keys */ + DiscreteKeys parents_; + + // the given CPT specification string + boost::optional spec_; + + // the CPT as parsed, if successful + boost::optional table_; + + public: + + /** Constructor from DiscreteKey */ + Signature(const DiscreteKey& key); + + /** the variable key */ + const DiscreteKey& key() const { + return key_; + } + + /** the parent keys */ + const DiscreteKeys& parents() const { + return parents_; + } + + /** All keys, with variable key last */ + DiscreteKeys discreteKeysParentsFirst() 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 a vector of doubles, with key's values most rapidly changing + std::vector cpt() const; + + /** 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 directly as a table */ + Signature& operator=(const Table& table); + + /** 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 % "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); + +} diff --git a/gtsam/discrete/SingleValue.cpp b/gtsam/discrete/SingleValue.cpp new file mode 100644 index 000000000..abd21a1a1 --- /dev/null +++ b/gtsam/discrete/SingleValue.cpp @@ -0,0 +1,78 @@ +/* + * SingleValue.cpp + * @brief domain constraint + * @date Feb 13, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + + using namespace std; + + /* ************************************************************************* */ + void SingleValue::print(const string& s) const { + cout << s << ": SingleValue on " << keys_[0] << " (j=" << keys_[0] + << ") with value " << value_ << endl; + } + + /* ************************************************************************* */ + double SingleValue::operator()(const Values& values) const { + return (double) (values.at(keys_[0]) == value_); + } + + /* ************************************************************************* */ + SingleValue::operator DecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0],cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) + table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /* ************************************************************************* */ + DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return DecisionTreeFactor(*this) * f; + } + + /* ************************************************************************* */ + bool SingleValue::ensureArcConsistency(size_t j, + vector& domains) const { + if (j != keys_[0]) throw invalid_argument( + "SingleValue check on wrong domain"); + Domain& D = domains[j]; + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(),value_); + return true; + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) throw runtime_error( + "SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared < SingleValue > (keys_[0], cardinality_, value_); + } + + /* ************************************************************************* */ + DiscreteFactor::shared_ptr SingleValue::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( + "SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared < SingleValue > (discreteKey(), value_); + } + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/discrete/SingleValue.h b/gtsam/discrete/SingleValue.h new file mode 100644 index 000000000..c1757f82d --- /dev/null +++ b/gtsam/discrete/SingleValue.h @@ -0,0 +1,72 @@ +/* + * SingleValue.h + * @brief domain constraint + * @date Feb 6, 2012 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * SingleValue constraint + */ + class SingleValue: public DiscreteFactor { + + /// Number of values + size_t cardinality_; + + /// allowed value + size_t value_; + + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0],cardinality_); + } + + public: + + typedef boost::shared_ptr shared_ptr; + + /// Constructor + SingleValue(Index key, size_t n, size_t value) : + DiscreteFactor(key), cardinality_(n), value_(value) { + } + + /// Constructor + SingleValue(const DiscreteKey& dkey, size_t value) : + DiscreteFactor(dkey.first), cardinality_(dkey.second), value_(value) { + } + + // print + virtual void print(const std::string& s = "") const; + + /// Calculate value + virtual double operator()(const Values& values) const; + + /// Convert into a decisiontree + virtual operator DecisionTreeFactor() const; + + /// Multiply into a decisiontree + virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, std::vector& domains) const; + + /// Partially apply known values + virtual DiscreteFactor::shared_ptr partiallyApply( + const Values& values) const; + + /// Partially apply known values, domain version + virtual DiscreteFactor::shared_ptr partiallyApply( + const std::vector& domains) const; + }; + +} // namespace gtsam diff --git a/gtsam/discrete/TypedDiscreteFactor.cpp b/gtsam/discrete/TypedDiscreteFactor.cpp new file mode 100644 index 000000000..daa745037 --- /dev/null +++ b/gtsam/discrete/TypedDiscreteFactor.cpp @@ -0,0 +1,117 @@ +/* + * @file TypedDiscreteFactor.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Mar 5, 2011 + */ + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ******************************************************************************** */ + TypedDiscreteFactor::TypedDiscreteFactor(const Indices& keys, + const string& table) : + Factor (keys.begin(), keys.end()), potentials_(keys, table) { + } + + /* ******************************************************************************** */ + TypedDiscreteFactor::TypedDiscreteFactor(const Indices& keys, + const vector& table) : + Factor (keys.begin(), keys.end()), potentials_(keys, table) { + //#define DEBUG_FACTORS +#ifdef DEBUG_FACTORS + static size_t count = 0; + string dotfile = (boost::format("Factor-%03d") % ++count).str(); + potentials_.dot(dotfile); + if (count == 57) potentials_.print("57"); +#endif + } + + /* ************************************************************************* */ + double TypedDiscreteFactor::operator()(const Values& values) const { + return potentials_(values); + } + + /* ************************************************************************* */ + void TypedDiscreteFactor::print(const string&s) const { + Factor::print(s); + potentials_.print(); + } + + /* ************************************************************************* */ + bool TypedDiscreteFactor::equals(const TypedDiscreteFactor& other, double tol) const { + return potentials_.equals(other.potentials_, tol); + } + + /* ******************************************************************************** */ + DiscreteFactor::shared_ptr TypedDiscreteFactor::toDiscreteFactor( + const KeyOrdering& ordering) const { + throw std::runtime_error("broken"); + //return boost::make_shared(keys(), ordering, potentials_); + } + +#ifdef OLD +DiscreteFactor TypedDiscreteFactor::toDiscreteFactor( + const KeyOrdering& ordering, const ProblemType problemType) const { + { + static bool debug = false; + + // instantiate vector keys and column index in order + DiscreteFactor::ColumnIndex orderColumnIndex; + vector keys; + BOOST_FOREACH(const KeyOrdering::value_type& ord, ordering) + { + if (debug) cout << "Key: " << ord.first; + + // find the key with ord.first in this factor + vector::const_iterator it = std::find(keys_.begin(), + keys_.end(), ord.first); + + // if found + if (it != keys_.end()) { + if (debug) cout << "it found: " << (*it) << ", index: " + << ord.second << endl; + + keys.push_back(ord.second); // push back the ordering index + orderColumnIndex[ord.second] = columnIndex_.at(ord.first.name()); + + if (debug) cout << "map " << ord.second << " with name: " + << ord.first.name() << " to " << columnIndex_.at( + ord.first.name()) << endl; + } + } + + DiscreteFactor f(keys, potentials_, orderColumnIndex, problemType); + return f; + } + + /* ******************************************************************************** */ + std::vector TypedDiscreteFactor::init(const Indices& keys) { + vector cardinalities; + for (size_t j = 0; j < keys.size(); j++) { + Index key = keys[j]; + keys_.push_back(key); + columnIndex_[key.name()] = j; + cardinalities.push_back(key.cardinality()); + } + return cardinalities; + } + + /* ******************************************************************************** */ + double TypedDiscreteFactor::potential(const TypedValues& values) const { + vector assignment(values.size()); + BOOST_FOREACH(const TypedValues::value_type& val, values) + if (columnIndex_.find(val.first) != columnIndex_.end()) assignment[columnIndex_.at( + val.first)] = val.second; + return potentials_(assignment); + } + +#endif + +} // namespace diff --git a/gtsam/discrete/TypedDiscreteFactor.h b/gtsam/discrete/TypedDiscreteFactor.h new file mode 100644 index 000000000..95f70898b --- /dev/null +++ b/gtsam/discrete/TypedDiscreteFactor.h @@ -0,0 +1,68 @@ +/* + * @file TypedDiscreteFactor.h + * @brief + * @author Duy-Nguyen Ta + * @date Mar 5, 2011 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * A factor on discrete variables with string keys + */ + class TypedDiscreteFactor: public Factor { + + typedef AlgebraicDecisionDiagram ADD; + + /** potentials of the factor */ + ADD potentials_; + + public: + + /** A map from keys to values */ + typedef ADD::Assignment Values; + + /** Constructor from keys and string table */ + TypedDiscreteFactor(const Indices& keys, const std::string& table); + + /** Constructor from keys and doubles */ + TypedDiscreteFactor(const Indices& keys, + const std::vector& table); + + /** Evaluate */ + double operator()(const Values& values) const; + + // Testable + bool equals(const TypedDiscreteFactor& other, double tol = 1e-9) const; + void print(const std::string& s = "DiscreteFactor: ") const; + + DiscreteFactor::shared_ptr toDiscreteFactor(const KeyOrdering& ordering) const; + +#ifdef OLD + /** map each variable name to its column index in the potential table */ + typedef std::map Index2IndexMap; + Index2IndexMap columnIndex_; + + /** Initialize keys, column index, and return cardinalities */ + std::vector init(const Indices& keys); + + public: + + /** Default constructor */ + TypedDiscreteFactor() {} + + /** Evaluate potential of a given assignment of values */ + double potential(const TypedValues& values) const; + +#endif + + }; // TypedDiscreteFactor + +} // namespace diff --git a/gtsam/discrete/TypedDiscreteFactorGraph.cpp b/gtsam/discrete/TypedDiscreteFactorGraph.cpp new file mode 100644 index 000000000..e0e18a885 --- /dev/null +++ b/gtsam/discrete/TypedDiscreteFactorGraph.cpp @@ -0,0 +1,68 @@ +/* + * @file TypedDiscreteFactorGraph.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Mar 1, 2011 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + TypedDiscreteFactorGraph::TypedDiscreteFactorGraph() { + } + + /* ************************************************************************* */ + TypedDiscreteFactorGraph::TypedDiscreteFactorGraph(const string& filename) { + bool success = parseUAI(filename, *this); + if (!success) throw runtime_error( + "TypedDiscreteFactorGraph constructor from filename failed"); + } + + /* ************************************************************************* */ + void TypedDiscreteFactorGraph::add// + (const Indices& keys, const string& table) { + push_back(boost::shared_ptr// + (new TypedDiscreteFactor(keys, table))); + } + + /* ************************************************************************* */ + void TypedDiscreteFactorGraph::add// + (const Indices& keys, const vector& table) { + push_back(boost::shared_ptr// + (new TypedDiscreteFactor(keys, table))); + } + + /* ************************************************************************* */ + void TypedDiscreteFactorGraph::print(const string s) { + cout << s << endl; + cout << "Factors: " << endl; + BOOST_FOREACH(const sharedFactor factor, factors_) + factor->print(); + } + + /* ************************************************************************* */ + double TypedDiscreteFactorGraph::operator()( + const TypedDiscreteFactor::Values& values) const { + // Loop over all factors and multiply their probabilities + double p = 1.0; + BOOST_FOREACH(const sharedFactor& factor, *this) + p *= (*factor)(values); + return p; + } + +/* ************************************************************************* */ + +} diff --git a/gtsam/discrete/TypedDiscreteFactorGraph.h b/gtsam/discrete/TypedDiscreteFactorGraph.h new file mode 100644 index 000000000..93e19e9d2 --- /dev/null +++ b/gtsam/discrete/TypedDiscreteFactorGraph.h @@ -0,0 +1,50 @@ +/* + * @file TypedDiscreteFactorGraph.h + * @brief Factor graph with typed factors (with Index keys) + * @author Duy-Nguyen Ta + * @author Frank Dellaert + * @date Mar 1, 2011 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * Typed discrete factor graph, where keys are strings + */ + class TypedDiscreteFactorGraph: public FactorGraph { + + public: + + /** + * Default constructor + */ + TypedDiscreteFactorGraph(); + + /** + * Constructor from file + * For now assumes in .uai format from UAI'08 Probablistic Inference Evaluation + * See http://graphmod.ics.uci.edu/uai08/FileFormat + */ + TypedDiscreteFactorGraph(const std::string& filename); + + // Add factors without shared pointer ugliness + void add(const Indices& keys, const std::string& table); + void add(const Indices& keys, const std::vector& table); + + /** print */ + void print(const std::string s); + + /** Evaluate potential of a given assignment of values */ + double operator()(const TypedDiscreteFactor::Values& values) const; + + }; // TypedDiscreteFactorGraph + + +} // namespace diff --git a/gtsam/discrete/examples/Doodle.csv b/gtsam/discrete/examples/Doodle.csv new file mode 100644 index 000000000..1ce4ecebb --- /dev/null +++ b/gtsam/discrete/examples/Doodle.csv @@ -0,0 +1 @@ +,Ron Arkin,Andrea Thomaz,Ayanna Howard,Wayne Book,Mike Stilman,Charlie Kemp,Jun Ueda,Patricio Vela,Magnus Egerstedt,Harvey Lipkin,Frank Dellaert,Irfan Essa,Aaron Bobick,Jim Rehg,Henrik Christensen,Tucker Balch,Karen Feigh,N/A 1,N/A 2 Mon 9:00-10.30,,1,1,1,1,,1,1,,,1,,,,1,,,1,1 Mon 10:30-12:00,,1,1,1,1,,,1,1,,,,,,,,,1,1 Mon 1:30-3:00,,,1,,,1,1,1,1,1,1,,,,1,,,1,1 Mon 3:00-4:30,,,,1,,1,1,1,,1,1,1,,1,1,,,1,1 Tue 9:00-10.30,,,1,,,,,1,,1,1,,,1,1,,,1,1 Tue 10:30-12:00,,,1,1,1,,1,1,,1,1,,,1,,1,,1,1 Tue 1:30-3:00,,1,,1,1,,1,1,1,1,1,,,,,1,,1,1 Tue 3:00-4:30,,1,1,,,,,1,1,1,1,,,,1,1,,1,1 Wed 9:00-10.30,,,1,1,,,,,1,,1,,1,,1,,1,1,1 Wed 10:30-12:00,,,,1,1,,1,1,1,,,1,1,,1,1,1,1,1 Wed 1:30-3:00,,,,,1,1,,1,,1,1,1,1,,,1,,1,1 Wed 3:00-4:30,,,,,1,1,1,1,1,,1,1,,1,,1,,1,1 Thu 9:00-10.30,,,1,,,,,1,,1,,,1,1,,,,1,1 Thu 10:30-12:00,,,1,1,1,,1,1,,1,,,,1,,1,,1,1 Thu 1:30-3:00,,,1,1,1,,1,1,1,1,,,,1,,1,1,1,1 Thu 3:00-4:30,,,1,,,,,,1,1,,,,,1,1,1,1,1 Fri 9:00-10.30,,,1,1,1,1,1,1,,,1,,,,1,,,1,1 Fri 10:30-12:00,,,1,1,1,,1,1,,,,1,,,1,,,1,1 Fri 1:30-3:00,,,1,1,1,,,1,,1,1,1,1,,,,,1,1 Fri 3:00-4:30,,,,,,,,,,1,1,,1,,,,,1,1 \ No newline at end of file diff --git a/gtsam/discrete/examples/Doodle.xls b/gtsam/discrete/examples/Doodle.xls new file mode 100644 index 0000000000000000000000000000000000000000..c607581e9c7c691a11ccd033d6567c61b6573bef GIT binary patch literal 13824 zcmeHN3yfS<8UF9~&d$uv`vFYpBiF5c*RpMmU8?p4C5xqL?E^3_Dbw9)ckS-ZI=kBk zzDBg6LI8p*BiIhTu3rGejisZ$0S^H)@g13SITOMQLCQf1it&t9ud8;TCw@J`$vIRZ{vCYx5_ zt%IL4_pchA@A{e;i+Y@bv-$d47Pmu#^V79WD~;Z*=H6=VN!%Uzwkh*-!&!up>p1jl zAGy6g#f4;5rMKAkZx$5bEphoSn~!UPji{W#`1uGfk}=dA^dl@+mnz-Gf#P&`Dhwj{ zTHo9{W5J9WZCmHQy>m{3=$t#mt#4^XySL6+xfNCx%3SGaRUXRW8x|6k@i;<0=6tAX=;ABPtw^5>p`zu*k~d1v59Ps``B%Cu(= zmQ`*9gRI{kw!+~8YA}U8>CB2MHVXmTZ8omAs9Q*AcSY}GZ-Ts3T-QWf*eS3|W=VUqC&YtEmw z@gHLJ5?QwLKB@igD6$~Hufoz}e|ik}s#HPXj$2ipe)?&7@WBUVbhIX;OSVIGT*jfl z9lshCw}T$%ybP24F>C5GHJCb#LFlmDILg;*qcRG*Hj1*Sw>Ca1OZNR*mW+>s4{fGV zr0uAF`zY!M1Gblpg0TdDNTVi!w*+Ou=u9WgA_1jL6GW~z)6YIk2M=;pEv{@R_x0U^ zr-R>Du&lw;y$AzoeMXW;oCuQ0j#G~O6chJP34i@! zMT_vd*OdpnMeWFJ9w~W?E=A^=Qa6O;^o)3msdGi8*IT?8-gxPVJ_h2Xl#E2)ud@ytn#N>>@V8{;JB(UM{B4*7a+#Q+So<&FLyv zDy6RBQukb7v1*{ZQYw0zd&>RA8%Xah4h$5%j`Hqer5hc)rnq;YPHoak$dk)m!$iF7*|$>RDIZF)%XZ zt=Unk48ia)R62^4U8TL=RlS2~=xdfduzY1@dvU;9Gc;6$sa3@aI<>mIt+#6@P4)JB z8%sSq5bKwfM+SxwT|L9YgUc?tq`O@1?kg?qD)(Pf*>&me#Y-z)OPAizzpZOz5PN98 zb+47L$=kSo^?L8!BgMX<)|y=GM&v03UR>G_E=T?EN2uM8=WpZ>&H^C1)j2HBZ#y!^ zw7+_IY|VXZ5AL~Z?ZHFHyE~4I0cg(V^5=K7AN~NjyCs}usBFfa@Q<(`SSLI)%P43JN4B4Q?L?C-zYCk zJp}#y=(7hXXIUZui4JY@NDNv$T<#vQ07m46v6`9^pin&mTAAplfY-j`+WSKfE+<6 zJnD$R6Q7Z1KI7h)e(u1Fom-}n{xawnq0jflJb7?$IW<2ik;vy=@(S=< zmbc}19QO5~DjkrI-09a(9hRqE`iZIUZeN#MPSSgz-*{g?fIjIb?)jlSi6`Y47O+BQ zEqFKJ9YXr+1UY4FU&r(JPWcYxU+wyk@on~%<$0NR0e|QFp4RQlS^taBZ`h~q{>LXf z#qp$R8_Tf{j_i%LH@0>h>8XFD?i(12#NWE5)z&T6#6uYW;_=*WxY`9Xd2GWCPaOAd z9NjRVF&L%M*O%{o;qi6bHmulc`11^ZzPaCMIMq5HtaT=QS>$>7C?t=#v_&ubn9=^Y zxgW)xa>LTR(~;EnB|@?XdM@Xp^m<0Cu*15?f(gKZW;V>t}$ht)Io}&eqRi zwQuVKKj%b*$7%4{CcG|Od~-gsktW}xE|ASWiq47@c_c2tdoY*dAnwyQU7LQ{_+KfAao;c)j&1dn99wQ^*t-XnQJR%HuN2=t}KLg%{hQ}ksfVX80^%y4s z&u|$a$isrcZmp65B6*$e*_z=)?UoGv{r6EW;+WAuM-}#Hw zrBO&ZC;QTtx-?#wCLpDU_|jxunyyPTkkVItX|^u)TNOf!rW>ze4?}3dbmK2RH3<|w z#iv|Q^bwy5sdCM#Tv(NhsB%$Nu0@rLsd8~uE}_aLRk@TZmsaI6s$5o;%c*jCs~qR= zg7F9Twizg0XpYW(s!36k6y;*xVKn%fAth^8R9I<76ctsPEsBaMDz2!6qLPYADGO;O z%P1NUsDS}zkE@z_@!3-M6 zIIJY?PwPm!b-#D16B-msFJP zOHr&ZY|V_KvWm(n%0`Lvalx!0i4te1pw1SB66dP`-4>u^YudiV@y(XmzC?euYq3$H zeY>3PODzhe78P+V@FlyPjZzExX4|t-YJo4=QR23Z)o&YS&_;6vMb=%WPlb zELGr{rJ1YZKq)|;I}Dq$BaW+=pw75LDXzw)I8d@{v3)77P>KU3Thm4`BF+nTuS*8 z_t179?DbMgMO;exQc9u3y!ZXya^T5n6>;1v2lmn`;?l~OxHGmj?KzHTC$`M? zCGLQ2>-M{h_fM=7g(i60*XC}n|?P1)m8Rz+M^MO+pr+2!mxE~`fGtctj-LMf|I$|{ty3Z<-? zYbe3Id#lv612DW_1%DU@>XCA&_0 zRhd&L&K=dgl~MIfYVAp_Ef7U&^b9%d62luY4)5P|B-uDX$_fuTaV>l=2FtJW#UxZm%ji zyG=IdWZ#nyW3F=yVpCTVw}QF*`Bsz9o4aT3Z?^J2-`Di-zK+eGW%MKFZrAT?MpowK zXR=Usp+UDhoZFYnr={S`I_qzFQ&t|};g783t$rPkWaK!v@pZ1rj^VY2kfs4d9?jvZ zACSu^yx~OTH}be#43!YjVr+!qO?>+yyb-PkLueCMJRwxTIXwjYIctT0Jjdk_Fz3h< zf>(siGXjqw&D|=W+=21QT?L<9Df7v*cRqRM&gUfj_{3)k=E*#NJY%QKLAxm9zn6R+ zo;|p*!nMAO)^cuj(W<=s0cE1H6usqM^o{Gk1Od7pkB{kz{AQO2zVu@r(UIoy3ZHLW z|C(y_{5QgQ_=?xqW=qC`U?C1B_#Hl z{qI2HLV5!dkB-=X_U(Ek_PYm(+mS&e;;{#b_}q-tz{78Q+>{6VjL+$)m^K-o{9r`l zV6YN?>qFz$lq)-jdP=3?Ars5gJ(tUo%kceiKiaq+-(bJ*T#6_iE{5&MP{K11#BcK% zWd98NeC_`;HqKV&^=#k4=+4gp*avUIxBK+nARbK=dYkBCW=3eg;>y)LzA^lThQE*_ L!W-A?34wnAHn@rE literal 0 HcmV?d00001 diff --git a/gtsam/discrete/examples/Doodle2012.csv b/gtsam/discrete/examples/Doodle2012.csv new file mode 100644 index 000000000..54520b614 --- /dev/null +++ b/gtsam/discrete/examples/Doodle2012.csv @@ -0,0 +1 @@ +,Karen Feigh,Henrik Christensen,Panos Tsiotras,Ron Arkin,Andrea Thomaz,Magnus Egerstedt,Charles Isbell,Fumin Zhang,Mike Stilman,Jun Ueda,Aaron Bobick,Ayanna Howard,Patricio Vela,Charlie Kemp,Tucker Balch Mon 9:00 AM - 10:30 AM,,,1,1,1,1,1,,,,1,,,, Mon 10:30 AM - 12:00 PM,1,,,1,1,,1,1,1,,1,,1,1,1 Mon 1:30 PM - 3:00 PM,1,1,1,,,1,1,1,1,1,1,1,1,,1 Mon 3:00 PM - 4:30 PM,,,1,1,,,1,,1,,1,1,1,,1 Mon 4:30 PM - 6:00 PM,,1,1,,,,,1,,1,1,,1,, Tue 9:00 AM - 10:30 AM,,1,1,,1,1,1,,,,1,1,,, Tue 10:30 AM - 12:00 PM,1,1,1,1,1,,1,1,,1,1,,1,,1 Tue 1:30 PM - 3:00 PM,1,1,1,,1,1,,1,1,1,1,,,1, Tue 3:00 PM - 4:30 PM,,1,,,1,1,,1,,,,,,, Tue 4:30 PM - 6:00 PM,,,,,1,,,1,1,,1,,,, Wed 9:00 AM - 10:30 AM,1,1,1,,1,,1,,,,,1,,, Wed 10:30 AM - 12:00 PM,1,,,,1,1,1,1,1,1,,,1,1, Wed 1:30 PM - 3:00 PM,1,,1,,,1,,1,1,1,,,1,, Wed 3:00 PM - 4:30 PM,,,1,,,,,,1,,,,1,,1 Wed 4:30 PM - 6:00 PM,,,1,,,,,1,,,,,1,, Thu 9:00 AM - 10:30 AM,,1,1,,,1,,,,,,,,, Thu 10:30 AM - 12:00 PM,1,1,,,,1,,1,,1,,,,,1 Thu 1:30 PM - 3:00 PM,1,,,,,1,,1,,,,,,, Thu 3:00 PM - 4:30 PM,,1,1,,,1,1,1,,,,,,, Thu 4:30 PM - 6:00 PM,,1,1,,,,,1,,,,,,, Fri 9:00 AM - 10:30 AM,1,1,,,,,1,,,,,1,,, Fri 10:30 AM - 12:00 PM,1,1,,,,,,1,1,1,,,,,1 Fri 1:30 PM - 3:00 PM,1,,,,,1,,1,1,1,,,1,1,1 Fri 3:00 PM - 4:30 PM,1,,,,,,1,1,1,1,,,,,1 Fri 4:30 PM - 6:00 PM,,1,,,,,,1,,,,,1,, \ No newline at end of file diff --git a/gtsam/discrete/examples/Doodle2012.xls b/gtsam/discrete/examples/Doodle2012.xls new file mode 100644 index 0000000000000000000000000000000000000000..981e2dc2545e3ddec207b01a92414779acd28cd4 GIT binary patch literal 13312 zcmeI3U2Ggz702(|Yw!2&%K@R0IRi3fx%LE!-jQK>~B@xnvJhY%_(|8r+&=ls_; zZ2=*Qx~b>RIrrRi@BHqWGj~4v)1SDn-1yesSIx-pelu?V-p`pyrB7i$h-Js1zTX!y zE~l^$)jPkp5rJG56FE6ye(r2#PGkOLzMnV0Hus^lvuXNUKgNzRCr*9M^nZT?yAUw( z8>oNj4=-W6VKyLOi_M04`Q?|*&6_vP=4Rh)&Mm@p%WT1Z5&!yPxCnVm0%ybevN2@+ zSr68S?GQH7ZVT=E{Y|q8xxb0F@Y~{PPK@V&B+r-}@+%ImzKNRSDZWD?* z{D;3y`v=_|+CuTF88hEZzlg2D@fvf{T$7_;=yn%YTH$}^_n;j^GbLkqpWS{1nzGqr zN9EaV)o#CGYhQ>gjc1lp<0J6B={#pEc4+bDD{f8igI6S}>D#CG zAKAZu=F-9YFCK`6iwAE_--b!vNA({)-&*(>8%>*xxg{|=I>UY-i>vpTteGCQ8u2zT zz1JKv$BeloLpc=2BO8bp4_L*;gZB><(|1l|+?NiVx`bYinuGX*`cW}Po7LVy~i9jv#Qg${jlG-vF$o(`HHE@{Fy)XLt93hAB3Wp8MpdB z##R4ZFVVA;{cd{kx|99?vp!`)IlRFy=6N{c@Rw2j!Cmx6cF`Z&MW5SIKY{M-c(TT1 z)7i8!(Rk#NvXeez^*DjNSn!+yIyw~HQX*n7sz#D4PD&vt!j7rncS z{@z{mfzt0+{rarOl~r?y#sm8u^*COo|MFPcI2+@ADcy}pTY4L5TUIwRwv_9q)Ymp9 z`k$~#{#hj5_vWRav^m#ZS@{CW6#jiZWy^Gcge7O8{9ZScV}FK#ehr$OzSe86gvVxP zk6{|mc01v5OxuO^RyaF@ou^t0;q0tvme#}BW7s*5v)Af`XIkwmOBnE&F&}sw zoj=m*^xDheLrcB(>RPL_+Uk^zIoIrTSHlad?e1Eyxr(SckHb0LTW)tsIL^*Ouhk4M zEOoCoKZWBs+q}|QUq!2jmYThl)@u0Z>gCqT3VJ-Vezn~RKeE*9Tq(k?z1#{vxYk~| z+Uy`eA8TI?&$pJY$nJV4e6qFBgpV`5W@kBkKiW52y*0Et-Rz<72fLTs^UKot)6Gt& z89vgz-s~+vKG$69wddR2@I$SYCd};^+O6>M*41keFRafmw|e1&&6W8jOwpsg#bzgb zcy+ajDSD{8-dVfDn5DI~YbWly>-zQUN28U?(fRJxyN)0KWT$<+z0_KqzkA{7)qD1$ zFT_JQEfXA`?_Tb%wdYsEC)S%QtJ8g4o3dsChk`y$8|5{WucG`N%Etik3B<{FQT`C6 zgjcj^bj;1tR^~;>;AjeCQRc@kmM_uo`-hDw*+obZ+XYjx`>sqMf<0?q|LSW$JA3)u z$xBv$$m$QRXhN5(W}Woh+{o5-S{^AdR+w?x0HBr+0~c}>X^Sb0n;f0N*% z{bjJI(EhR*hxV5_*~UEN`K#fYuKm?9Bif&DeDW7yU8em_nJMzO2XPboGh;ZN@QWe|;cK#ENRRlXcvLrT=b!B)1NtLvC!pIqqLWx2)n#_jNo0=N$rWl;C+Ewk4%ua!PR_kiJEVwh zI^i;ESJ^=)v_|caTesN_nPEwjICrFwkDMRhDB<095Pf~#_izJmuDpR`(Ne)Rawev`-l4Xsg zI$8Q81td+8v`2Z7*=Zo(M?5pcIs?`T`3%mCgnR~PhOV2SI=NGh=SZ$zTGFgDhyY!u zS!WOdx~@q5l}OU8GdTD4$Q<&bS!Zya(w0@StdXQyXK+2xBMZpO6iIuOrNn*%ql;L} ziaPOy{^+a zBu8i9>m1@*TWZ!h zVl7vr#8UEh1G62mmg{daw|QcnC)RQ`)?PGg$@JQbj_W+db)IsFtbKJmokQ}7Yh9;V z=MmR>EINneiFKahI!~jJSZP!1_j z4k-}p0>yQKSQil2x<8#m3dFiVaV>Wlx}9cSKwKx}3&grWaa{mwZK?Bh0mrTDbPg#H z>jK4ffmj#7T6@u~WhIpygRHC@n9+!Jkyy)pY2u|wtc#RGip07|tc#RGij+f&V6DfZ z^L3H(wcO9?vFNxi66+$dE>c_T>Uz*<}Cd|e{eCCVWsit7^PkP^jpiCD{>v+hslkP=wyI-Rde zV6E$PT$hM-2~4!5&LJgQhm+G}9eBGzSMEw6sGrC#%uDXz;D*JZ?Y zGGk@RA!TA+rnoK>>oTz}6YDaumS+@toH}2ZDXz;D*JX<9GUe+s#dVo-NSRodiFKJ+ z%RN>?LRNbXw2fF-h_yTs(w2H1QlYr65NkY*p>{f7))iu1A=VXQU7>tkp&U}7b%@--CS#G+UIW$7&&-#39xWv37{HL#!Q&Ylq@m zo`2}$)2wB+*Fb&5+6C*xQl8UlNyoKItX+z0msq=$LtKh$msq>R+9lR5#kC982|<@w zyTsZh*7Af?x6}7BF0porwM(pBiffnR+NHRbr!Pr=vf67v6S4M)wY=(2SbG%L9_4F~ z;@Sgi-A=EsJ&J3OSbLPOJ!0(UF% zCDv79T_x64Vl7YY_0j5knJQT8vFJ5ll~`AawLCdYe9CICVcs=hE${htosR1o#dVEX z*C?*>W`VruxULcF8nLbs>l(#%jq-Jk@^uZY6B0GB){@TGHDX;O)-_@+??*HVz2>V? zT-PXH*NC+|mrceZtGx!~D6Z?ox=uNy4%SKUb;{Ruit9SDu2WpsDTmaFb)8t(iFKV= z*D0>+6xVf%>pI1Comkh2b)8t(iM70m(#Njjx=yU+ZGg7aaV@L8hI|HOpFR1++9%dN z#kEgy?GtOCSo^dN@hPr-V(k-apIG~pLwsWGQ(XJR+9%dNSSNAj6KkJX`^4HO*7D9z zd(rpkK3MBI&01D_4Rh;&bwI2G%GUwKbwF_)5bJ&9pd2D^SM?F;xRy6INu8|rZUb#&#_^U_-lEAjAoAOH zD$NFAp`b=gLr%9kg<`Y22YE?#ml$;#Sq6rZ(_X8){;k`cM=1?hiGw z=U}LbsZI?wvCW>LCVT1^=5FI-Dz{Q{CtWsY%*RtJZ{ptrW1ji8yr46uEc6izZJCvlocDG&anl-m<2<<+5l3L&3UNGYFDNGU#~ z6dNhyfAX1x?8^%#DdlyCl=3-*l=3K8N_j9Tr938+QXbk!DTghk+|WuXw^vfiVqZ#G zdP*q^At~juETvo|q?D8;rJO)g%4Z2u#;-EpMjvzk3qw6`y7;$-*X$3Q4$7W+I-2-5 zxdppa+-)2)Z`3sY{{_VEDfHem%jU8j$MPFB-Bynqr(ypxj%^0JD5me+rQ$8B<)ef6 z_}@zZZ?T(Q&Hvs0WzNKhoc8jOgnSwyc|l$%%ZpBV0Vgkp(lFXc5+oxa86~Pj$!QP}5D*ZMoFr#P$zhm5aufvtMPZPf5lKpvEFvIT zKp18aQG(2fz$nAaYtFgfz3034y#N2#``3DFz0j=gsoqt+ch%mzsw=eKG$f}u3s3>4 z0RT7;d{7(DO(g?>SCjz23M`P>X+r~o-2;N1ZbjU24|0$U_xBSho+T420#1SS|Nrs- zrWvTcHDcT^&D14`V|>^kc6XHQ{7f@-uHda=SWKg6inog$U)WKU$oC7T7AZG_M!vj{ z+Vfa>@7C(S=|<1jBCpKtDVQ7c(9+xd%eF5r7xI!KPTF8*?&^4GjGsbR*Awwj2I=Qs zamqDXcTAp@)D|&6wxLcmrrqLZ{yLlYiRSfLVeHZ5*L#M(ryf5@WREiv<$n}2Pn-1` zi*}G3=+DUsg7OBU?R9U9N37_z3@Dd0wB%m6xd`*0;dJ*&(=rrDx6r=E>6c=1&wy7m zmXQlvKlnNCb({RKLHJE%ulaeb4?LJJzK>QwF64mv=<)EGvo01%pIeMVP#OHx>reca zn|0Eu9~~t;{%#QByJFo#Xbn|#EDR3s8PaPz(9k*y#lF8#~@E zETp%g=eyJsb+k)aEMm*l*EV*?p6+`{t4CEZwKSc|)uBnR4K`TPedBxF{EC{L)i$B4 zNoe&tv%>8D?C*rg8^28bS~A6JK8h6?jcDBAb2p@NMXN2?FLd0NUU&6+rMKX$4YSA% zQzOj8_!N({8eIKY&3$`+asx>!9-|k&_4t7yhZkB~9T`3!L`i&`9Em&sbUgfvoY2eE88wzj4ZUW9N%6%!CFJuBe#v0fjdtbS*>&-r zhQ4!FK8zloriX%WFIJk@h40P41_wnAZKD5nuczk@2JC?g+74p5fYW5*eo}vK_b_On zuL~6Fd-4YQ$JVDf+4`Vf{D1qX&6@D(m!=!qjq8e=jJ`XSA){O1I8-z*5e2wjzU$?} zZFrR3h8!s}dLEo-Plt@}*%f`h^WBqYfJ0<6&gv$c@adl-A6LR?A^GziqD%vecIp(F zv?4+uXWJB)6nW$}Og{gNQt*&pf=Qo#lCm%FNBK>zC{|CS>vb;Ni(^mKk9u^JH?u!_(1+&lN+f13o9FZE}npz46Jp{mZdlU&yR<(0Mo0YQ^Qp~y5HCZL#gXK=X?A47PfLmdr# zJo;3Sm-&cQpsH|olLta%w^SuCT^7owBo&RE5pp zucwxlta~G$lQ;q;PwvC`!eG8U8z(~xESyjH;^nV^FA^U0KmykK(M zHhO7+a zj{B}Hr@ieN?z~2B@ovFHwuZAQ+^}TPdHr!!(zFdVt@m5VRBO`y-p+`y7`xs|#*-IG z>Q}_KaSxr@XFCVfDINxv$up8=uh~dm&+rR0GS08H8!Ic`<6#och+GNbP%YIt7R6g? zW3aboMY2B~S#mwvHr&#U$oUp^<^xGXRnDw!XIu4eCxqeJPgEy}5e6qj@XrZx3v>=U znTvlI%YWR;{L@&*XO7YKNi)Uo#_lp51WZLTJuL4^{!lYRZ9TQa*sA|lw-E|0;-KuE zexbUpsH(G{FN!ODQ_{p0_~f2|uf6J`V}IT2?fU{s#r}5#bLGSbZssf*-7TYWRlzv_ z6lWQHnRs3oVU{{}RrY0YpHuH%ef%*CSnDZh6iG{^f7w734vPOT3SH8UIwEj-|CjiG-24xPcf5wZv`?1nb`{V!WqQt~B?d;%ulv<2P@u zT*!mbyt!c-^V~*rvN1MQv#sKfmH3gE-)R?(Ew$AaSgy$?Z4;0?OQF)i*e?nX4K>6c zW#HeAU14$6(~oxd_4|+XRJMbu6;phU0<-$ zv5q2H=sf=ix0%!5_f@Rs$c5O9sUG9vZG%&rA|&=T@_sAZYol1K>9{$vo!~3qDc#;K+~??K!0sDRA&UjLQ@5usB&wP2 z1xgGtzWxeaCFiu1)AY&8I?r%E%}(k2nba1bQ}fQZ3!2es;GK zzm8Pj%=0(Q*n<}VHE+pQp4|S@FQ2m2d2}j2i7`Bg@L{rrJEgIhZ=|jGBiZ-B@qGJm z-aCe)mm*#%8tcN*F7`t^uCTA!%Y7-Viw8;x^-o00){ZQ%@yH*ZGd|1rFkY+_g(KHe zn^P~E!EH&>Q|7@cKai*9KaO;mDv1_-Sbj}aF7ee3n1XgCFKT;5&uAQG3C-5#prO3? zMDG`$^L=h8rm?Rv)17g+b{o57tpgG6U}es|>t7`>CD^Cr!h*t>b zwdJ8Tx6G^c6AXoI(TDyhdg5Mmn4^T$qJITFdt}3k+Q;a|1>xZ!#S!gy)>cO6UQ3KY zZ2DfD3%+eS{e=9&MFt~p3XP^O-&b`{adQd1`W;hU$&EgH@NxF4Eayo2{`KNVhDu9M z+@+uyNzv^uMf)wHJzgANb!i!;4vo-}+R%%O&EQlP3?ja#tn;EuqqTdTD?`~Nb)mL8 z$JW|*w$j74K(J1l(?!XbOPqZr-r;m?*O~AL`uksP<(?Ty3zF}IsmAQRcq~ScKmV?_ zHSt>bRlnqC>sg95BB2qqPgGA`zi9Gxd}FN7ceS}GN7`FP1Hm2~IDUK6%^|d+Iz@PW z(S=6Ehhl$reHKVi@~6;N%ITX>N+{Crg7*7SSgAf|QShCp)iEzMPun8aR`==jKa0Kl zPKnLT-<5OWT)ve}rFF5e&2gTUugZetFz~dJh+<(c^>Gi1mZH+UZBTNV@TQ*IZqt zt=!)t!VOAi6!$H^-YzuQyyx$@oN;$UQOAnl{>039jr8PVB+nafgnN-7Q=d-lo9pww zf~#6OJnc8RCSM5N{{D1@Y~fT}6_)E6<9zQIgeJp2sr8FEqxsw&FY}||c#4F(#TPW+ z@-+)*M|(=wc$5skbmJ7vHWdzW_nG~kt=$*dPF??vkEiIg*>Z4pE&aJq-v~j-Cfd!+ zpL}KZP-Di6x)pWzdZN}`(m2yqTJk>pdV17zb5izLyzBggr`rP4u}iyQ^|}R`t|wBR zHAIt*o%2nZjj!rl`rv5I>p-SKn>6Rfj}xVLJmY?9NRuj0KRIqKqV~z$)rRU|tKR%( z)UMELvdJfSCj*;R4DCXcr_ z_j-knMoM{KDQx}Z)D^UOX_A=CaDQ)44^3mIb>OSpt)t*&RL)u_V2|VFvhSm7(BEkm zPtKbVkrYdJ^ID*6d@Zsk69cueS)0>SeX}IuTQ@hD6O}5A7;--&wj1{3@N-k;=(Z*< z<6e8r=nU?uLv94^GsUOjSb4MCO8LP?q=e_Lvdg90U-y zWl(U0pZlL4qnVKj7&d{)F8<hM0u`WVkp z^{C0U=+ac%NAKZiqta^H89pi!Yw=`d&`tNRSMQ7+eEgJ8zJMgpB&|MW}DDAU2_psoQZ9>m+>XaSbF0xJs#Yz zNJf_r_8%iT8y5NGqa~$&U<`j1N{x)jCRm5fJsgyAmO?6z5wFA&Bd<67y1jR2+#;NX z%4b>Gwt(L-q7n7x zJc;7E!6*)!?)X$GWJ>Kr>04I`s^+a1&8pj1#1z)Na)h5?#MUB{uNquW6kt*DbGY zMrg8}W{R)Tyh3MX%Ms^cE`0fIicj(>rOjNeFW1&gJ;rWjiBe41?1`t2KV=vC34A68 zl%IG0$lJwnj0zjw7RkCoUGKooa&Gc%jP0aWf}pshFqiFdC2uW_wqsv8^OZhXQ2_p1 zI!vpqjjq#XBE`>i$k_;evG9q8@|T71rlh$~^XxaiHd0_>{dB7QJ1HOH{CvC=Xw}9l zgPBrpOWqREu@OKzzBee-)Y>#B_vZ25B-b+*ccbMSHe_)T6q7*oO}!G=+hdCfh*suA zHdEofoo@G8Cg_^S9{RqgRc8~PmkeKe&Y|)qreFG{TD>0aUeZsK=VkY-GvBwpKwlco zZ7a1OHJg<0bcY?BQh1g26y@n8$~ZrKYM_Bq=}Y3bS>g6LVM>I$(MOi@aK|rFAM?+( zibVN6pS&-jNM-)8?<<$m^*-^cP^H?R1?iz383C}`xzzW@j2Z3At{!X|cRwXhs2+n* zP#s+G`!oKc^MWA}-*3veU%c}9HNya(7$IGJU}&Fa0cPm17QA@3fq#DjCd((bXgZN; zQTHPEGzoSU8#%-sB-?@}Yc@FD?<}LBWyXV0-Iq*z_QTF4NH$_V(QQv#%=6`)C%*Y= zE8-uG-#eG=CQJ*PiCNIMXE<&^LslEFR}DfRtc*RgPxqM>|M=Mb(+GVzirXX_SRMNVpAbs~xs>;7Ee{Lzg_=DzMz~$|P8hh%HJVtHQ7kqrOnaCaO zUxK9_A-@r7bM!x`GA-rFjwx1O+r5fIGw#k##~~&xCEebbPqV!z?gX?J7vOW!;p2TD zx#FE}E#GZj?og3is3U1%44%f#eGGc{sy1eIvy0_aUh{gb0Ou!`C#LnW2?zwPD}`&> z_}psH7kKl$QkbC5&Q$TMeugt~ISH5d1;TUkqF?Bd{?-r{U3E4u0}bIGsAT8Q z82=h7J~JN;Sd*sf!?X1b z(i$D)S6HMaD_ncIcRSQZpLvy1g}JJFi`m)DtbR)#uGLb?ZVqS~^lKfrPhz<=fBhSy zcA}{ItE)zFS31)Rel>OF`A2zPbG;km(;S%`??1XAd4@G!nneunFu%STs-0o<8W@yY z40_GE%5>lQEp(1G&g>y`8EPv+byjRX`yih3or}B^vF3Z=el&XLVB`9)TjAni*S>NI z^D#BMaZxPBiblQvhSUC`tSnS$Jn{CH1(wn?(VDUc3aCHb6AwB1nQ%Rv=OC)LTlXbT zpxu0}l+wSnsvBQE`_V=R=GarItTar@hvQpA=vfdze z)QlS9)vR&VWw?G!iC^z@c2LG9kLP3Rd7Kqn!8pX|27}4Q=Y!#=MjX?ZrWmL)6o0H! zUi0mbk|6zUzLbM!bbJ)VJO}G_|5)0;gv0*|gr7ZH3IOeA;x6?zFP#%Z<<5XI*Zm{~ zoyJoVT(x!`b~fd%&sD;+Fr`op1LMy6tr(Z?>p}*E>!v-A5s=e-!A11ufo%7Sh3KxD zS;)M0NI>;C6iD>FvA%G*FDKptPbWgKhY=p^=D@Ge$Iw3z7`x))iL&`hRzqty1n zE3?ZI$%9oZL*=tqFYOA2s~Kn6h@Z}OHhKFq>Rfd_XM&+l*s0Q;^IS@ek9)kWeU0Tb zQE{x-G6TfxjQ*&)vghSp*ia6VU?p9)?JNPoF^rEzr znqfvOA?z$w{Xv08UzMs7|bI(96{AAy}# z{N=@qU+Zn$s3=61o2opO%tjyS5wrhj?%|Ch4(#cTiWQ7IiY|{9H@uc2<=;tFsSJD8 zfiCDYEe+(jXSJg6kx_ha%fMDtJB)2YV$-R*C2P1GbC@OZiKWYZih{@IhGfb!rr?I> za2Ja$=CYTSr#1XuH~@};Ea)$`7o+lT6AZG|6{@a!(rIsPz4N4`j&k*TxJGKMIX#1} znUm{8pGMvJ6n1v|np`3|CF(Kz@KE#gHmNJkp6BJ*nD(8TOX9Q;q$&-!cmlu1ia91y z2P2>PtN4z|!mxIb1RS(r|K&W;f>!M0I1f~RXZ4AX1%Q*218i}Fy50({a0l|T0dRhY3HnsxP3t+jM6D+=Q0cV_DgYIbFym{kf_!HWb-~WCb%>7xFn3ni^ z{C}tZx4ScLL9YG)Kn9lMPyKLn4{`+naso&L^#mCK0E(0G&V~iw2?pVq6FBhC_D3hM z%b#%NAMLlyv_YCq5N2?_6L>=Z9)y)ZpUMrClLCarJ^w=E@)y|OF9aO-kAoS!(zyp% zn1jC}AWv}jAf11}WU+3}*9^gSNs!-7H}`8gAS@2Tl^))@`XDR@o}WAdbuIpc$u4;Z z>)-klK4lr;XME!y@VNjNfR)4}|evw|<#{Yn40z=GB#=QX2l-}|CW4%D$P7MJzfFIxvcmo08H2@F* z%m6(=3y=Wr0D%A$@BlI1AcZ&BdV+TcTfKoG!1r${{eYYQe98}`&;!1K&wTsinY%y; z`1DEtmVlNq5G(%A9t5Q4rFW!frJqZeN^d>j3K9DkCN|K?+jSxn3-)_5@(DL5{r<`0 zN&hE2{n@`O$gL)b^#eS?R)6sPQ>LJQ(o6i6JCA?|T>r!foQ&a~WPI=!Dc%1vk_6xa z_ID!niS(f$><8Yt|1sWwjrM;wdm!yFtn=Kz#+s#gWj{?f4ErlLAn*S!)t@tcBGq4K z-t{kZf1PW03O))M3VjMC3S~fmLWANe1%%=nc&AEnZ@$mL@{{!*o<$r?y3+4X^Jh3Z(;%!3! zpf(6JUvmFy+cW#ahLEvO{Hrb42msW-gZALVzuLq=ePBa^-|Hy2h6INGV>~jlF#rg5 z4+nJ~0JNcZA_BcVy@CaViM`N1mxip0KD-dp@CqXaBmL)Ffsxzflr+V zD9E?~&=>iGdk7i#KVe-mF@XG!_EP|m>EwT_gOi6zZvf`gKqVPKPR0zJVkRSJCL;|1U^aq`^3T3c z*gZMOPLWekQc<6#p*;f%LtftP?7w(?D za_`~u(}G%!y=-QaSRq-Lz*rhuc8+tLT*4xvVizyT$tx%-DXVB-)6vztu5Vy|%fiwM z+!n5G?jD|A-abLWA)#U65s`8635iL`DXCf6k8*PJ9zV%1E-5W5uc)l5e%aLA(%Saw zb$eg`z~IpE+mUxu(=)Sk^B)%$(U`T*>t8m$e%r+T*!#JEaESl)`$Vo2IsfbUTe-kV zAUj1tK~6z^A{W`IuoJR8;>9bl7pEvf>2+Epa*<1oAY1xJ3 z(89PA(f*L^e@(F1|4WknQ?P%@wFuCWlYxsz&J1V(#MjMZQfOilc1s5*nyjXb_S%*s z0Tj03Nse<;Ka~PLr!LML`W~B@wtLlmX@4c7dA8~8g;ej)i`VXsoVSn=70I&lFU<=* zusw_(Sk!NOY1H<)#dG60X=e;Q!HW;G$IoLG50rZFJx8M9Tx*uCxcOmj2R$?@4yGB} zE}o@Rx1~YX%@Y3d&;-Yy*33wEP9)pm>A+%AOiR?;W$MJ{XlG1F_UqMK!dw=OOcg0r zP*1(wK-=wVi`4bBJj07+&`5)g3xls6%T&y&v?TU!b-GHh*Ps17o#haKS^jzB_e8G~ zb2mBh13wVZEem6B!44;@v0+T8@wp_Rzgw^ko3nB+nr$}mY2?cqn=LLlvg)IcivPBw z{nH}aJZ5NHzSn`l$0*LJfxZl{MT0tdv1PgGtnEb~!(FRA`4CrgMzbPvNFmdP1RX77D8DTK0>)87e|B-FMMsgbgOiEse)>0x$W!{ymq?>tpyV$0ncTx zk$~O+Bnfz%^P~vT7mqg~0a_j@Bp`NKg9JPj14uvu4Hij^A7do}l29B&kgq?2b%^}L2-Oy*m=Io}Bl!EW^Bvo~TiIKCkmVALh}rs4d;hvna_%75@P zmKF2~DH>h)iT2j5)0-fu+|AKHddFdSy}!^%S2E~stbg5i=j5tO!N(Vzgd*JsCjEkPs>z&GYM$68N{qk0B%5yZ*I)=)&do*`Ih`Oz!3I&$=>(7u@Z6WT*umL`pZN zzl=NT*|@3gY_mi()D15avN=^nNmDtTU1*i+ zYw$4bOHWsT<;E5#GZ=qm>(HwfkM@6?q{m4$x?lJnvt%~*2 z<27?xZRS7&atGFQOTPFl`>6uBcSvaP_S5b9oyF~q_)^@t?{-w~&FBr_Hq z_Fr}nxBch(Uw-!WZy18L9Ql95dmccKvmp>zFg_aF(9b7^L-tE0MGmKQYLxjz@`NoK zI4(s<7kJveoUX%kcYf%UryN)jnwRkRY|CHMpMst@tZ^a2qR>^ktrv1Gww5T{G;jaz z5SgAcS+-Sod_T-=cB^uxiY-D{qO7|Mm9_j!(CQ!<&$=4%(=crrv6dH)@UEB(mZI5e z%`ePZZHR;LPB(||UpD;ZJ{v3s=lmLKBATdXc@!Y0)Ttd(_i54Y8+{x#mBreGl_;ubX8ve#h8Va}qyc=i#CRd-E}gz(hiQuIXWe`-q3y%+yE zRbu2DyNeBIs~7_JkqYcKP9w2Xy~3O8AY--5c+YTF+;2ysK|El3)cU)By+GT2FOST- z7li_9D|&85Vwo~-1hBBCJ`+wZSdlEn%er8RfYZXPa3FpT#_P+`)K_3f?pYO&_B$v%dJWrhxJZqIFgYO&XPAk>5 z%*8A&p5}cjGss6Pu7i%SlDo?@clg@VB=^`qToG!CdJQ>2A%T*TyQFMawwP~p z_!0u(hPOkodc&wz9b~@-CqV=sr8wk|Kp)iLI?P4U6Pdw`_fTxX6O>y-whyIqogqcv zkZj^=G&s8<$Fp-V@8aVur?YrH+rp8CdirI;lqQeNVc*L{&T)Ae$e783)XHRzZGd-& zX1nCAHDT)JJUzY&p2~5HU+#2L&38lizt>y}(%%)Tt~Ayk-O>5A8GVdF#5-{|?^0|X zv~i*Oj-(hD@J_d0W=&kj!rQ#Wo0EUO-dS8SY|R`(xHa2FX}P{kkRXmxeVn~5(~uiJ zgKf;uzc2jmin&#RVua{e!}sss54ZMI%ING%#FU*MxpQhR78q8JD0N7A zd93jJ3x+s-!%iSKEKqJz2YN9x-z?}+`IK?sqeZKh&z}k!>gpFJPyf)8Hr|v;H;?MR zm4o^enyb;Qac^H0L9zU;rzLN@qjC{uIJYlF4^wV$#F->OB?t(NI=5NNWJ@%8ulW)| zGfRri*VeKT;aa+B>zFuyIHyvuQPOmNSETT34a9K>GyWU4W)R8I zDTt;wAL^FD4kyZ)HXy0IBlS|`dT1TLtw_I`>U0f;$ztx-Zn1lFB(^;E+qbE++{8Km zsB;$}ufUlMj`EX-cj(~}Yfg)@2XJgsCQQfkxC9MuV0=Ca(EHHo+F9=uz8jmrdhw9u7)OBmqBmwdF%SgZ&FTlTmU1K@k^_QHO8q+bLWARz2_1-*-57nBX6<)4-%D*ZvbXG-NGE(P>P}es1LLRF0lO z=_R43aqT9aTQlQz@|MrOwNB+X`G(YQ5ox~O^+K_sGhEUJyXmdhxLwWB68fn{qCUA0 z!BHmZGkn%RBIi8fk^d{qkz*QS+aM~Sv1i7kbnv`uQcQRU!UWw`_a(e$1wP3JyB&+U z{NguzRn4YV-lBHetMl#46%zsQNjme5u?%Z_pIfPS(>KnIVoV=p|Vf}>=C`I^% zH4I~lcqbVqT5@wk59jgR6@BD=!OmZ0HD!OJ1^d~oz~}eS_!nA5;yL2fJc@(J3p2py ztwx&+D%ytif%Y)3mrovBSaYNdGyFglSqks(vz5$f@O9*wg7Eo7#`9gkG1RaX_LML6 zLQ9rbqDw5Jy4dV}+Cz@ZUQlOFfG!OU)*Trmk4ojb--uJafi;PPMPr~z{LK3q+6P+r zWop8uf~ekhM1s5q>k8pUtEo8gEcPJVcg={-u;!D*o7e3|fkQ)!-}m~6LUD$`Z{$_1 z)4*k^6eP3!7L*O|dgN&{twtxWEQ}pITKHwjp1)I;Z);cI*}(dEHh=wd{>`1XQR^XW zaE{)k;BU=!Mq?GTdp}l)&)Op&RDthnv+w(Ca`QSPV|yK6<{gv3E%WZWgdgTz#)gz;cZ28sv z>)n%(AwIvS^&yvTwKZ2M|M3vYNV&1?EwiL8*^e>r{Z`H-#3|{1?~c;>NR=K4jmua28XRu#d>8Zz%D7 z-;1}x<#fugGM@Rmf%Y6M@Z4^PJgcetO5~ajkQ;r|YWZ!$#st#kFFyDEX0ur{^RHxW zO3w1iAYrLmXV>6@3cFl`W3Kg3@2|6m*G8E~MfshRbDmmd$YBW6fmS9_#EP-s6KN7K z&7(Vb?KStLWSBP;6tD^3>dSgMGC{9>Vv|XR0$TSC(#nNK!G9E*}=PRws7m-;*2Kbhstm3LuF?*06`=lGg4-}iT2OIyY3vjvx# zLwgo*DqY9Ti(Y%aw#!3MnS@9Xn4Y4;G6@o^nMqu6%i$Pa zL$q!qc1gDf|4`&P&HxI8FZTE0e6z#j`sP^(7bm*~Z0x;%!()&vPkX(i&^6;hMe7hH zMDLYV{OdI+lW)7#U}mZ#E!HXxBD(_fnul;|V!Q1o=&bRn4u-1?Rj!qI$7wClChS~5 z4O#}mXwr;9W1Tsa&zM=IwA?b(Zobo~ z$j8=Yh4E~-qGP@t)zlkRFbdarnK{6}kc6?U!D*wvSA^l71W%lS8RC6c`I8{h*hfyM z%dm&>L~%mkYy1($$Z#0JMo@?1%&qV#YyPTEoeq){H=n+NpG&ixax*+O;;_#%oehbh zo8W}Mcj6>!5WyI45S11wl7byY*X<^d05_!;1SMWjGY`{~(P@RzSa4cH4Vm1}r>xJ$ zSq<1)^&3tkw$p=o5A#(^+$nD&C61Q_q%;rA%h~ao_Xqa1x4fmUg}-xpX^cEl0G&=8 zq$#QaW;hR{JW?cFInGn#sLG7v5{+wUMkYDYz$|cQ9I%7OgP$D5aBS~aF3-yK@GO1f zlbw+pX?74Duv(K1BSw0!CpsCXQ+0V1}vp0vJKN zgAw!-nG#n%#`y{&&4?5}} zD%MO0EK(#uBC9NzNkL|4V6XQOo?NB=@2rcFfWG1B8!k@UPAn0+n9DL! zAN>eT5vsL!Y-u!_L%FLy2jAzub7Si5*M}vll>+?gn-EGGT*t5zf5Se4vmrws8lQ`z zhA{Nj;#K+_$XCa+)t4udJ6)^2x%^fxv^ueFX)tvO<$BJg`b|Q-UvHb`4ki&{ZDO?wF zz|fz2i2NEZDV^JT`sX?Ksb>6J>aUQ8m?n;NH7ikqAVek`o`Y68;N^8htKF;5*ozAz z0TN;B{6ep7u28L9RptM`V_Psn@Qs+JFzm!|E()8DZ(W+e*0=C4N;Ze2F7nQqWh!7a zLx+^iclAZ)KE|{%$Xw-gqEx%PT5uVi8804UaxUD;xGPC#nqn@Gcz#MkvRGr`04e^$ z-chKrHITMF_Ow8HoW-bOb^PM@=OW1Q#jFjA?Txd9&)I~Dr2P)VOg?bWYl6utjv;

W>bR}E*&n5@!b9uK$OR|r4u3a>EjXncuX&rCYX>xg?KsQj+XE@(RE5^>uV;x&YPy*(XxxUML)^L@~v zO;T}G{D!Akuh!E5)(iuyM`cvcs-xb8I?<+@bohJ0V+mLB_g2sJkcaop%@3_GzHWbo zU1_54>gq`ewy7xe78kX~B#Q0Y>I;XRH)XbYl1xh@-#=ph>k`vU4tHP12Vq?YaRf;)(K>*lg^S@;(Zd-n^cuaO<2S!UU#3%G zzh!AAngQvh9p{h8sdnj2!$+-KEqxpMx#zW5JBtW%Tc&I2{uRwN4{3b*^FO@s9$ zYlZ^-ZZD@~2;Fh98X0v$OC}|vpRyNi!i9Hw^IN?lWq-`P&$gvv=Pr8dvJ|Hc-mq7N zvh_LI39lp7$%`?pJ2f8Be1`#Hi#;T;aSR|BLNw@o@{pXJiuK`_(Hni4r$_3-eKA&V zqFvV3tOlgc5Zv*@HTaEz#cSt9nOYENYQ%Q^ z{^lq+C4ZBF1nA;TL9ddn(N1-le;^WZ&lBH|^VP>X4Mi)O;Hy^sefH1|eTZ{}Gsig` zgQ#Ze1U0ic1x2_Dz6Lun)I$T2BZy;XR~joA*~0TpQZ4!9I;ZWlu-_lg#qe%y7hp^l z<5oq}iI?y_2Vd|xI4SEXf_Yd}sdqm730hSn8F^+`&9YDp*V<5u&qFKWeQ~Oo-2oFu zeB>JX^KXSGK_y#Gry-}qecqi>XJ0(jpH=f@sHk#^32z%*);`EP2E8mFR2rA)8z)W} zViL}V=u^hp8BG!ND#N?^qKR1#4JOFfjqv35VZfdIipe1MDKvoU(9yRO*via7Z? z>fXrOmD4ynTk13y9Djm;@X^5hRY=D1y>40<3@cf(>PrGL;4=6PG(QzV>i7x#6ifq) z?3>_+gE974T?YTDP>A544>&L8e+hF`}aS9dltz>-$RPUj8W zAO;!>mJK9Z?C51N7sByhomlp3n9=I0N49quCFXlPqQE%X|5y+smC)n1!!W-(ABXTH z0qIV(5TQYg%#p$p-W>e5;njG%UM{EpBi>URI=F<5LH-o3q1WzcBSkc^1dUh>csGjn zjDyLb2WOj~Mr#akghtjEk@bS}-m9p_Uk{_kU&Q(jqR2EneWRh6!=WADHN=iHW|;%D zSaiixL<%ps0GCyIrxP}4l|tlH5d-WoV0mqG)!f4FJMXHJ7U z1bH?*%6OhgyM&iD!6Ew?88sF1yw~(0S4J=mOd=)KE8hy4yC?mEGdZjsZ4Ud&yl>a& zkH37R?9O#RITs^mjUS!nz_+cm)I@kD$D;A9=>fS(gOtZ}NYJ>k=0cZT#s(+VIRZj$VyZ zc&XKm#E>q=18bbdeIi@<(t9kZ@Px2BnBgRxK0FEItTGWV7sb!g)32_fmw7GyJic!A zqZ}iLcY{I|dVt^@3*OCa)vv*?aR+652b`O;Pm<-G*ePAek>!V^&$2p zs5-wZ(uv}O*#5pn&%qx4k?IM|7^BhTu9^dNQZVm@?a>l^Ij5Ao1P}Q~Fz|3%oNzcU z@9{&i;n@#w;02!(q$i1VFxNSMB15I$b*Sws?9;X3VQBIS)l5ri=L?+TfP)^! zDc%Dgg+@^fq8;?~raME>4J-t;jBcTC?Hj4V?%1yVFD1_xyqZR#1*`%nf4eP;~4e#}Lf&~XWviP~ib>p}cz{&YSD{CYh; z?I7_O31=pbqd0@57~$h9>anA}a22q)O%#B+<72=Kp7LrB4MDPVM(r}T0zEvS!Rw7U zvyYGtHIXb$9m+dW-no=`~SdHd(oP%@0Q5t*@ zc9?P8>O_q5JFCpm~(tA8JSr>d17JqfMMme#Eiw^PA!Spb` z{+2`pQ+rGl;}Adi--u+!V~~9)It?$A1k_mwyDfZ9<1~@Wo4@5(+<`UTsysYX4_C20 zdt?YR!>3^*k1(iIP^Cr>++N{0`k`zv8N5TK$(j>ALFwME1YF=FOzu@v3ofVYpzIFW z$S2|gsu_}ZWcUfssEuog>5MMQWQD2Yp+$IV&FWyXg*)re!~qiDrZW;FE8O#2BV4S?MclcoNF6`8cv zxNs>Vi+bep-v+{ z)S!C^%-@iNG%(;j-^~OD(pLQlyx_FPFZLMePE&Lb`L{w&9D7-mFItLb;oEQahUhR% z%`!)SjVJo4uMq@;K`=SAR(S;vwoVpj>~k)eQoYgf`-0_0T#3;rm|!Q)!IdX_JWxxl zgIsz$-q;^|Lr!GflHu7#cyi20Jy_|tduno@cZNtMPfIj@pfhEpl02;5s3+E(A z;wP|sS~HT!-n_fSiEfrTHH)<^L?WCEA18WGO%pA3AHs}VDQzE4BFL_3(8I2F;nmU5 zZK;%%OVj&CjD3q?>1yfGSd)T0{M1^#ntnz){WVtj}?svni8((HH|Ock*nLA+Pa zJH-;ufnMuL!Q{9ACV?~Q<8Sud(Oi|{B3zjvisQM#3KQP%4KrlSqJpj(lQtaP;78cFO-YMi-v);-ArWj zZX!V+dp(>Ry%8VK%uh?;Ny8j&Phd0{@T0caG;BML4Doln(@wSTTh+n zzp~Fuhnw4GaVN_~t6Y5k(9h$tkXhNw&cKW74(G14a$XnU0I9Zj64`&lfs>%`?f=z&hfczOS&u z@RP&!1LKuEcsWbr`H@!`!E4))v}Qz&Zp|g~V1-tAPrPnweJt{Rr@2(RK=(<;agdLDQl8#b8{1u&pR;~H z2kKh4b<;aCpsDDSkAqCYMduBh5BU{T&s=b3ox8%+VtOB&OE~4nMBSF|TKQOhc~qm@ zd@gd#`GQ^=sgsb)P9;%IzK0zAEr3dDN!6w-hUW5kwTCua&X+DSp&bD zdODo%lo;IM&p)VAk>0>H-S0?+Q%r6-qi)jw+r8%7qCBywqHh0wL6w6|5HGbV1!myMKKtxF_nLFfwa!7=I2NwYV=1tMfK127(TRmIRplg+Ym*c)E;&pR z@805&aHW8YeY(ir388H{lt{;Hcj=UZ2f6qJ8s$hwA#O&+VE%Fn`&{?Xwk5aaeEJ5u z2SXfvix{bEL64HCLyJ))&c4T=z(VXAwP`I~)o4wu2!yGo?nD`763*L^MDPn!v+vw= zu{G>4YY7ry8@>L6mEF{L=Y8HQM3b52l@~YG%}g?Jz1}=)D9LM0mv+;2p%!1 zTc8yUGqg<)a)BWNnbu(Ujfd1lmFg860&3&l9){vwW|paXL$#MYeXlf-{O<%HuTjt zTK66)V(BZ=43xSEdg%i#vbn6rC}nGD6-uff&O391#V~u;nIkkZR?hjs(}tEg^R`eV z5UiuZ=R#JBzQt%9E+t#h)uj*9-PKnv@}Z7lO+y)<}jGWSrYV7 zZfD6-~rjNu)F z@36%;)LrDb)UY2Cg>p6Ys3OzUy@hO%Nq^F_B8$F&VfM2Mp?h9=r{7JZMJjAkloGCX zw5z)u+_Dba2axiD!9Ic}wN;}DN0Zg#l+*ydLk3S`9x39`=nfkpQBGa$(1vc*fng+^ zXDUun=A!A;KKNILFqJ9OgWAqQz&(bxcUCBKmFayJ5h`;{AD%8ttQr=$eSlr;A`WK| zH&iH(;GQvdE}p8uIC^oW&q6d-%K@_wgj0}l-sF&68Dz=PI}a^$RfqI9Ra0erVkEDX z^U&Qhv$-fw7J2&wLj+~d(gdJ8q}PQ+dOym71uRDsJs8`0N4S;jul&PkNr=p{P6*`0 z(I~qi45x*O2>aQ|bIIa4aC&NxGOoIqU^{Clgh|@rwv-~xv8W!0>b7}^3w4abN~1|d60#$Br3 z-P)HS3zkq6O5MvPOZZaeq;y^|O9f}v>7AWz+8FR*KEiOrETh-p6bMyXOo1x@2XX#1 z$k1CvqbI!Df%Nhhg4cR+vzJ?t<^|^mD8~==(sgnux8VZ4GNssWe|@N6tDo3>{KB(e zd&R86VGn~mrb#8Z>H|W|utz8tM<)8I|6Fp8JE^d_B$c4oL3AODOc>~Jf}XF*;v2yf z)G?lxQG-!lfZ%R}!|m{j*uv;Bl_PyFP*Scz4M}mZ zhWvMg(~en$D-cE{ibOw#x-=ZKiEzTTLjOJShTXKSVw3_l0wGS3rM%0WD$aLXB|gzW zZX@j`oa1VhJow-$E6Hh2|7dU5_em(p9b4j8-K577nmzUwdOUrlU4zjZR#lQnts_QA zw>%c?JDu+8AlU)TdZw(&!Y>?4-YsI^rb``ZtwD!f%RHaOaDvyiPS;A52xLkqo+ig$ zK2*~r^*b4HO1=^|3EC2IRiy9~dB{CMIyxa^kf6;W%@mBNoMkSKvcn5;8(mlB?C?%Y z=@mt|-S}mPooBo4&zJAulAU6 zvJ#z`RySWtdQeQ#u>FyvlEw9<74;~_iMvS#*F}IwxDK(PGf9Llzy+>ApR4EA@kRWx zg1wv8VTn6n=zPMxb=cXI1aQngT!)Q4eh8l-?I*U5r?1192hl5Ufr9t%UtyjdDeV%; zf?P)Gktj^YplwD-QT&(U$ZA%fF-!b%e7GvQc6Ndb5$y zk7cr>u*0>~2BrPY@yhW^>Xk1jWY3#@U6q4HemXKA zxO)=1+?BX-NfFthV{v!=8w>_ZoY^YnqyVUqMWve=fkg4AGHA=l0kxs|GhLn;op(@J z&eVnDj)@JfKg$zORhT%(>UldUr=3psx%@d>2gVJM#x95%k$=#qM|mO?7ff;XI_!@k ziWiL!6xZ9oWZ1oORYNDBED!G7I_x}DgoxCXLGBu8bk4wsG1D9lAk%o8p#sj{-r3x| zJkpiuV-*VARC-xXcyb z3CQnAmeblfe9T2CfJ>{!?l&)IVV_drKIGld(poiDSg@O*^@W3=^Kbo{%p-UXBcz51 zTcJx_#k$0x_oCKZBpoL-Syoy5S@sP%?bAlyUVcOo@b<3J%SvWSnU%-pDvTAnU+)U5 zg#(fhjoXRW1sNEB_Om+#vw6K6HS3{AHs5_tkSSM<(u)st6 z`u-;Nj6PSL)lx@#ik3jTMmgD)ar~lWhJ#8#Mu)kp?y*M9HV=w6bSw>8hnaGgpYkrT z5Mqr1+^sT!(l`mXvZ1Inyh}}@b#h?t1jmBhKX}znX4kn*lNP$peRa7nt=&f-X~Y?dc@M)tn6-WG z=v@@#1W`=WGk}Gmmp_3*Ml7O+)FXduObB6;81GJ(fs`v^GjBU<4J$%WkE;sW$XSee zOpz#|+h|JW@J?z~jv!_5%L35@9`_)dz7a-qNUpAoBr_snXqFcAVPh|6(~Y{&U`RV*L9JAuKYa zK&~bs04_*82}FU|;x2JhN4VR12_oKlg}TK2_M3`u+0|ykRb0FXjpobPzSy$GJ!K=5!BsTo7DurVTc*^ZVs>pA zC1-inc`+vBCudlwh$jt6qzM{G1-@Avk17XgZt_yd)@-g&t+s9PXGFEx#w|TTa!{iU zx8-S%2`7V=!RdKIP-G@~dN@=D(afjw^!B$5Ed-aKeC-A+* z;g%{Quk$y*-7ZT7rU9XFG{?NGhAK)-GlTZUharj|p)W55sdH|<{y;d>>OE_^CVZZ&`7$K^ zQ6X#z8%4^e6mdyRzi@-?#=jTub`TPf)z45ls=@Dr8g1uZrt{R|z5*0GPTYl3){aIg zvOJAw-MXfD|K%Ei6qn$a5OfL%+mQs*?7_2n*CwqUWZE?j!nGifHQPMTQLF{Au7`9< z%`K>dMHViprXJuaaEyWv<9gT5B`}x5xH4Z?kq-%{v+Q_h+816?6rg3R!ggbe&)2`1sKi-~x&{58*2KHdN8Xk=n0%<^Yb)PDgt5+p$NOnqd z;|xvq49RsItSEeaT;&q8^iiHRP2)~Z{`rGfyJ>QC7Z)p1H#R2n%9B+@kPLQ47X zH7kuqij!FfIi%o)1Ihr|&$oi3qe7YG$ee!41GQW#TXiZRF(h{-E4?C|n?erPIJzqumzJ8C=e=o*rIt3T*!tVDUv{T2hFq46I2kNZ;qG>K zI~%XuJ6LqI+&!^dlJXBL1vAq%Q51MIEaDM zE9}BQdYJxy_R)yoAikrj$3U1XFD^LF>6~x;@aJ=yNO^kW@n{p~WsNcyU;W3A&0t`^ zB;(}-VaOd|TF#~s|7P&|pZnzADkciGKShH}V6$3w9anct5xsEB)6aB|2`4I`+2*IV zoxE$sH{RE@7eh&(zuesP3GBYrniXg2{h*`KV*WWLmtDQ^+~%TZ%BzKg5Ezx$5eJHOAO_&B3j> zMD&30pIEgEk8ZrL?tFNmHt5z3i>x(I?Z0-@n9>}Y% zCq*~LfvdxPx%4{S4YMl72-DbwxUw*CtYV+@_M=56mRVX^^$jIOCHZ47r)sZNx!j|oErN4%cE2B^=5Btiy)yQm8{!jbH%jMSA0$z$t<`6r zro9{QVizx!8gu*kw?rSBa`vXb8Vpa9)&6!rDPc-8lo2v#v_gp1xBW!5ahrF(ag>_H zkLy3V7jE_R&aT78pP-i~(bW&(1wfK(*+yiy6GLDK7vSqi0XYpu8hF4kgBBo7qYkaZ zsK$bQu)q|C0DpEJ78J)j3R}~s@;G4vz9MvS!%_fog$d`cT5#TOnppz)UX55=ATj?C z`Y(OYh8o`a#~X%;qR)5!0bJV_-=a25RdWA0knvx-+TS&55v1{BIAo5xg?jZ#gS_qf z1Gh)dSvyCf`4gK8|66Z#9{23-AJiR`LnlHH8a*k>-xjrDvrW^tgEtZWFb+C|w7HNf z3T>)h?F4Kg^+V%>RJV3xmo%OcHz+yd%&gO$(c3e}iy}Qt^?Y{jRmLB>z&dZ`umvYQ zD_SB+3NQ01HqA`Yuq`#2vva!Do>&gOd($i1?xD ztrYKcraR`iX)okNFN-Gi?0DfI>*Hb=`$qp;(V4Jj)g(=hcvW z-gaNAZB8oE-*j4RWyH+wn08BjPIBtp6vIv{&}jtu(AF@HiQGHoR7VxZ$yl@&nO1oO zaJ6ff#S@MUHM^bDK4DBod7VCFpI>vh;{KtVAQqIx=Ac*RL+z$MEwIThp1dYOPMk&m z4R6177(T0wQ96?-^mtaG)!W#-d_+vmP1@sSOifvzVfqa7MrNe1*lw2{A2()xg>ASt zZVdRvw(}&u&~Hy|NKpAddT956e9Y-}m^6`z1u3_`;qsHKz(z|1rdiDNKiFLli4zwP zz-TMB%Kk@R?f+pH5;`=(7P9TfYty~*N?9w?Drzp<(Gf+jnKyQD<<;yp?jM`WOOsvL z&;wxd|H~T|I+PCScJZ89KSWAdoKKS}Vt1!mxz4w|w3p_X$Ezodl``T{>AsF273lQ+ z_Cp__S^keb451X>RZdhcs63RlS1n@I_c5ybzGsUuA~Ykj-F93xIb(s-?5IJRwXvRG zJ#ar~o5qIuWg2;(pwrr52WPYdkXwRU7fMq$E+YzgvCBYVy0|QAG{5)j=W9c~wmhxM z$JH!+mqKi<-Zay_Wi&8<1ovw2ZpPz#ktxc1q>9*vr!JT)*8+xo`gA zq=|{7ty%;w-a?%9vrHS2ZTgauooGF`j8a9!*n(9|bz|&yJxS1mwqyu8uzS22n+Yf` zh_pGM1xE*lrk>+*G*Dd^4*d0gl*7a)+6G(NP=BO|bym2HBuiA8esptC=q}VLg6|>T zau?6u(!Lmog)8l_5Q|va?e&KEBg^N7qfW2WgT%4X$d_*{tvw|J2qqtUy6#Tp#70uo z0{5j$VyE7GG}AFQoUHfA3u_Bf0(t}P9+SM=Zkn`Ru&;NbW1{bcZSEkI&fZgh&m*JW zO=)o8vG@COGoN$6aFavHON<)as^Snh@MteHiSrepg@NE06qo*EXv#n45l_M;W3oFz zd$}i$_b2s&%I(=h0g>6SNdmFn{(ZX*e|;O6J?~`OmWtV98!sE8V?}nzIog)Kc5&gEI(=_?U+MvxuH zw`W$M$f*l1OjfRC6cWP=;Zg(mq}m~AP^RS@oC&!A7hXY3%&dkdkv=5sTH%-h`LgM| zSiQ$Pf8fvaI(0(fHSMFh=-}MiIZaO^B(3c7y(OerOwA>_xq4Vp zaPrP0mtIxJB_H|t@iJYvEkk~)BSV=!hh`Q9>By2c25m-Qp$04hY)!gh9vKSeodj|v zPJ+nV5KCNW1pjat1;XY7tUy*>gt~mkM?xTj86oj^$2>wqFDSt2H({76I3%=_*k)J5 zfF9l?@zrSjfCT=AQ6hx@j$Yjlhdi;-*o89&n}L7&G7!=L36ze3p6ve@Uj$#A!!8KD z1kv=!32@E3e;XgMZ5t#CDmY9>KqH4I?r7HTyLEiA3VSeXTLCBSIF9ZqCAP|fm^SPg ztw#|46Et^1!N3huX}*O_vvXGV!@j0CpajjxDNXhM8p4lQ*i96;-=sC zl<@c56X3p`|1>}YopspXKSBZx-1xsa!b;cwV2Bp0ih5x@Epi=t^$=nsk$a3l1SeXQ z5$}wMm+H_@lQdaH_+l!pMBlN5Jip z&21DsHTp{8nP3q>osj$YMkq^NFcKf4>~1$=UlPTzD{cM%Bh&kp}d>RU?GFX^Q+Gk8H%7eRxdl|P;g zM2h$!zdi%TG9j~qtjsp0{`~j13|Npf+~2+S|I7OsO!fQ!nGNkgT|D)Vm}3$BpO|IF z-!R9JUnnrpz#v`L^azMz%0Y=izs3l7X>Xq3h zB?lhvfRUi=2A@t5LG)H{?cU9}r=6Uygp3{x0#{w^KKC$aJ|U zbaSOk8Tk4mNm*w0ngP8{=t{)B<5t`1Yb^qd2rDNnxv32^Ypa|8*Ph~8f;};E4nk(~PIdW>BRUkv!#}5TQYg z_VXVsp||XuwC&Df3QHbov+n!hze6luVrr^+IV(z4xZ|O8$DoPU`%B+9i7>TPS%@~S@K`e<9O_0LVnr#@=>E@2`r?B> z_S&8sf^**}D>OB|Xgd?7vAX##Jy$MM&GfM0!+75v+w+7T)6@DtM7e*{ct7wo}!4Fdt0;GO`f`cRCuiab?>=P3=J;a_xS&}p#P8rlj?hSI-TY(a3^~QbX$UMQ|!%PR%1$B)}OvBMj^uzd{U`(HcPqTvLt|bZI1O z(;O7LeWTE7P+Wss( z&VA@)C)oMI&_?B1QJU941GVjH+1J*q#g0ivaDG(q*Y28IL=5fp7({g>UCBsr{z7m# zgn754H`F=e-F_95SGJ!TK0$;=jVRs4iS`eGs5MS>(MJxQg`9#gVaKqPM#izYDhu(APE!aeQbP+vufKNhd{*5KNmrdfsq!a3apHJg6>V zg0GG0{tep84szPRe%B%j^+_7DYu#%gBo{6Zjyh#)*!LS(Z``zRMYlWsprt zPaOPFiVWODqHlLvPOk6ZPX++#OPo4EgwX6E5ZNejL5z~v2j>21B)#%W060DbAmSN6 z)+Qj2k{X|ZLo@||K+_HI%RZhO0MbWmKzd~v6K(>DzBmSYT4CBBM&-hpXuM@*n|RsTCU-cn(BF#~^1A(rsG$3vBXOs2*I+6T#hsPbh&CS%_*PZxp^rJTAEA zLR^s=vjbblhRUfy@1#z=4QfY2u~;eeN^Yv)Fmmq_lMRaAEY@Lf-u?67z@?jR5dV2> z#Q%6`K>CjavtY?-{5=bDf18E4-?M;b9p)nMfbu~)8Z6%$D(_1RPiG9v>qh*` zLFs#eBN#_U+y4FkiTH2a$$PN+f3^Iq0k_=(W_Ro}ni*jLVyd=3_-SaaJmn-1#q5lD z=NuGfX$&ZgLt2OF82$6z|L>#u-$(O*^sX^L|0rh`svZBtJI_ydd?|9-hU$iU*47Y~ za`_zhep!lz($`;s?#?WUJVIt;g)-PPDD&&sV``(Mc(BY63byLdQ_Jy^Unr+7Z z4s|pAyu@02Js;aRB29*1&|e@BcbV1xRNMLjz)TXwf zfj%ZU%sT{e;kOp6y8e3m*|~+Zm*SSw0Q(Ged)#ikI7G=0yh$%9 z_gmY-6>siF!=UC1v+s;WY0@KdX|8!W8q<$=O0?i^T$eVzmKD0`<) z-78>Rryd}#ZmJ%m_^3Js~%l2p=TYBaAsWd`r>G@dA0s2(T%e@r!KZJqdAgmU*D(CMf z*q;o@-^(k&itorx5n#&O)Zz(4qO*_a7j+*(Jxw&1BoKp;*va-*gcR+p4FDXb3vU0xTb~(OyT0zCf zPiH(dL-AMuuf3dFl#`%7?b;dja#i_(4Ug1Hk%L-l8C$3~C3B1(qY&J)OHaB$QS31S zhO2$C7z)e8i~i9;y}1?>-Z7bI5H}o2jyS7ZLkyxBPZdI9cwu&SoP!IZs6(+hywrrY8*~f*`$q$G2K^Z%8b(&XRi(2{<%mKzuO zxd%{+duN^Uqjp#wv&qJ)^Q3lHqYlyojPrn%)C zF!n&tCA@m8n~Fp+S5C*?&HVWB)J53FFXl9?KnzI2AQ#c5bS@!sh+Qf((YYv^l zE8-`M+xuKXz3XD_h;Y|TnzEa#Eo)e?B$%6Xs(|?)051t`QA4jxkd}Xd6pwEpX`p&} zntx&*3^yfw9TphFecw{B_DbkAg12C?HbaaO2Y>SQ;zZqs&Xg}Zx34a~`%q#rwkZpC zal;>5z{UQ>j}079rGAo|bOqAh)=scv+FMj(Qnn`^L+wIV_nQS=?)L62R7sGPnfJjy zwbe?_ws-H2;=eIq9C@BG*mT#Lnd6x}WRU1Z%QJVkdeDJMq#N#7hH@7l6uIR6@@RT9 zAAM-PFIQ2b$rXi4SH+IvruSUNdGsAyG8)$(D&j;mE{U`o=)xaSMaKC_MaXbW+z?Np zfdA4!G`HtTg|i1&83V}$j-Yqr-301((2cf!y0564baX1S(#gIH=bh|oXY7_~!}*4oFeq=rOPL%4K29{kJ&g|FW4*U{R+I~fP77Roe^Oi zP=9+Md4}y9l!i~LV#sR5Y?}7<;+b$jWze6Trp4?@X-xIa4kt6|7PHtr8E%b2(S&3J zje=)(wokABn%%9Ku-%64(&ANJPIbh-W?;f2yB3Kv^_4#UAJ!K2UYt~7P*U$S!WA-Z z3O>y)>-oBT!3WW5$4|HIbWr3>8G1)))W!_2O&-|Yd*+hE>5*;pXW0*%<({3~@>tb3 z+UaeNpoQDR-(?^JHUbFK*I`Mzuz6}^x2zUoJLRZUg#~l+FhYcP`{p@L$b$IUjnBsG;Lu`|A!~_9`kv_=T_q1$Squ69F%9WZw4^0@so6=z){JcLPw8#n5DM=q*=o zScge%T2n&Up(FU_#IaCF9-PC<%Od*guvp8dfFZlY5~mmu7R_HU0wW|3&)}c?ipVSC z{S0~^=&5`4?j&WxC!7_vBg24L!wm0}1tl<~b(>Mh(L(4z*iZ#2H8+7gR5 z`fAeN@9u!C=okxj;9gM`yNcQxd+ki$tvBCpo@G^@=`%jC(zY;Zc`NO$EO28BPdLEa z54Q63t|ta1-)TAc%vh5?-=cgcW$xFnDdRjgSrL~QP+mIr<;5e)8L>ZLjJG0wH7UnA z%a48+t&nT{bBiOH{Jrg_gQl0tR7Ov4c6uZ4{;Ns(^Y)#)>*PBwH15AJ_f*S()z~|esH3GOW`Z5Z>>Dn-pTxXKG#Qh8W42R4igi-c(J)G5RQIKVYg*BC zdEnu^{RvPcT3ITMiFCt^jgswsic2!g=5pEkG68fR_{t~15Xj#}-9zLMSup}r zG6+KoPM{Z5ssO1JgA1+@9@qBR-Iyja0U2L836C;Ridjhtl0$r;vVzxPCEi5mVd8`x z__bOI!OJ8%(21?ipe-|?nn&&nTy(#~_l!ionYY2`QJyMpoA^gZ?_IX1^b6sS8t z%~@}?zL=I})p+-0Zg8V3nTV|N()596F;~(^cDf#Bi$B_v3HG<%+|%r{xEkOeZ@-j6 zZp41!?*(R=Qv@?EHHPX+eaeMMHL?f0k9!75)@w#5fAp}8Pj_);vkvH^e!Q*#mc9eCeCR=_R|xsJ$$Nl935J{n7XnC6vQPRK00$ z7yXwR*5PXUR@SQiooMM-J=@KNw@1fC-Wv}^B<>;?Ni>%xTv;XUyapjmgO_wl`O&yk ze!+E)NEUbFG@Vvkg5E0F+mC-;o6eOTXBG7X=hB}mhW!jBhI=*T)OiLo=$S?`eIG{UA)=Y`ILpb{&7j7u}WktaL1CEj+3>mF_VeD}$M&5VgaFqC$@r3jyg{a0{wx(Fbec!Ik?CRMCOjkkPsihUms*$;w zH!0E$dbE=7SPK zjCWX){}%0h@-JeW!SVY#LWoVM-D>c=0n&h%P=CH)q1U8tVpoeS*m;HT7E->X?=~2;f3PtIK2V>7&w5!>F>9O&ShkN~7w;)4-Zfa2hl)D?n zAY%K_?+=@Gg}sHxpgk-kA&ZNUSj+;>r=_<<)M9bV0v>D*B7ua-e6C zJPW^BK^a*Y2cjMJCy-hnZgos~>xKZ9vC3I#}2#{A3eJi}Ru_pcw|0jN|e^%%uOq zXB~EEdUTxHB0zA^ibT&WZGvN=JQtX09Kvdelk=EUtMhfm%JV75Kxtr=!aD3k=MZDz zR}RslhIBQyo^Q5LCd*zZE-~l;FmePJ4 z+YY$dlQ1)!k`B)0v5s?}@a;3#j^3(mH46f&dAwOLrhIjkD%g?5*KcV*r7SR%B&}_s z3a-SgkUEIG zH-RN~^(}Z3z#Io%W$dC(yu$f@}FZdvZ4^J-J#ys_2sX%3Uz3aEZ+S}6n3_jm! zx3TLR7_h7IQmC$x=Nir)?!-087+}8VD(3qQvWxlWW;)F5HRejnrYJ5W3K=xSQ(r2_ z936@-FfUjWhe&jaawm$&U9OTnRNh69<6vqXB8>-GTdE?mz%+V>WI3aVR&$w#s>_;E4&iBswyG3C`@by?nJ^JDJE@NFEM zQFVgz5PR*n(DIs;NZr;M69am_g$DfKS<3;5+vTyxvWNi)9WlQ{N9X5(>#)87ORxDn zv67{Zg7UFiY(3v{W6+nL`RA)oFK$%~r~bUg72-eA#PSb*dAk$)uTf>fC)dN2GnWvM z>w)>?)_04vX~2GP;N+efX_2p6AIN(>iI99bmS}z9z0vco3qo<*cvf8d_wmG8i!D6$ z*T%Yp=To3dD9G|)25xl%{PxN{mAW}EcuxEGUb-r12y$nW4>3FSdf0zvdzva z(DtWpp_CIOuXmSn2>2zeB|!0%%cyprg#PD6DS-<*o#^4!*s#3fnU{EHZV~6T^4*}d zVt#cQ;1m4M`RRmaUYXx~&#yI7z*}q~yra`)dhQg8yKw4VJ*cm;qq03mDO7{iu&kA{ zYaZX=wZiIe)?qag?-qa^BcalcF+WDtVD;y;D_ap%wDab`3Azr;T^v~Dg)=tuvb(TP zEQ+ba;r<4ZWI-!1B+P0y$(eM6yq3+aXogkCtfr(~f?%(T4&eu@BJ`%QZ~WznO`ZzI zv_04`{QJcE6K0Fwb=f>A0ix&iDs~I;Tb<~5h#T+YY4UeL3OE^gdLXMVTt$RTU5UJ4 z>=Q(W5KoyHMqD_A2PMiTN9%Pk|VBTURsUTe40(G&@{le*g$f`T3uoh+Y5VVDMi+ zUJAq~g#j+DItpm{51PomF~pqYZ!iqfPy#J7ncBPrcJHZ_Rbq|Po1Y};bILX;10K&F7ykoZrLgjd_*WoDAG`iJ_|Uo7=6d0W z9+PPJ++wnIV+})8@iTX8Wk%x?8SZ))L{BxE!&!*U$!b!jTG_crcjQAc7ti{pq5N=V-WAE+nIw~RLhRSx@8Wtm7aPjL*r>W&`w8YYQG`<1!a>HL6(BPSa2^xg+RO0K6k+U!rHCel#CU=dLLDnRYYhvu(8#BFb}juw8fXT z{Snw1wG#ejtMUNlOrL3L)^Z7NclDowcC=_u%!>R>z_`HB>=a*_(;|lcX$<0?&tU>) zGRV0uG;j0!eo_?_A{2n3O#jqEe^lb_*z&VGn>+KULGEyUlLKtNEpyj^@)?^QEx-_g%^S{sxihj0#2)T2B)jCA$GcrH*{j zr58OF+P68=(&U<4mnuZT`Iu^IZ?-K6oTYgZ-6$U>TVks0<~Y-*I`4R!1?3og9+CO3z)zrso@JP+E45G@Wz zh6Tw+t=`FIE_9*ue|{q9hZ0T+gc%dyp%QU^98fszgXiqJ1A5WRXh?!krgcBtU9sQu*$%W)hl@<;mbKw%e zF6HMIH5F`?-$<&$2~9XiaTG)I7S9q1zXorq^LT z>0ksWa}LX@&j#|4sRuDzzPx(!8Wb=!2VG7rc25C+T=yT$ED0{%jkHhk{#MV9bh}d z>rOb!kM_p>sH4fsKykw@!n~{eHz!gWqv6+42oFX#v!Q5STmm0-*@VAm{T_s#~v zp;RO2BrbDewF4h~j!SylbFsK>F3+29XH;5hSKOhItk&Gq#$WiX_CooWb;O4$%M8Q9 zD=$l)BFh+&SSFgjm{4`(Xg`w?6>m&rDzq~i&oXoWRG-~5TKw#dNTu>1 z6Rd@t0lgl9#NQYaxWYe>q;=TZ#+nDyGfIS(;2?0}dICQ-`o2gK*Cx0F4N_0L)r@Z3 z>*x^s2cS>}K%w@Hx_Ga~Xng-Mp!<}9)%x9iXwC72GFBhn{qZ+U?10g^+Puh=gX^$Q zN3z#pX%hQ3M1uZn#>K&0F~5<{^@;U}%($i#6>DV64s&HRmbRFrG~07|Akv zV4e*$9fQJB30xgkds^V&zn|aV*Hb>5J?+Q$gKMm&RLpnq1c>zgktM7pJ~0=Ut;4`J zktc~5O2(+)Y|0u%)%Mq2)PeWW7U^TTaotocR-IU4uI6g4%vHZ+dn&nxk$-FA7~ANUmHq0KoA0&?F|2F$?Fab=T3QOTunigxs>OXzQ`iR?Fmyq)<$g&orza z+M_D3Cx=muoWG7?$|_^mL^lP3mk?SHRz4eIQ!N)owo;ekkASK|?*L-Yk2}PR2oAa8 zG|*o#b)vdOotMb#EPES6=a=5or8mUeeJ#H_ zDIIM-b?sbMbCdbLba~d|SagG}=L6+syEk!lj)#t&u}R$X#?D+VTe3)@!a1PGCAF=; z0IfklrO31a84|gZ0IP&5*R&eBtCP*O*fwCe=!GRi1hb@$z#k20wz(z9{!NsdKez zqP_g(U`%@?qAy|iCSr3TV>3Q-={ZM&j(FM))RIEB@U-AaD84X*ON~0n`7Z5{OFsL- z2+_MR&OK6`ApOjDPyNq6&52?|Wl7B{%MDRuS(=aO)g+CyxOB9ygy)gjC2MxW_|21$ zfzA1mqvpQP?5qwwFsawRxg6Is76{%%U=Lw&?Ji8aELeCo0@D8k`d4>u7^g}Ys1lH+ph=Cv5F7!S zUmb@YjM%)7(7#hNWQ+hV+xLfaYF@v73WFJgVrB=naQQuL5dgtsAngcTHvi<#0ZJ1- zV;%NFNgaeG9wj3^GtahxRqo8QYQ~QX_PIP(LvJ(>oX3{j8|TGV08ZhA0@9K-?@EwQ zbuMZSJ5oD61pO&2sA%c-7kHyD{aawzfY-JLF;Ym7RCiI*w5PgpJ5M5w;jSC@G%%6FgFMqCY_IaPn^t;jWdd;)7`N1uIn#&An$!t=CG)A5$qCWAD&QQ4U{ z15Y{^8}pFNwIa{@%#-$h7fX5-692FEuKXYBt&e|Yy=6(1CCja3Nm+V&lrV~0!oBvT zE|O&$%$RY_$QF}IGNB@C%2LeOcUllh*|I!B#%PF?#9)lwxW}Zz)BOvc=lgoi_dDl@ z_xqgB`+Uy&{ho7%^P7quap!y@tHt&&skQ61q1>lN)n01vBmbvURl+o5@4X0^>)x1? zu80Jk$(DbWw`ZA?ljvt_;pAvz%tqCVgIh*{qC1;&MZ&n3jH?T%$8gwA~= z@KY4*zzouPFlk~Kiyfcpg^Y{`>ZQuh3BG&wXd4FUXJ;RPE~8FpCxq-wOscp~q~Qjl z-VCJsO}981^iYZT*f}EIuA?PkVHwH=dmJS^o=)^~Xwl67sI^2Nc{AUZMc+al9Hna? zz@swCE}kg{S_6lp{)G_iYpW+5!eKHJi| zg83SJ-6pyu2KFZ0Q)j}Sn=&@}L^JsbJL8`Rkmu;pYvLd1LSJVE2N z(fG-6*P~_)Jds0=UEBI{(uxN)^ca)_)!j+L))kzLis4q7H9Vu50|^j>-NGDn2A*oH{a%0L#Np z@BQ%lOwY%r!%V4(i3iNkvYKa$JYafZP6;gZ5|dlvr7JITgG{UonO}$pqYtSqv>ea} z;KBFhX^U0KEUL|3P_35@@PT0IvP01xB zep0V6uwHxILRpg82KJ!6TDjS+l4sm2A~x^sEMaKIrY+aY{4bDqkc{0V3SW~9g6<-uAu|<+poPvD#YP6~Po4Jh{7i|Khk@aI-hq;8l{v>@63Ud= zh$^YfFo8>9I!Dy3)Dsrrq@bLJ*tgWAM>r)jJLu^-BR-#PclR&I_x;{yD32jq>%!F08uRw;2jhYSk_eT5 zR2c-KZ4lw2N1$#83< zd(6pq!RZ~9*3@4|uXKEQs#l2HeJ;h3GmgwH5>X*sWWvm6a!U8?rnp}AC3orjlDw`S z)M~YTCNmfyj>BE4&+TOLe8`?FQ$f|YPbrcZ=cCgwwxQ~BweaB%X^>N2ottgb0q~vP zw~Dn@vpI$U-1`wH1PT4f&DDQ4x)?MLj>g$r_}?m|8R)0G%dW)SzxS-gD2eC9OvnZMqqztKmTu2 z)N+8_3mEpSV|WPU0MR>pI(V;uSM6C1s{W%r+w(FtD*%z=e-Wneezynf0*B+iQL+O6 zOjzgNnXw2YB}i_};v_y6Axn!#hV6)sC^~LRAL7jlUz!28sr;#Tgb1+W>k<$T-=$fb@s6&j!@Lk`v%5F44 zM53{|5ja{3aP##n`JmQcb&^$4f5`cj1vmoh+685f3AtUp3_&YDk#%V)kb)WHX@WBUb;H#~Zi6JM*`IUkp0MUSg J)#)ptzX3(oArSxo literal 0 HcmV?d00001 diff --git a/gtsam/discrete/examples/schedulingExample.cpp b/gtsam/discrete/examples/schedulingExample.cpp new file mode 100644 index 000000000..7bb401d6a --- /dev/null +++ b/gtsam/discrete/examples/schedulingExample.cpp @@ -0,0 +1,344 @@ +/* + * schedulingExample.cpp + * @brief hard scheduling example + * @date March 25, 2011 + * @author Frank Dellaert + */ + +//#define ENABLE_TIMING +#define ADD_NO_CACHING +#define ADD_NO_PRUNING +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace boost::assign; +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +void addStudent(Scheduler& s, size_t i) { + switch (i) { + case 0: + s.addStudent("Michael N", "AI", "Autonomy", "Perception", "Tucker Balch"); + break; + case 1: + s.addStudent("Tucker H", "Controls", "AI", "Perception", "Jim Rehg"); + break; + case 2: + s.addStudent("Jake H", "Controls", "AI", "Perception", "Henrik Christensen"); + break; + case 3: + s.addStudent("Tobias K", "Controls", "AI", "Autonomy", "Mike Stilman"); + break; + case 4: + s.addStudent("Shu J", "Controls", "AI", "HRI", "N/A 1"); + break; + case 5: + s.addStudent("Akansel C", "AI", "Autonomy", "Mechanics", + "Henrik Christensen"); + break; + case 6: + s.addStudent("Tiffany C", "Controls", "N/A 1", "N/A 2", "Charlie Kemp"); + break; + } +} +/* ************************************************************************* */ +Scheduler largeExample(size_t nrStudents = 7) { + string path("/Users/dellaert/borg/gtsam2/gtsam2/discrete/examples/"); + Scheduler s(nrStudents, path + "Doodle.csv"); + + s.addArea("Harvey Lipkin", "Mechanics"); + s.addArea("Wayne Book", "Mechanics"); + s.addArea("Jun Ueda", "Mechanics"); + + // s.addArea("Wayne Book", "Controls"); + s.addArea("Patricio Vela", "Controls"); + s.addArea("Magnus Egerstedt", "Controls"); + s.addArea("Jun Ueda", "Controls"); + + // s.addArea("Frank Dellaert", "Perception"); + s.addArea("Jim Rehg", "Perception"); + s.addArea("Irfan Essa", "Perception"); + s.addArea("Aaron Bobick", "Perception"); + s.addArea("Henrik Christensen", "Perception"); + + s.addArea("Mike Stilman", "AI"); + s.addArea("Henrik Christensen", "AI"); + s.addArea("Frank Dellaert", "AI"); + s.addArea("Ayanna Howard", "AI"); + // s.addArea("Tucker Balch", "AI"); + + s.addArea("Ayanna Howard", "Autonomy"); + // s.addArea("Andrea Thomaz", "Autonomy"); + s.addArea("Charlie Kemp", "Autonomy"); + s.addArea("Tucker Balch", "Autonomy"); + s.addArea("Ron Arkin", "Autonomy"); + + s.addArea("Andrea Thomaz", "HRI"); + s.addArea("Karen Feigh", "HRI"); + s.addArea("Charlie Kemp", "HRI"); + + // Allow students not to take three areas + s.addArea("N/A 1", "N/A 1"); + s.addArea("N/A 2", "N/A 2"); + + // add students + for (size_t i = 0; i < nrStudents; i++) + addStudent(s, i); + + return s; +} + +/* ************************************************************************* */ +void runLargeExample() { + + Scheduler scheduler = largeExample(); + scheduler.print(); + + // BUILD THE GRAPH ! + size_t addMutex = 2; + scheduler.buildGraph(addMutex); + + // Do brute force product and output that to file + if (scheduler.nrStudents() == 1) { // otherwise too slow + DecisionTreeFactor product = scheduler.product(); + product.dot("scheduling-large", false); + } + + // Do exact inference + // SETDEBUG("timing-verbose", true); + SETDEBUG("DiscreteConditional::DiscreteConditional", true); + tic(2, "large"); + DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + toc(2, "large"); + tictoc_finishedIteration(); + tictoc_print(); + scheduler.printAssignment(MPE); +} + +/* ************************************************************************* */ +// Solve a series of relaxed problems for maximum flexibility solution +void solveStaged(size_t addMutex = 2) { + + // super-hack! just count... + bool debug = false; + SETDEBUG("DiscreteConditional::COUNT", true); + SETDEBUG("DiscreteConditional::DiscreteConditional", debug); // progress + + // make a vector with slot availability, initially all 1 + // Reads file to get count :-) + vector slotsAvailable(largeExample(0).nrTimeSlots(), 1.0); + + // now, find optimal value for each student, using relaxed mutex constraints + for (size_t s = 0; s < 7; s++) { + // add all students first time, then drop last one second time, etc... + Scheduler scheduler = largeExample(7 - s); + //scheduler.print(str(boost::format("Scheduler %d") % (7-s))); + + // only allow slots not yet taken + scheduler.setSlotsAvailable(slotsAvailable); + + // BUILD THE GRAPH ! + scheduler.buildGraph(addMutex); + + // Do EXACT INFERENCE + tic_("eliminate"); + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + toc_("eliminate"); + + // find root node + DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + if (debug) + root->print(""/*scheduler.studentName(s)*/); + + // solve root node only + Scheduler::Values values; + size_t bestSlot = root->solve(values); + + // get corresponding count + DiscreteKey dkey = scheduler.studentKey(6 - s); + values[dkey.first] = bestSlot; + size_t count = (*root)(values); + + // remove this slot from consideration + slotsAvailable[bestSlot] = 0.0; + cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(6-s) + % scheduler.slotName(bestSlot) % bestSlot % count << endl; + } + tictoc_print_(); + + // Solution with addMutex = 2: (20 secs) + // TC = Wed 2 (9), count = 96375041778 + // AC = Tue 2 (5), count = 4076088090 + // SJ = Mon 1 (0), count = 29596704 + // TK = Mon 3 (2), count = 755370 + // JH = Wed 4 (11), count = 12000 + // TH = Fri 2 (17), count = 220 + // MN = Fri 1 (16), count = 5 + // + // Mutex does make a difference !! + +} + +/* ************************************************************************* */ +// Sample from solution found above and evaluate cost function +bool NonZero(size_t i) { + return i > 0; +} + +DiscreteBayesNet::shared_ptr createSampler(size_t i, + size_t slot, vector& schedulers) { + Scheduler scheduler = largeExample(0); // todo: wrong nr students + addStudent(scheduler, i); + SETDEBUG("Scheduler::buildGraph", false); + scheduler.addStudentSpecificConstraints(0, slot); + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + // chordal->print(scheduler[i].studentKey(0).name()); // large ! + schedulers.push_back(scheduler); + return chordal; +} + +void sampleSolutions() { + + vector schedulers; + vector samplers(7); + + // Given the time-slots, we can create 7 independent samplers + vector slots; + slots += 16, 17, 11, 2, 0, 5, 9; // given slots + for (size_t i = 0; i < 7; i++) + samplers[i] = createSampler(i, slots[i], schedulers); + + // now, sample schedules + for (size_t n = 0; n < 500; n++) { + vector stats(19, 0); + vector samples; + for (size_t i = 0; i < 7; i++) { + samples.push_back(sample(*samplers[i])); + schedulers[i].accumulateStats(samples[i], stats); + } + size_t max = *max_element(stats.begin(), stats.end()); + size_t min = *min_element(stats.begin(), stats.end()); + size_t nz = count_if(stats.begin(), stats.end(), NonZero); + if (nz >= 15 && max <= 2) { + cout << boost::format( + "Sampled schedule %d, min = %d, nz = %d, max = %d\n") % (n + 1) % min + % nz % max; + for (size_t i = 0; i < 7; i++) { + cout << schedulers[i].studentName(0) << " : " << schedulers[i].slotName( + slots[i]) << endl; + schedulers[i].printSpecial(samples[i]); + } + } + } + // Output was + // Sampled schedule 359, min = 0, nz = 15, max = 2 + // Michael N : Fri 9:00-10.30 + // Michael N AI: Frank Dellaert + // Michael N Autonomy: Charlie Kemp + // Michael N Perception: Henrik Christensen + // + // Tucker H : Fri 10:30-12:00 + // Tucker H AI: Ayanna Howard + // Tucker H Controls: Patricio Vela + // Tucker H Perception: Irfan Essa + // + // Jake H : Wed 3:00-4:30 + // Jake H AI: Mike Stilman + // Jake H Controls: Magnus Egerstedt + // Jake H Perception: Jim Rehg + // + // Tobias K : Mon 1:30-3:00 + // Tobias K AI: Ayanna Howard + // Tobias K Autonomy: Charlie Kemp + // Tobias K Controls: Magnus Egerstedt + // + // Shu J : Mon 9:00-10.30 + // Shu J AI: Mike Stilman + // Shu J Controls: Jun Ueda + // Shu J HRI: Andrea Thomaz + // + // Akansel C : Tue 10:30-12:00 + // Akansel C AI: Frank Dellaert + // Akansel C Autonomy: Tucker Balch + // Akansel C Mechanics: Harvey Lipkin + // + // Tiffany C : Wed 10:30-12:00 + // Tiffany C Controls: Patricio Vela + // Tiffany C N/A 1: N/A 1 + // Tiffany C N/A 2: N/A 2 + +} + +/* ************************************************************************* */ +void accomodateStudent() { + + // super-hack! just count... + bool debug = false; + // SETDEBUG("DiscreteConditional::COUNT",true); + SETDEBUG("DiscreteConditional::DiscreteConditional", debug); // progress + + Scheduler scheduler = largeExample(0); + // scheduler.addStudent("Victor E", "Autonomy", "HRI", "AI", + // "Henrik Christensen"); + scheduler.addStudent("Carlos N", "Perception", "AI", "Autonomy", + "Henrik Christensen"); + scheduler.print("scheduler"); + + // rule out all occupied slots + vector slots; + slots += 16, 17, 11, 2, 0, 5, 9, 14; + vector slotsAvailable(scheduler.nrTimeSlots(), 1.0); + BOOST_FOREACH(size_t s, slots) + slotsAvailable[s] = 0; + scheduler.setSlotsAvailable(slotsAvailable); + + // BUILD THE GRAPH ! + scheduler.buildGraph(1); + + // Do EXACT INFERENCE + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + + // find root node + DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + if (debug) + root->print(""/*scheduler.studentName(s)*/); + // GTSAM_PRINT(*chordal); + + // solve root node only + Scheduler::Values values; + size_t bestSlot = root->solve(values); + + // get corresponding count + DiscreteKey dkey = scheduler.studentKey(0); + values[dkey.first] = bestSlot; + size_t count = (*root)(values); + cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(0) + % scheduler.slotName(bestSlot) % bestSlot % count << endl; + + // sample schedules + for (size_t n = 0; n < 10; n++) { + Scheduler::sharedValues sample0 = sample(*chordal); + scheduler.printAssignment(sample0); + } +} + +/* ************************************************************************* */ +int main() { + runLargeExample(); + solveStaged(3); +// sampleSolutions(); + // accomodateStudent(); + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam/discrete/examples/schedulingQuals12.cpp b/gtsam/discrete/examples/schedulingQuals12.cpp new file mode 100644 index 000000000..230218997 --- /dev/null +++ b/gtsam/discrete/examples/schedulingQuals12.cpp @@ -0,0 +1,264 @@ +/* + * schedulingExample.cpp + * @brief hard scheduling example + * @date March 25, 2011 + * @author Frank Dellaert + */ + +#define ENABLE_TIMING +#define ADD_NO_CACHING +#define ADD_NO_PRUNING +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace boost::assign; +using namespace std; +using namespace gtsam; + +size_t NRSTUDENTS = 9; + +bool NonZero(size_t i) { + return i > 0; +} + +/* ************************************************************************* */ +void addStudent(Scheduler& s, size_t i) { + switch (i) { + case 0: + s.addStudent("Pan, Yunpeng", "Controls", "Perception", "Mechanics", "Eric Johnson"); + break; + case 1: + s.addStudent("Sawhney, Rahul", "Controls", "AI", "Perception", "Henrik Christensen"); + break; + case 2: + s.addStudent("Akgun, Baris", "Controls", "AI", "HRI", "Andrea Thomaz"); + break; + case 3: + s.addStudent("Jiang, Shu", "Controls", "AI", "Perception", "Ron Arkin"); + break; + case 4: + s.addStudent("Grice, Phillip", "Controls", "Perception", "HRI", "Charlie Kemp"); + break; + case 5: + s.addStudent("Huaman, Ana", "Controls", "AI", "Perception", "Mike Stilman"); + break; + case 6: + s.addStudent("Levihn, Martin", "AI", "Autonomy", "Perception", "Mike Stilman"); + break; + case 7: + s.addStudent("Nieto, Carlos", "AI", "Autonomy", "Perception", "Henrik Christensen"); + break; + case 8: + s.addStudent("Robinette, Paul", "Controls", "AI", "HRI", "Ayanna Howard"); + break; + } +} + +/* ************************************************************************* */ +Scheduler largeExample(size_t nrStudents = NRSTUDENTS) { + string path("/Users/dellaert/borg/gtsam2/gtsam2/discrete/examples/"); + Scheduler s(nrStudents, path + "Doodle2012.csv"); + + s.addArea("Harvey Lipkin", "Mechanics"); + s.addArea("Jun Ueda", "Mechanics"); + + s.addArea("Patricio Vela", "Controls"); + s.addArea("Magnus Egerstedt", "Controls"); + s.addArea("Jun Ueda", "Controls"); + s.addArea("Panos Tsiotras", "Controls"); + s.addArea("Fumin Zhang", "Controls"); + + s.addArea("Henrik Christensen", "Perception"); + s.addArea("Aaron Bobick", "Perception"); + + s.addArea("Mike Stilman", "AI"); +// s.addArea("Henrik Christensen", "AI"); + s.addArea("Ayanna Howard", "AI"); + s.addArea("Charles Isbell", "AI"); + s.addArea("Tucker Balch", "AI"); + + s.addArea("Ayanna Howard", "Autonomy"); + s.addArea("Charlie Kemp", "Autonomy"); + s.addArea("Tucker Balch", "Autonomy"); + s.addArea("Ron Arkin", "Autonomy"); + + s.addArea("Andrea Thomaz", "HRI"); + s.addArea("Karen Feigh", "HRI"); + s.addArea("Charlie Kemp", "HRI"); + + // add students + for (size_t i = 0; i < nrStudents; i++) + addStudent(s, i); + + return s; +} + +/* ************************************************************************* */ +void runLargeExample() { + + Scheduler scheduler = largeExample(); + scheduler.print(); + + // BUILD THE GRAPH ! + size_t addMutex = 3; + // SETDEBUG("Scheduler::buildGraph", true); + scheduler.buildGraph(addMutex); + + // Do brute force product and output that to file + if (scheduler.nrStudents() == 1) { // otherwise too slow + DecisionTreeFactor product = scheduler.product(); + product.dot("scheduling-large", false); + } + + // Do exact inference + // SETDEBUG("timing-verbose", true); + SETDEBUG("DiscreteConditional::DiscreteConditional", true); +#define SAMPLE +#ifdef SAMPLE + tic(2, "large"); + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + toc(2, "large"); + tictoc_finishedIteration(); + tictoc_print(); + for (size_t i=0;i<100;i++) { + DiscreteFactor::sharedValues assignment = sample(*chordal); + vector stats(scheduler.nrFaculty()); + scheduler.accumulateStats(assignment, stats); + size_t max = *max_element(stats.begin(), stats.end()); + size_t min = *min_element(stats.begin(), stats.end()); + size_t nz = count_if(stats.begin(), stats.end(), NonZero); +// cout << min << ", " << max << ", " << nz << endl; + if (nz >= 13 && min >=1 && max <= 4) { + cout << "======================================================\n"; + scheduler.printAssignment(assignment); + } + } +#else + tic(2, "large"); + DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + toc(2, "large"); + tictoc_finishedIteration(); + tictoc_print(); + scheduler.printAssignment(MPE); +#endif +} + +/* ************************************************************************* */ +// Solve a series of relaxed problems for maximum flexibility solution +void solveStaged(size_t addMutex = 2) { + + // super-hack! just count... + bool debug = false; + SETDEBUG("DiscreteConditional::COUNT", true); + SETDEBUG("DiscreteConditional::DiscreteConditional", debug); // progress + + // make a vector with slot availability, initially all 1 + // Reads file to get count :-) + vector slotsAvailable(largeExample(0).nrTimeSlots(), 1.0); + + // now, find optimal value for each student, using relaxed mutex constraints + for (size_t s = 0; s < NRSTUDENTS; s++) { + // add all students first time, then drop last one second time, etc... + Scheduler scheduler = largeExample(NRSTUDENTS - s); + //scheduler.print(str(boost::format("Scheduler %d") % (NRSTUDENTS-s))); + + // only allow slots not yet taken + scheduler.setSlotsAvailable(slotsAvailable); + + // BUILD THE GRAPH ! + scheduler.buildGraph(addMutex); + + // Do EXACT INFERENCE + tic_("eliminate"); + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + toc_("eliminate"); + + // find root node + DiscreteConditional::shared_ptr root = *(chordal->rbegin()); + if (debug) + root->print(""/*scheduler.studentName(s)*/); + + // solve root node only + Scheduler::Values values; + size_t bestSlot = root->solve(values); + + // get corresponding count + DiscreteKey dkey = scheduler.studentKey(NRSTUDENTS - 1 - s); + values[dkey.first] = bestSlot; + size_t count = (*root)(values); + + // remove this slot from consideration + slotsAvailable[bestSlot] = 0.0; + cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(NRSTUDENTS-1-s) + % scheduler.slotName(bestSlot) % bestSlot % count << endl; + } + tictoc_print_(); +} + +/* ************************************************************************* */ +// Sample from solution found above and evaluate cost function +DiscreteBayesNet::shared_ptr createSampler(size_t i, + size_t slot, vector& schedulers) { + Scheduler scheduler = largeExample(0); // todo: wrong nr students + addStudent(scheduler, i); + SETDEBUG("Scheduler::buildGraph", false); + scheduler.addStudentSpecificConstraints(0, slot); + DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate(); + // chordal->print(scheduler[i].studentKey(0).name()); // large ! + schedulers.push_back(scheduler); + return chordal; +} + +void sampleSolutions() { + + vector schedulers; + vector samplers(NRSTUDENTS); + + // Given the time-slots, we can create NRSTUDENTS independent samplers + vector slots; + slots += 3, 20, 2, 6, 5, 11, 1, 4; // given slots + for (size_t i = 0; i < NRSTUDENTS; i++) + samplers[i] = createSampler(i, slots[i], schedulers); + + // now, sample schedules + for (size_t n = 0; n < 500; n++) { + vector stats(19, 0); + vector samples; + for (size_t i = 0; i < NRSTUDENTS; i++) { + samples.push_back(sample(*samplers[i])); + schedulers[i].accumulateStats(samples[i], stats); + } + size_t max = *max_element(stats.begin(), stats.end()); + size_t min = *min_element(stats.begin(), stats.end()); + size_t nz = count_if(stats.begin(), stats.end(), NonZero); + if (nz >= 15 && max <= 2) { + cout << boost::format( + "Sampled schedule %d, min = %d, nz = %d, max = %d\n") % (n + 1) % min + % nz % max; + for (size_t i = 0; i < NRSTUDENTS; i++) { + cout << schedulers[i].studentName(0) << " : " << schedulers[i].slotName( + slots[i]) << endl; + schedulers[i].printSpecial(samples[i]); + } + } + } +} + +/* ************************************************************************* */ +int main() { + runLargeExample(); +// solveStaged(3); +// sampleSolutions(); + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam/discrete/examples/small.csv b/gtsam/discrete/examples/small.csv new file mode 100644 index 000000000..144ead08c --- /dev/null +++ b/gtsam/discrete/examples/small.csv @@ -0,0 +1 @@ +,Frank,Harvey,Magnus,Andrea Mon,1,1,1, Wed,1,1,1,1 Fri,,1,1,1 \ No newline at end of file diff --git a/gtsam/discrete/label_traits.h b/gtsam/discrete/label_traits.h new file mode 100644 index 000000000..0a0c39094 --- /dev/null +++ b/gtsam/discrete/label_traits.h @@ -0,0 +1,41 @@ +/* + * label_traits.h + * @brief traits class for labels used in Decision Diagram + * @author Frank Dellaert + * @date Mar 22, 2011 + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Default traits class for label type, http://www.cantrip.org/traits.html + * Override to provide non-default behavior, see example in Index + */ + template + struct label_traits { + /** default = binary label */ + static size_t cardinality(const T&) { + return 2; + } + /** default higher(a,b) = a hasher; + return hasher(a); + } + }; + + /* custom hash function for labels, no need to specialize this */ + template + std::size_t hash_value(const T& a) { + return label_traits::hash_value(a); + } +} diff --git a/gtsam/discrete/parseUAI.cpp b/gtsam/discrete/parseUAI.cpp new file mode 100644 index 000000000..417063da2 --- /dev/null +++ b/gtsam/discrete/parseUAI.cpp @@ -0,0 +1,157 @@ +/* + * parseUAI.cpp + * @brief: parse UAI 2008 format + * @date March 5, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +//#define PARSE +#ifdef PARSE +#include +#include // for parsing +#include // for ref +#include +#include +#include +#include + +#include + +using namespace std; +namespace qi = boost::spirit::qi; + +namespace gtsam { + + /* ************************************************************************* */ + // Keys are the vars of variables connected to a factor + // subclass of Indices with special constructor + struct Keys: public Indices { + Keys() { + } + // Pick correct vars based on indices + Keys(const Indices& vars, const vector& indices) { + BOOST_FOREACH(int i, indices) + push_back(vars[i]); + } + }; + + /* ************************************************************************* */ + // The UAI grammar is defined in a class + // Spirit local variables are used, see + // http://boost-spirit.com/home/2010/01/21/what-are-rule-bound-semantic-actions + /* ************************************************************************* */ + struct Grammar { + + // declare all parsers as instance variables + typedef vector Table; + typedef boost::spirit::istream_iterator It; + qi::rule uai, preamble, type, vars, factors, tables; + qi::rule > keys; + qi::rule > table; + + // Variables filled by preamble parser + size_t nrVars_, nrFactors_; + Indices vars_; + vector factors_; + + // Variables filled by tables parser + vector

tables_; + + // The constructor defines the parser rules (declared below) + // To debug, just say debug(rule) after defining the rule + Grammar() { + using boost::phoenix::val; + using boost::phoenix::ref; + using boost::phoenix::construct; + using namespace boost::spirit::qi; + + //--------------- high level parsers with side-effects :-( ----------------- + + // A uai file consists of preamble followed by tables + uai = preamble >> tables; + + // The preamble defines the variables and factors + // The parser fills in the first set of variables above, + // including the vector of factor "Neighborhoods" + preamble = type >> vars >> int_[ref(nrFactors_) = _1] >> factors; + + // type string, does not seem to matter + type = lit("BAYES") | lit("MARKOV"); + + // vars parses "3 2 2 3" and synthesizes a Keys class, in this case + // containing Indices {v0,2}, {v1,2}, and {v2,3} + vars = int_[ref(nrVars_) = _1] >> (repeat(ref(nrVars_))[int_]) // + [ref(vars_) = construct (_1)]; + + // Parse a list of Neighborhoods and fill factors_ + factors = (repeat(ref(nrFactors_))[keys])// + [ref(factors_) = _1]; + + // The tables parser fills in the tables_ + tables = (repeat(ref(nrFactors_))[table])// + [ref(tables_) = _1]; + + //----------- basic parsers with synthesized attributes :-) ----------------- + + // keys parses strings like "2 1 2", indicating + // a binary factor (2) on variables v1 and v2. + // It returns a Keys class as attribute + keys = int_[_a = _1] >> repeat(_a)[int_] // + [_val = construct (ref(vars_), _1)]; + + // The tables are a list of doubles preceded by a count, e.g. "4 1.0 2.0 3.0 4.0" + // The table parser returns a PotentialTable::Table attribute + table = int_[_a = _1] >> repeat(_a)[double_] // + [_val = construct
(_1)]; + } + + // Add the factors to the graph + void addFactorsToGraph(TypedDiscreteFactorGraph& graph) { + assert(factors_.size()==nrFactors_); + assert(tables_.size()==nrFactors_); + for (size_t i = 0; i < nrFactors_; i++) + graph.add(factors_[i], tables_[i]); + } + + }; + + /* ************************************************************************* */ + bool parseUAI(const std::string& filename, TypedDiscreteFactorGraph& graph) { + + // open file, disable skipping of whitespace + std::ifstream in(filename.c_str()); + if (!in) { + cerr << "Could not open " << filename << endl; + return false; + } + + in.unsetf(std::ios::skipws); + + // wrap istream into iterator + boost::spirit::istream_iterator first(in); + boost::spirit::istream_iterator last; + + // Parse and add factors into the graph + Grammar grammar; + bool success = qi::phrase_parse(first, last, grammar.uai, qi::space); + if (success) grammar.addFactorsToGraph(graph); + + return success; + } +/* ************************************************************************* */ + +}// gtsam +#else + +#include + +namespace gtsam { + +/** Dummy version of function - otherwise, missing symbol */ +bool parseUAI(const std::string& filename, TypedDiscreteFactorGraph& graph) { + return false; +} + +} // \namespace gtsam +#endif diff --git a/gtsam/discrete/parseUAI.h b/gtsam/discrete/parseUAI.h new file mode 100644 index 000000000..070c62e08 --- /dev/null +++ b/gtsam/discrete/parseUAI.h @@ -0,0 +1,22 @@ +/* + * parseUAI.h + * @brief: parse UAI 2008 format + * @date March 5, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +#include +#include + +namespace gtsam { + + /** + * Constructor from file + * For now assumes in .uai format from UAI'08 Probablistic Inference Evaluation + * See http://graphmod.ics.uci.edu/uai08/FileFormat + */ + bool parseUAI(const std::string& filename, + gtsam::TypedDiscreteFactorGraph& graph); + +} // gtsam diff --git a/gtsam/discrete/tests/data/FG/alarm.fg b/gtsam/discrete/tests/data/FG/alarm.fg new file mode 100644 index 000000000..40fbb6f9d --- /dev/null +++ b/gtsam/discrete/tests/data/FG/alarm.fg @@ -0,0 +1,935 @@ +# ALARM network +# from http://compbio.cs.huji.ac.il/Repository/Datasets/alarm/alarm.dsc +37 + +2 +0 5 +2 2 +4 +0 0.9 +1 0.1 +2 0.01 +3 0.99 + +2 +1 4 +3 3 +9 +0 0.95 +1 0.04 +2 0.01 +3 0.04 +4 0.95 +5 0.01 +6 0.01 +7 0.29 +8 0.7 + +2 +2 4 +3 3 +9 +0 0.95 +1 0.04 +2 0.01 +3 0.04 +4 0.95 +5 0.01 +6 0.01 +7 0.04 +8 0.95 + +1 +3 +2 +2 +0 0.2 +1 0.8 + +3 +3 4 5 +2 3 2 +12 +0 0.95 +1 0.01 +2 0.04 +3 0.09 +4 0.01 +5 0.9 +6 0.98 +7 0.05 +8 0.01 +9 0.9 +10 0.01 +11 0.05 + +1 +5 +2 +2 +0 0.05 +1 0.95 + +3 +3 5 6 +2 2 3 +12 +0 0.98 +1 0.5 +2 0.95 +3 0.05 +4 0.01 +5 0.49 +6 0.04 +7 0.9 +8 0.01 +9 0.01 +10 0.01 +11 0.05 + +1 +7 +2 +2 +0 0.05 +1 0.95 + +3 +7 8 34 +2 3 3 +18 +0 0.98 +1 0.98 +2 0.01 +3 0.01 +4 0.01 +5 0.01 +6 0.4 +7 0.01 +8 0.59 +9 0.98 +10 0.01 +11 0.01 +12 0.3 +13 0.01 +14 0.4 +15 0.01 +16 0.3 +17 0.98 + +3 +9 10 34 +3 2 3 +18 +0 0.333 +1 0.333 +2 0.333 +3 0.98 +4 0.01 +5 0.01 +6 0.333 +7 0.333 +8 0.333 +9 0.01 +10 0.98 +11 0.01 +12 0.333 +13 0.333 +14 0.333 +15 0.01 +16 0.01 +17 0.98 + +1 +10 +2 +2 +0 0.1 +1 0.9 + +3 +10 11 34 +2 3 3 +18 +0 0.333 +1 0.98 +2 0.333 +3 0.01 +4 0.333 +5 0.01 +6 0.333 +7 0.01 +8 0.333 +9 0.98 +10 0.333 +11 0.01 +12 0.333 +13 0.01 +14 0.333 +15 0.01 +16 0.333 +17 0.98 + +1 +12 +2 +2 +0 0.1 +1 0.9 + +1 +13 +2 +2 +0 0.01 +1 0.99 + +2 +13 14 +2 3 +6 +0 0.98 +1 0.3 +2 0.01 +3 0.4 +4 0.01 +5 0.3 + +3 +15 30 32 +4 4 3 +48 +0 0.97 +1 0.01 +2 0.01 +3 0.01 +4 0.01 +5 0.97 +6 0.01 +7 0.01 +8 0.01 +9 0.01 +10 0.97 +11 0.01 +12 0.01 +13 0.01 +14 0.01 +15 0.97 +16 0.01 +17 0.97 +18 0.01 +19 0.01 +20 0.97 +21 0.01 +22 0.01 +23 0.01 +24 0.01 +25 0.01 +26 0.97 +27 0.01 +28 0.01 +29 0.01 +30 0.01 +31 0.97 +32 0.01 +33 0.97 +34 0.01 +35 0.01 +36 0.01 +37 0.01 +38 0.97 +39 0.01 +40 0.97 +41 0.01 +42 0.01 +43 0.01 +44 0.01 +45 0.01 +46 0.01 +47 0.97 + +1 +16 +2 +2 +0 0.04 +1 0.96 + +3 +17 24 30 +4 3 4 +48 +0 0.97 +1 0.01 +2 0.01 +3 0.01 +4 0.97 +5 0.01 +6 0.01 +7 0.01 +8 0.97 +9 0.01 +10 0.01 +11 0.01 +12 0.01 +13 0.97 +14 0.01 +15 0.01 +16 0.6 +17 0.38 +18 0.01 +19 0.01 +20 0.01 +21 0.97 +22 0.01 +23 0.01 +24 0.01 +25 0.01 +26 0.97 +27 0.01 +28 0.5 +29 0.48 +30 0.01 +31 0.01 +32 0.01 +33 0.01 +34 0.97 +35 0.01 +36 0.01 +37 0.01 +38 0.01 +39 0.97 +40 0.5 +41 0.48 +42 0.01 +43 0.01 +44 0.01 +45 0.01 +46 0.01 +47 0.97 + +1 +18 +2 +2 +0 0.05 +1 0.95 + +3 +18 19 31 +2 3 4 +19 +0 1 +1 1 +6 0.99 +7 0.95 +8 0.01 +9 0.04 +11 0.01 +12 0.95 +13 0.01 +14 0.04 +15 0.95 +16 0.01 +17 0.04 +18 0.95 +19 0.01 +20 0.04 +21 0.01 +22 0.01 +23 0.98 + +3 +19 20 23 +3 3 2 +18 +0 0.98 +1 0.01 +2 0.98 +3 0.01 +4 0.01 +5 0.01 +6 0.01 +7 0.98 +8 0.01 +9 0.01 +10 0.98 +11 0.69 +12 0.98 +13 0.01 +14 0.3 +15 0.01 +16 0.01 +17 0.01 + +2 +21 22 +3 2 +6 +0 0.01 +1 0.19 +2 0.8 +3 0.05 +4 0.9 +5 0.05 + +1 +22 +2 +2 +0 0.01 +1 0.99 + +3 +22 23 24 +2 2 3 +12 +0 0.1 +1 0.95 +2 0.9 +3 0.05 +4 0.1 +5 0.95 +6 0.9 +7 0.05 +8 0.01 +9 0.05 +10 0.99 +11 0.95 + +1 +24 +3 +3 +0 0.92 +1 0.03 +2 0.05 + +4 +16 24 25 29 +2 3 4 4 +96 +0 0.97 +1 0.97 +2 0.97 +3 0.97 +4 0.97 +5 0.97 +6 0.01 +7 0.01 +8 0.01 +9 0.01 +10 0.01 +11 0.01 +12 0.01 +13 0.01 +14 0.01 +15 0.01 +16 0.01 +17 0.01 +18 0.01 +19 0.01 +20 0.01 +21 0.01 +22 0.01 +23 0.01 +24 0.01 +25 0.01 +26 0.1 +27 0.4 +28 0.01 +29 0.01 +30 0.3 +31 0.97 +32 0.84 +33 0.58 +34 0.29 +35 0.9 +36 0.49 +37 0.01 +38 0.05 +39 0.01 +40 0.3 +41 0.08 +42 0.2 +43 0.01 +44 0.01 +45 0.01 +46 0.4 +47 0.01 +48 0.01 +49 0.01 +50 0.05 +51 0.2 +52 0.01 +53 0.01 +54 0.01 +55 0.01 +56 0.25 +57 0.75 +58 0.01 +59 0.01 +60 0.08 +61 0.97 +62 0.25 +63 0.04 +64 0.08 +65 0.38 +66 0.9 +67 0.01 +68 0.45 +69 0.01 +70 0.9 +71 0.6 +72 0.01 +73 0.01 +74 0.01 +75 0.2 +76 0.01 +77 0.01 +78 0.01 +79 0.01 +80 0.15 +81 0.7 +82 0.01 +83 0.01 +84 0.01 +85 0.01 +86 0.25 +87 0.09 +88 0.01 +89 0.01 +90 0.97 +91 0.97 +92 0.59 +93 0.01 +94 0.97 +95 0.97 + +1 +26 +2 +2 +0 0.1 +1 0.9 + +1 +27 +3 +3 +0 0.05 +1 0.9 +2 0.05 + +2 +27 28 +3 4 +12 +0 0.05 +1 0.05 +2 0.05 +3 0.93 +4 0.01 +5 0.01 +6 0.01 +7 0.93 +8 0.01 +9 0.01 +10 0.01 +11 0.93 + +3 +26 28 29 +2 4 4 +32 +0 0.97 +1 0.97 +2 0.97 +3 0.01 +4 0.97 +5 0.01 +6 0.97 +7 0.01 +8 0.01 +9 0.01 +10 0.01 +11 0.97 +12 0.01 +13 0.01 +14 0.01 +15 0.01 +16 0.01 +17 0.01 +18 0.01 +19 0.01 +20 0.01 +21 0.97 +22 0.01 +23 0.01 +24 0.01 +25 0.01 +26 0.01 +27 0.01 +28 0.01 +29 0.01 +30 0.01 +31 0.97 + +4 +16 24 29 30 +2 3 4 4 +96 +0 0.97 +1 0.97 +2 0.97 +3 0.97 +4 0.97 +5 0.97 +6 0.95 +7 0.01 +8 0.97 +9 0.97 +10 0.95 +11 0.01 +12 0.4 +13 0.01 +14 0.97 +15 0.97 +16 0.5 +17 0.01 +18 0.3 +19 0.01 +20 0.97 +21 0.97 +22 0.3 +23 0.01 +24 0.01 +25 0.01 +26 0.01 +27 0.01 +28 0.01 +29 0.01 +30 0.03 +31 0.97 +32 0.01 +33 0.01 +34 0.03 +35 0.97 +36 0.58 +37 0.01 +38 0.01 +39 0.01 +40 0.48 +41 0.01 +42 0.68 +43 0.01 +44 0.01 +45 0.01 +46 0.68 +47 0.01 +48 0.01 +49 0.01 +50 0.01 +51 0.01 +52 0.01 +53 0.01 +54 0.01 +55 0.01 +56 0.01 +57 0.01 +58 0.01 +59 0.01 +60 0.01 +61 0.97 +62 0.01 +63 0.01 +64 0.01 +65 0.97 +66 0.01 +67 0.01 +68 0.01 +69 0.01 +70 0.01 +71 0.01 +72 0.01 +73 0.01 +74 0.01 +75 0.01 +76 0.01 +77 0.01 +78 0.01 +79 0.01 +80 0.01 +81 0.01 +82 0.01 +83 0.01 +84 0.01 +85 0.01 +86 0.01 +87 0.01 +88 0.01 +89 0.01 +90 0.01 +91 0.97 +92 0.01 +93 0.01 +94 0.01 +95 0.97 + +3 +24 30 31 +3 4 4 +48 +0 0.97 +1 0.97 +2 0.97 +3 0.01 +4 0.01 +5 0.03 +6 0.01 +7 0.01 +8 0.01 +9 0.01 +10 0.01 +11 0.01 +12 0.01 +13 0.01 +14 0.01 +15 0.97 +16 0.97 +17 0.95 +18 0.01 +19 0.01 +20 0.94 +21 0.01 +22 0.01 +23 0.88 +24 0.01 +25 0.01 +26 0.01 +27 0.01 +28 0.01 +29 0.01 +30 0.97 +31 0.97 +32 0.04 +33 0.01 +34 0.01 +35 0.1 +36 0.01 +37 0.01 +38 0.01 +39 0.01 +40 0.01 +41 0.01 +42 0.01 +43 0.01 +44 0.01 +45 0.97 +46 0.97 +47 0.01 + +2 +31 32 +4 3 +12 +0 0.01 +1 0.01 +2 0.04 +3 0.9 +4 0.01 +5 0.01 +6 0.92 +7 0.09 +8 0.98 +9 0.98 +10 0.04 +11 0.01 + +5 +12 14 20 32 33 +2 3 3 3 2 +108 +0 0.01 +1 0.05 +2 0.01 +3 0.7 +4 0.01 +5 0.95 +6 0.01 +7 0.05 +8 0.01 +9 0.7 +10 0.05 +11 0.95 +12 0.01 +13 0.05 +14 0.05 +15 0.7 +16 0.05 +17 0.95 +18 0.01 +19 0.05 +20 0.01 +21 0.7 +22 0.01 +23 0.99 +24 0.01 +25 0.05 +26 0.01 +27 0.7 +28 0.05 +29 0.99 +30 0.01 +31 0.05 +32 0.05 +33 0.7 +34 0.05 +35 0.99 +36 0.01 +37 0.01 +38 0.01 +39 0.1 +40 0.01 +41 0.3 +42 0.01 +43 0.01 +44 0.01 +45 0.1 +46 0.01 +47 0.3 +48 0.01 +49 0.01 +50 0.01 +51 0.1 +52 0.01 +53 0.3 +54 0.99 +55 0.95 +56 0.99 +57 0.3 +58 0.99 +59 0.05 +60 0.99 +61 0.95 +62 0.99 +63 0.3 +64 0.95 +65 0.05 +66 0.99 +67 0.95 +68 0.95 +69 0.3 +70 0.95 +71 0.05 +72 0.99 +73 0.95 +74 0.99 +75 0.3 +76 0.99 +77 0.01 +78 0.99 +79 0.95 +80 0.99 +81 0.3 +82 0.95 +83 0.01 +84 0.99 +85 0.95 +86 0.95 +87 0.3 +88 0.95 +89 0.01 +90 0.99 +91 0.99 +92 0.99 +93 0.9 +94 0.99 +95 0.7 +96 0.99 +97 0.99 +98 0.99 +99 0.9 +100 0.99 +101 0.7 +102 0.99 +103 0.99 +104 0.99 +105 0.9 +106 0.99 +107 0.7 + +2 +33 34 +2 3 +6 +0 0.05 +1 0.01 +2 0.9 +3 0.09 +4 0.05 +5 0.9 + +3 +6 34 35 +3 3 3 +27 +0 0.98 +1 0.95 +2 0.3 +3 0.95 +4 0.04 +5 0.01 +6 0.8 +7 0.01 +8 0.01 +9 0.01 +10 0.04 +11 0.69 +12 0.04 +13 0.95 +14 0.3 +15 0.19 +16 0.04 +17 0.01 +18 0.01 +19 0.01 +20 0.01 +21 0.01 +22 0.01 +23 0.69 +24 0.01 +25 0.95 +26 0.98 + +3 +14 35 36 +3 3 3 +27 +0 0.98 +1 0.98 +2 0.3 +3 0.98 +4 0.1 +5 0.05 +6 0.9 +7 0.05 +8 0.01 +9 0.01 +10 0.01 +11 0.6 +12 0.01 +13 0.85 +14 0.4 +15 0.09 +16 0.2 +17 0.09 +18 0.01 +19 0.01 +20 0.1 +21 0.01 +22 0.05 +23 0.55 +24 0.01 +25 0.75 +26 0.9 diff --git a/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai b/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai new file mode 100644 index 000000000..aacf458ed --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai @@ -0,0 +1,18 @@ +MARKOV +3 +2 2 3 +3 +1 0 +2 0 1 +2 1 2 + +2 + 0.436 0.564 + +4 + 0.128 0.872 + 0.920 0.080 + +6 + 0.210 0.333 0.457 + 0.811 0.000 0.189 \ No newline at end of file diff --git a/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai.evid b/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai.evid new file mode 100644 index 000000000..59f3e67a5 --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/sampleMARKOV.uai.evid @@ -0,0 +1,3 @@ +2 + 1 0 + 2 1 \ No newline at end of file diff --git a/gtsam/discrete/tests/data/UAI/uai08_test1.uai b/gtsam/discrete/tests/data/UAI/uai08_test1.uai new file mode 100644 index 000000000..d205773fc --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test1.uai @@ -0,0 +1,996 @@ +BAYES +54 +2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 +54 +4 1 17 27 0 +4 49 32 38 1 +2 22 2 +2 19 3 +1 4 +2 49 5 +3 17 19 6 +4 36 26 28 7 +1 8 +5 22 51 4 28 9 +4 8 23 26 10 +2 5 11 +1 12 +3 27 53 13 +3 38 12 14 +3 32 29 15 +3 23 45 16 +4 49 38 23 17 +6 32 36 33 43 39 18 +4 23 26 44 19 +5 31 22 38 23 20 +6 22 29 34 37 40 21 +1 22 +5 49 31 2 29 23 +2 4 24 +3 49 5 25 +4 22 5 29 26 +2 2 27 +3 25 47 28 +1 29 +2 2 30 +1 31 +1 32 +3 42 41 33 +3 36 25 34 +5 32 38 5 41 35 +1 36 +10 36 27 19 53 18 46 50 35 11 37 +2 22 38 +3 38 26 39 +1 40 +1 41 +3 2 25 42 +4 1 23 4 43 +1 44 +4 34 7 0 45 +4 2 30 33 46 +5 49 23 26 27 47 +3 31 44 48 +1 49 +3 26 41 50 +1 51 +4 36 8 23 52 +3 32 26 53 + +16 + 0.285714 0.714286 + 0.461538 0.538462 + 0.307692 0.692308 + 0.300000 0.700000 + 0.333333 0.666667 + 0.714286 0.285714 + 0.588235 0.411765 + 0.588235 0.411765 + +16 + 0.625000 0.375000 + 0.750000 0.250000 + 0.625000 0.375000 + 0.166667 0.833333 + 0.555556 0.444444 + 0.545455 0.454545 + 0.500000 0.500000 + 0.428571 0.571429 + +4 + 0.666667 0.333333 + 0.571429 0.428571 + +4 + 0.461538 0.538462 + 0.272727 0.727273 + +2 + 0.230769 0.769231 + +4 + 0.625000 0.375000 + 0.583333 0.416667 + +8 + 0.800000 0.200000 + 0.500000 0.500000 + 0.333333 0.666667 + 0.250000 0.750000 + +16 + 0.411765 0.588235 + 0.500000 0.500000 + 0.769231 0.230769 + 0.692308 0.307692 + 0.625000 0.375000 + 0.600000 0.400000 + 0.833333 0.166667 + 0.571429 0.428571 + +2 + 0.700000 0.300000 + +32 + 0.833333 0.166667 + 0.250000 0.750000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.526316 0.473684 + 0.500000 0.500000 + 0.428571 0.571429 + 0.500000 0.500000 + 0.444444 0.555556 + 0.666667 0.333333 + 0.636364 0.363636 + 0.384615 0.615385 + 0.222222 0.777778 + 0.411765 0.588235 + 0.526316 0.473684 + 0.583333 0.416667 + +16 + 0.357143 0.642857 + 0.363636 0.636364 + 0.166667 0.833333 + 0.777778 0.222222 + 0.473684 0.526316 + 0.538462 0.461538 + 0.500000 0.500000 + 0.470588 0.529412 + +4 + 0.470588 0.529412 + 0.400000 0.600000 + +2 + 0.500000 0.500000 + +8 + 0.473684 0.526316 + 0.555556 0.444444 + 0.411765 0.588235 + 0.714286 0.285714 + +8 + 0.800000 0.200000 + 0.444444 0.555556 + 0.600000 0.400000 + 0.666667 0.333333 + +8 + 0.500000 0.500000 + 0.333333 0.666667 + 0.625000 0.375000 + 0.692308 0.307692 + +8 + 0.285714 0.714286 + 0.714286 0.285714 + 0.500000 0.500000 + 0.500000 0.500000 + +16 + 0.833333 0.166667 + 0.454545 0.545455 + 0.625000 0.375000 + 0.250000 0.750000 + 0.727273 0.272727 + 0.588235 0.411765 + 0.400000 0.600000 + 0.500000 0.500000 + +64 + 0.166667 0.833333 + 0.666667 0.333333 + 0.692308 0.307692 + 0.538462 0.461538 + 0.500000 0.500000 + 0.250000 0.750000 + 0.437500 0.562500 + 0.473684 0.526316 + 0.769231 0.230769 + 0.400000 0.600000 + 0.555556 0.444444 + 0.272727 0.727273 + 0.473684 0.526316 + 0.818182 0.181818 + 0.750000 0.250000 + 0.416667 0.583333 + 0.588235 0.411765 + 0.769231 0.230769 + 0.500000 0.500000 + 0.473684 0.526316 + 0.833333 0.166667 + 0.444444 0.555556 + 0.600000 0.400000 + 0.529412 0.470588 + 0.727273 0.272727 + 0.615385 0.384615 + 0.444444 0.555556 + 0.400000 0.600000 + 0.642857 0.357143 + 0.200000 0.800000 + 0.333333 0.666667 + 0.437500 0.562500 + +16 + 0.666667 0.333333 + 0.250000 0.750000 + 0.625000 0.375000 + 0.357143 0.642857 + 0.500000 0.500000 + 0.300000 0.700000 + 0.526316 0.473684 + 0.600000 0.400000 + +32 + 0.444444 0.555556 + 0.583333 0.416667 + 0.500000 0.500000 + 0.571429 0.428571 + 0.400000 0.600000 + 0.500000 0.500000 + 0.333333 0.666667 + 0.666667 0.333333 + 0.473684 0.526316 + 0.500000 0.500000 + 0.545455 0.454545 + 0.454545 0.545455 + 0.500000 0.500000 + 0.466667 0.533333 + 0.777778 0.222222 + 0.222222 0.777778 + +64 + 0.333333 0.666667 + 0.818182 0.181818 + 0.526316 0.473684 + 0.375000 0.625000 + 0.625000 0.375000 + 0.444444 0.555556 + 0.473684 0.526316 + 0.533333 0.466667 + 0.500000 0.500000 + 0.500000 0.500000 + 0.363636 0.636364 + 0.300000 0.700000 + 0.250000 0.750000 + 0.562500 0.437500 + 0.571429 0.428571 + 0.642857 0.357143 + 0.666667 0.333333 + 0.363636 0.636364 + 0.384615 0.615385 + 0.600000 0.400000 + 0.818182 0.181818 + 0.428571 0.571429 + 0.625000 0.375000 + 0.562500 0.437500 + 0.583333 0.416667 + 0.529412 0.470588 + 0.529412 0.470588 + 0.545455 0.454545 + 0.333333 0.666667 + 0.230769 0.769231 + 0.500000 0.500000 + 0.230769 0.769231 + +2 + 0.588235 0.411765 + +32 + 0.333333 0.666667 + 0.333333 0.666667 + 0.428571 0.571429 + 0.600000 0.400000 + 0.750000 0.250000 + 0.666667 0.333333 + 0.411765 0.588235 + 0.583333 0.416667 + 0.800000 0.200000 + 0.545455 0.454545 + 0.333333 0.666667 + 0.375000 0.625000 + 0.571429 0.428571 + 0.285714 0.714286 + 0.555556 0.444444 + 0.461538 0.538462 + +4 + 0.500000 0.500000 + 0.625000 0.375000 + +8 + 0.727273 0.272727 + 0.461538 0.538462 + 0.777778 0.222222 + 0.400000 0.600000 + +16 + 0.555556 0.444444 + 0.600000 0.400000 + 0.571429 0.428571 + 0.833333 0.166667 + 0.777778 0.222222 + 0.357143 0.642857 + 0.285714 0.714286 + 0.642857 0.357143 + +4 + 0.461538 0.538462 + 0.250000 0.750000 + +8 + 0.692308 0.307692 + 0.529412 0.470588 + 0.437500 0.562500 + 0.666667 0.333333 + +2 + 0.727273 0.272727 + +4 + 0.500000 0.500000 + 0.571429 0.428571 + +2 + 0.375000 0.625000 + +2 + 0.428571 0.571429 + +8 + 0.666667 0.333333 + 0.444444 0.555556 + 0.500000 0.500000 + 0.416667 0.583333 + +8 + 0.357143 0.642857 + 0.461538 0.538462 + 0.272727 0.727273 + 0.411765 0.588235 + +32 + 0.470588 0.529412 + 0.466667 0.533333 + 0.700000 0.300000 + 0.555556 0.444444 + 0.444444 0.555556 + 0.666667 0.333333 + 0.466667 0.533333 + 0.466667 0.533333 + 0.200000 0.800000 + 0.588235 0.411765 + 0.166667 0.833333 + 0.333333 0.666667 + 0.526316 0.473684 + 0.562500 0.437500 + 0.333333 0.666667 + 0.700000 0.300000 + +2 + 0.166667 0.833333 + +1024 + 0.250000 0.750000 + 0.307692 0.692308 + 0.500000 0.500000 + 0.666667 0.333333 + 0.818182 0.181818 + 0.500000 0.500000 + 0.625000 0.375000 + 0.615385 0.384615 + 0.500000 0.500000 + 0.285714 0.714286 + 0.230769 0.769231 + 0.692308 0.307692 + 0.333333 0.666667 + 0.625000 0.375000 + 0.437500 0.562500 + 0.625000 0.375000 + 0.272727 0.727273 + 0.636364 0.363636 + 0.181818 0.818182 + 0.500000 0.500000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.818182 0.181818 + 0.437500 0.562500 + 0.500000 0.500000 + 0.750000 0.250000 + 0.375000 0.625000 + 0.625000 0.375000 + 0.700000 0.300000 + 0.466667 0.533333 + 0.411765 0.588235 + 0.666667 0.333333 + 0.750000 0.250000 + 0.285714 0.714286 + 0.250000 0.750000 + 0.571429 0.428571 + 0.555556 0.444444 + 0.428571 0.571429 + 0.500000 0.500000 + 0.666667 0.333333 + 0.571429 0.428571 + 0.222222 0.777778 + 0.615385 0.384615 + 0.461538 0.538462 + 0.250000 0.750000 + 0.666667 0.333333 + 0.200000 0.800000 + 0.384615 0.615385 + 0.300000 0.700000 + 0.466667 0.533333 + 0.625000 0.375000 + 0.562500 0.437500 + 0.583333 0.416667 + 0.500000 0.500000 + 0.727273 0.272727 + 0.571429 0.428571 + 0.250000 0.750000 + 0.333333 0.666667 + 0.500000 0.500000 + 0.545455 0.454545 + 0.333333 0.666667 + 0.666667 0.333333 + 0.461538 0.538462 + 0.181818 0.818182 + 0.714286 0.285714 + 0.666667 0.333333 + 0.470588 0.529412 + 0.500000 0.500000 + 0.470588 0.529412 + 0.500000 0.500000 + 0.416667 0.583333 + 0.625000 0.375000 + 0.625000 0.375000 + 0.692308 0.307692 + 0.500000 0.500000 + 0.666667 0.333333 + 0.714286 0.285714 + 0.600000 0.400000 + 0.461538 0.538462 + 0.500000 0.500000 + 0.500000 0.500000 + 0.181818 0.818182 + 0.750000 0.250000 + 0.357143 0.642857 + 0.400000 0.600000 + 0.625000 0.375000 + 0.250000 0.750000 + 0.461538 0.538462 + 0.250000 0.750000 + 0.333333 0.666667 + 0.272727 0.727273 + 0.428571 0.571429 + 0.166667 0.833333 + 0.600000 0.400000 + 0.750000 0.250000 + 0.583333 0.416667 + 0.769231 0.230769 + 0.769231 0.230769 + 0.545455 0.454545 + 0.470588 0.529412 + 0.454545 0.545455 + 0.555556 0.444444 + 0.714286 0.285714 + 0.384615 0.615385 + 0.428571 0.571429 + 0.636364 0.363636 + 0.583333 0.416667 + 0.384615 0.615385 + 0.357143 0.642857 + 0.571429 0.428571 + 0.642857 0.357143 + 0.636364 0.363636 + 0.714286 0.285714 + 0.230769 0.769231 + 0.333333 0.666667 + 0.428571 0.571429 + 0.533333 0.466667 + 0.625000 0.375000 + 0.444444 0.555556 + 0.357143 0.642857 + 0.555556 0.444444 + 0.500000 0.500000 + 0.333333 0.666667 + 0.384615 0.615385 + 0.600000 0.400000 + 0.333333 0.666667 + 0.700000 0.300000 + 0.500000 0.500000 + 0.545455 0.454545 + 0.800000 0.200000 + 0.625000 0.375000 + 0.250000 0.750000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.666667 0.333333 + 0.666667 0.333333 + 0.692308 0.307692 + 0.400000 0.600000 + 0.692308 0.307692 + 0.666667 0.333333 + 0.555556 0.444444 + 0.666667 0.333333 + 0.222222 0.777778 + 0.562500 0.437500 + 0.500000 0.500000 + 0.666667 0.333333 + 0.230769 0.769231 + 0.555556 0.444444 + 0.307692 0.692308 + 0.800000 0.200000 + 0.400000 0.600000 + 0.666667 0.333333 + 0.285714 0.714286 + 0.500000 0.500000 + 0.444444 0.555556 + 0.555556 0.444444 + 0.272727 0.727273 + 0.600000 0.400000 + 0.428571 0.571429 + 0.400000 0.600000 + 0.526316 0.473684 + 0.333333 0.666667 + 0.750000 0.250000 + 0.636364 0.363636 + 0.333333 0.666667 + 0.750000 0.250000 + 0.500000 0.500000 + 0.818182 0.181818 + 0.375000 0.625000 + 0.333333 0.666667 + 0.625000 0.375000 + 0.583333 0.416667 + 0.230769 0.769231 + 0.769231 0.230769 + 0.800000 0.200000 + 0.636364 0.363636 + 0.384615 0.615385 + 0.562500 0.437500 + 0.727273 0.272727 + 0.250000 0.750000 + 0.600000 0.400000 + 0.538462 0.461538 + 0.750000 0.250000 + 0.428571 0.571429 + 0.300000 0.700000 + 0.555556 0.444444 + 0.692308 0.307692 + 0.230769 0.769231 + 0.333333 0.666667 + 0.454545 0.545455 + 0.666667 0.333333 + 0.583333 0.416667 + 0.454545 0.545455 + 0.562500 0.437500 + 0.666667 0.333333 + 0.500000 0.500000 + 0.250000 0.750000 + 0.625000 0.375000 + 0.588235 0.411765 + 0.818182 0.181818 + 0.500000 0.500000 + 0.250000 0.750000 + 0.636364 0.363636 + 0.181818 0.818182 + 0.333333 0.666667 + 0.411765 0.588235 + 0.500000 0.500000 + 0.428571 0.571429 + 0.230769 0.769231 + 0.333333 0.666667 + 0.562500 0.437500 + 0.666667 0.333333 + 0.600000 0.400000 + 0.333333 0.666667 + 0.500000 0.500000 + 0.333333 0.666667 + 0.714286 0.285714 + 0.333333 0.666667 + 0.714286 0.285714 + 0.454545 0.545455 + 0.181818 0.818182 + 0.400000 0.600000 + 0.750000 0.250000 + 0.636364 0.363636 + 0.300000 0.700000 + 0.222222 0.777778 + 0.200000 0.800000 + 0.777778 0.222222 + 0.500000 0.500000 + 0.384615 0.615385 + 0.411765 0.588235 + 0.818182 0.181818 + 0.357143 0.642857 + 0.588235 0.411765 + 0.285714 0.714286 + 0.562500 0.437500 + 0.529412 0.470588 + 0.466667 0.533333 + 0.454545 0.545455 + 0.800000 0.200000 + 0.571429 0.428571 + 0.250000 0.750000 + 0.500000 0.500000 + 0.400000 0.600000 + 0.444444 0.555556 + 0.600000 0.400000 + 0.500000 0.500000 + 0.200000 0.800000 + 0.642857 0.357143 + 0.666667 0.333333 + 0.600000 0.400000 + 0.250000 0.750000 + 0.500000 0.500000 + 0.600000 0.400000 + 0.300000 0.700000 + 0.363636 0.636364 + 0.727273 0.272727 + 0.250000 0.750000 + 0.500000 0.500000 + 0.666667 0.333333 + 0.615385 0.384615 + 0.642857 0.357143 + 0.473684 0.526316 + 0.437500 0.562500 + 0.545455 0.454545 + 0.411765 0.588235 + 0.466667 0.533333 + 0.666667 0.333333 + 0.333333 0.666667 + 0.562500 0.437500 + 0.700000 0.300000 + 0.500000 0.500000 + 0.473684 0.526316 + 0.357143 0.642857 + 0.571429 0.428571 + 0.416667 0.583333 + 0.555556 0.444444 + 0.833333 0.166667 + 0.727273 0.272727 + 0.181818 0.818182 + 0.750000 0.250000 + 0.200000 0.800000 + 0.470588 0.529412 + 0.583333 0.416667 + 0.625000 0.375000 + 0.800000 0.200000 + 0.400000 0.600000 + 0.437500 0.562500 + 0.400000 0.600000 + 0.444444 0.555556 + 0.454545 0.545455 + 0.181818 0.818182 + 0.615385 0.384615 + 0.533333 0.466667 + 0.428571 0.571429 + 0.625000 0.375000 + 0.777778 0.222222 + 0.333333 0.666667 + 0.588235 0.411765 + 0.285714 0.714286 + 0.500000 0.500000 + 0.636364 0.363636 + 0.428571 0.571429 + 0.727273 0.272727 + 0.500000 0.500000 + 0.285714 0.714286 + 0.818182 0.181818 + 0.250000 0.750000 + 0.555556 0.444444 + 0.181818 0.818182 + 0.727273 0.272727 + 0.529412 0.470588 + 0.625000 0.375000 + 0.555556 0.444444 + 0.777778 0.222222 + 0.714286 0.285714 + 0.727273 0.272727 + 0.300000 0.700000 + 0.411765 0.588235 + 0.222222 0.777778 + 0.800000 0.200000 + 0.642857 0.357143 + 0.769231 0.230769 + 0.562500 0.437500 + 0.600000 0.400000 + 0.400000 0.600000 + 0.600000 0.400000 + 0.461538 0.538462 + 0.500000 0.500000 + 0.461538 0.538462 + 0.750000 0.250000 + 0.307692 0.692308 + 0.444444 0.555556 + 0.400000 0.600000 + 0.666667 0.333333 + 0.727273 0.272727 + 0.250000 0.750000 + 0.666667 0.333333 + 0.500000 0.500000 + 0.473684 0.526316 + 0.727273 0.272727 + 0.444444 0.555556 + 0.428571 0.571429 + 0.285714 0.714286 + 0.500000 0.500000 + 0.470588 0.529412 + 0.500000 0.500000 + 0.363636 0.636364 + 0.428571 0.571429 + 0.615385 0.384615 + 0.500000 0.500000 + 0.555556 0.444444 + 0.500000 0.500000 + 0.250000 0.750000 + 0.642857 0.357143 + 0.400000 0.600000 + 0.411765 0.588235 + 0.250000 0.750000 + 0.700000 0.300000 + 0.500000 0.500000 + 0.416667 0.583333 + 0.692308 0.307692 + 0.500000 0.500000 + 0.357143 0.642857 + 0.750000 0.250000 + 0.181818 0.818182 + 0.166667 0.833333 + 0.250000 0.750000 + 0.714286 0.285714 + 0.769231 0.230769 + 0.666667 0.333333 + 0.714286 0.285714 + 0.333333 0.666667 + 0.285714 0.714286 + 0.750000 0.250000 + 0.166667 0.833333 + 0.500000 0.500000 + 0.466667 0.533333 + 0.714286 0.285714 + 0.545455 0.454545 + 0.166667 0.833333 + 0.428571 0.571429 + 0.750000 0.250000 + 0.307692 0.692308 + 0.428571 0.571429 + 0.818182 0.181818 + 0.375000 0.625000 + 0.625000 0.375000 + 0.250000 0.750000 + 0.700000 0.300000 + 0.300000 0.700000 + 0.625000 0.375000 + 0.642857 0.357143 + 0.428571 0.571429 + 0.500000 0.500000 + 0.777778 0.222222 + 0.444444 0.555556 + 0.333333 0.666667 + 0.428571 0.571429 + 0.307692 0.692308 + 0.333333 0.666667 + 0.166667 0.833333 + 0.571429 0.428571 + 0.333333 0.666667 + 0.500000 0.500000 + 0.538462 0.461538 + 0.250000 0.750000 + 0.416667 0.583333 + 0.500000 0.500000 + 0.500000 0.500000 + 0.625000 0.375000 + 0.473684 0.526316 + 0.375000 0.625000 + 0.470588 0.529412 + 0.454545 0.545455 + 0.500000 0.500000 + 0.333333 0.666667 + 0.500000 0.500000 + 0.363636 0.636364 + 0.600000 0.400000 + 0.166667 0.833333 + 0.769231 0.230769 + 0.588235 0.411765 + 0.642857 0.357143 + 0.636364 0.363636 + 0.833333 0.166667 + 0.166667 0.833333 + 0.470588 0.529412 + 0.700000 0.300000 + 0.700000 0.300000 + 0.666667 0.333333 + 0.714286 0.285714 + 0.384615 0.615385 + 0.500000 0.500000 + 0.777778 0.222222 + 0.454545 0.545455 + 0.500000 0.500000 + 0.181818 0.818182 + 0.526316 0.473684 + 0.700000 0.300000 + 0.777778 0.222222 + 0.529412 0.470588 + 0.714286 0.285714 + 0.428571 0.571429 + 0.500000 0.500000 + 0.588235 0.411765 + 0.571429 0.428571 + 0.750000 0.250000 + 0.500000 0.500000 + 0.666667 0.333333 + 0.363636 0.636364 + 0.571429 0.428571 + 0.454545 0.545455 + 0.444444 0.555556 + 0.250000 0.750000 + 0.363636 0.636364 + 0.272727 0.727273 + 0.333333 0.666667 + 0.615385 0.384615 + 0.615385 0.384615 + 0.333333 0.666667 + 0.583333 0.416667 + 0.166667 0.833333 + 0.428571 0.571429 + 0.400000 0.600000 + 0.454545 0.545455 + 0.500000 0.500000 + 0.714286 0.285714 + 0.500000 0.500000 + 0.800000 0.200000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.300000 0.700000 + 0.454545 0.545455 + 0.416667 0.583333 + 0.615385 0.384615 + 0.600000 0.400000 + 0.357143 0.642857 + 0.454545 0.545455 + 0.230769 0.769231 + 0.428571 0.571429 + 0.500000 0.500000 + 0.562500 0.437500 + 0.555556 0.444444 + 0.571429 0.428571 + 0.750000 0.250000 + 0.166667 0.833333 + 0.285714 0.714286 + 0.400000 0.600000 + 0.461538 0.538462 + 0.333333 0.666667 + 0.555556 0.444444 + 0.416667 0.583333 + 0.466667 0.533333 + 0.333333 0.666667 + 0.444444 0.555556 + 0.375000 0.625000 + 0.642857 0.357143 + 0.727273 0.272727 + 0.470588 0.529412 + 0.363636 0.636364 + 0.714286 0.285714 + 0.666667 0.333333 + 0.411765 0.588235 + 0.250000 0.750000 + 0.437500 0.562500 + 0.500000 0.500000 + 0.400000 0.600000 + 0.400000 0.600000 + 0.428571 0.571429 + 0.222222 0.777778 + +4 + 0.454545 0.545455 + 0.363636 0.636364 + +8 + 0.285714 0.714286 + 0.625000 0.375000 + 0.400000 0.600000 + 0.727273 0.272727 + +2 + 0.526316 0.473684 + +2 + 0.454545 0.545455 + +8 + 0.529412 0.470588 + 0.500000 0.500000 + 0.538462 0.461538 + 0.500000 0.500000 + +16 + 0.526316 0.473684 + 0.571429 0.428571 + 0.562500 0.437500 + 0.230769 0.769231 + 0.333333 0.666667 + 0.750000 0.250000 + 0.333333 0.666667 + 0.600000 0.400000 + +2 + 0.714286 0.285714 + +16 + 0.230769 0.769231 + 0.454545 0.545455 + 0.571429 0.428571 + 0.777778 0.222222 + 0.466667 0.533333 + 0.250000 0.750000 + 0.384615 0.615385 + 0.571429 0.428571 + +16 + 0.666667 0.333333 + 0.555556 0.444444 + 0.363636 0.636364 + 0.833333 0.166667 + 0.400000 0.600000 + 0.818182 0.181818 + 0.692308 0.307692 + 0.692308 0.307692 + +32 + 0.533333 0.466667 + 0.400000 0.600000 + 0.666667 0.333333 + 0.333333 0.666667 + 0.588235 0.411765 + 0.363636 0.636364 + 0.470588 0.529412 + 0.500000 0.500000 + 0.636364 0.363636 + 0.400000 0.600000 + 0.636364 0.363636 + 0.428571 0.571429 + 0.500000 0.500000 + 0.714286 0.285714 + 0.272727 0.727273 + 0.357143 0.642857 + +8 + 0.250000 0.750000 + 0.285714 0.714286 + 0.583333 0.416667 + 0.571429 0.428571 + +2 + 0.375000 0.625000 + +8 + 0.666667 0.333333 + 0.300000 0.700000 + 0.529412 0.470588 + 0.473684 0.526316 + +2 + 0.500000 0.500000 + +16 + 0.666667 0.333333 + 0.200000 0.800000 + 0.500000 0.500000 + 0.500000 0.500000 + 0.666667 0.333333 + 0.714286 0.285714 + 0.470588 0.529412 + 0.533333 0.466667 + +8 + 0.307692 0.692308 + 0.470588 0.529412 + 0.333333 0.666667 + 0.333333 0.666667 + diff --git a/gtsam/discrete/tests/data/UAI/uai08_test1.uai.evid b/gtsam/discrete/tests/data/UAI/uai08_test1.uai.evid new file mode 100644 index 000000000..5ca206d95 --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test1.uai.evid @@ -0,0 +1,11 @@ +10 + 0 1 + 2 0 + 9 0 + 16 1 + 20 1 + 21 1 + 22 0 + 26 1 + 39 0 + 41 1 diff --git a/gtsam/discrete/tests/data/UAI/uai08_test1.uai.output b/gtsam/discrete/tests/data/UAI/uai08_test1.uai.output new file mode 100644 index 000000000..c376783fe --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test1.uai.output @@ -0,0 +1,3 @@ +z -2.7351873 +m 54 2 0.0 1.0 2 0.5995855 0.40041456 2 1.0 0.0 2 0.3761365 0.62386346 2 0.25656807 0.7434319 2 0.6449692 0.35503078 2 0.4957979 0.5042021 2 0.69854456 0.30145544 2 0.7 0.3 2 1.0 0.0 2 0.5303537 0.46964625 2 0.44570237 0.5542976 2 0.5 0.5 2 0.55686617 0.4431338 2 0.6284742 0.3715258 2 0.5607879 0.43921205 2 0.0 1.0 2 0.54289234 0.4571077 2 0.5770133 0.42298666 2 0.547688 0.452312 2 0.0 1.0 2 0.0 1.0 2 1.0 0.0 2 0.5760513 0.4239487 2 0.592929 0.40707102 2 0.63438964 0.36561036 2 0.0 1.0 2 0.52899235 0.47100765 2 0.5998554 0.40014458 2 0.7750039 0.22499608 2 0.50000435 0.49999565 2 0.36475798 0.63524204 2 0.44666538 0.55333465 2 0.43111995 0.56888 2 0.37207335 0.62792665 2 0.5581817 0.4418183 2 0.16809757 0.83190244 2 0.4813641 0.5186359 2 0.43732184 0.56267816 2 1.0 0.0 2 0.54721755 0.45278242 2 0.0 1.0 2 0.51865995 0.48134002 2 0.51229435 0.48770565 2 0.7142385 0.2857615 2 0.53666514 0.46333483 2 0.6171147 0.38288528 2 0.46532288 0.5346771 2 0.46330887 0.5366911 2 0.36718464 0.63281536 2 0.4735739 0.5264261 2 0.5244508 0.4755492 2 0.604569 0.395431 2 0.3945428 0.6054572 +s -11.533098 54 1 0 0 1 1 0 1 0 0 0 0 1 1 0 0 0 1 1 1 1 1 1 0 1 0 0 1 1 0 0 1 1 1 1 1 0 1 1 1 0 0 1 0 1 0 1 0 1 0 1 1 0 0 1 diff --git a/gtsam/discrete/tests/data/UAI/uai08_test2.uai b/gtsam/discrete/tests/data/UAI/uai08_test2.uai new file mode 100644 index 000000000..a75b376ed --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test2.uai @@ -0,0 +1,269 @@ +BAYES +21 +4 4 4 4 4 4 4 4 4 2 2 2 2 2 2 2 2 2 2 2 2 +21 +1 0 +1 1 +1 2 +1 3 +1 4 +1 5 +1 6 +1 7 +1 8 +3 1 0 9 +3 1 3 10 +3 5 1 11 +3 2 6 12 +3 6 4 13 +3 3 6 14 +3 5 7 15 +3 7 2 16 +3 7 3 17 +3 0 8 18 +3 3 8 19 +3 8 4 20 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.25 0.25 0.25 0.25 + +4 + 0.1 0.2 0.3 0.4 + +4 + 0.25 0.25 0.25 0.25 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + +32 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.1 0.9 + 0.9 0.1 + diff --git a/gtsam/discrete/tests/data/UAI/uai08_test2.uai.evid b/gtsam/discrete/tests/data/UAI/uai08_test2.uai.evid new file mode 100644 index 000000000..1214f3c1b --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test2.uai.evid @@ -0,0 +1,13 @@ +12 + 17 0 + 10 0 + 19 0 + 18 0 + 11 0 + 13 0 + 15 0 + 20 0 + 9 0 + 12 0 + 16 0 + 14 0 diff --git a/gtsam/discrete/tests/data/UAI/uai08_test2.uai.output b/gtsam/discrete/tests/data/UAI/uai08_test2.uai.output new file mode 100644 index 000000000..a124d2b4c --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test2.uai.output @@ -0,0 +1,3 @@ +z -5.264346 +m 21 4 0.116109975 0.20537 0.29463002 0.38389003 4 0.10865768 0.2028859 0.2971141 0.3913423 4 0.11159538 0.20386513 0.29613486 0.3884046 4 0.105094366 0.20169812 0.29830188 0.39490563 4 0.116109975 0.20537 0.29463002 0.38389003 4 0.11159538 0.20386513 0.29613486 0.3884046 4 0.10865768 0.2028859 0.2971141 0.3913423 4 0.1 0.2 0.3 0.4 4 0.10956474 0.20318824 0.29681176 0.39043528 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 2 1.0 0.0 +s -5.7635098 21 3 3 3 3 3 3 3 3 3 0 0 0 0 0 0 0 0 0 0 0 0 diff --git a/gtsam/discrete/tests/data/UAI/uai08_test3.uai b/gtsam/discrete/tests/data/UAI/uai08_test3.uai new file mode 100644 index 000000000..2abb99bc2 --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test3.uai @@ -0,0 +1,94 @@ +MARKOV +9 +4 4 4 4 4 4 4 4 4 +13 +1 7 +2 1 0 +2 1 3 +2 5 1 +2 2 6 +2 6 4 +2 3 6 +2 5 7 +2 7 2 +2 7 3 +2 0 8 +2 3 8 +2 8 4 + +4 + 0.1 0.2 0.3 0.4 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + +16 + 0.9 0.1 0.1 0.1 + 0.1 0.9 0.1 0.1 + 0.1 0.1 0.9 0.1 + 0.1 0.1 0.1 0.9 + + diff --git a/gtsam/discrete/tests/data/UAI/uai08_test3.uai.evid b/gtsam/discrete/tests/data/UAI/uai08_test3.uai.evid new file mode 100644 index 000000000..18748286e --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test3.uai.evid @@ -0,0 +1 @@ +0 diff --git a/gtsam/discrete/tests/data/UAI/uai08_test3.uai.output b/gtsam/discrete/tests/data/UAI/uai08_test3.uai.output new file mode 100644 index 000000000..1ddb8297a --- /dev/null +++ b/gtsam/discrete/tests/data/UAI/uai08_test3.uai.output @@ -0,0 +1,3 @@ +z -0.44786617 +m 9 4 0.116109975 0.20537 0.29463002 0.38389003 4 0.10865768 0.2028859 0.2971141 0.3913423 4 0.11159538 0.20386513 0.29613486 0.3884046 4 0.105094366 0.20169812 0.29830188 0.39490563 4 0.116109975 0.20537 0.29463002 0.38389003 4 0.11159538 0.20386513 0.29613486 0.3884046 4 0.10865768 0.2028859 0.2971141 0.3913423 4 0.1 0.2 0.3 0.4 4 0.10956474 0.20318824 0.29681176 0.39043528 +s -0.9470299 9 3 3 3 3 3 3 3 3 3 diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp new file mode 100644 index 000000000..2c3d61fe8 --- /dev/null +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -0,0 +1,524 @@ +/* + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @date Mar 6, 2011 + */ + +#include +#include // make sure we have traits +// headers first to make sure no missing headers +//#define DT_NO_PRUNING +#include +#include // for convert only +#define DISABLE_TIMING +#include // for checking whether we are using boost 1.40 +#if BOOST_VERSION >= 104200 +#define BOOST_HAVE_PARSER +#endif + +#include +#include +#include +#include +#include +using namespace boost::assign; + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ******************************************************************************** */ +typedef AlgebraicDecisionTree ADT; + +template class DecisionTree; +template class AlgebraicDecisionTree; + +#define DISABLE_DOT + +template +void dot(const T&f, const string& filename) { +#ifndef DISABLE_DOT + f.dot(filename); +#endif +} + +/** I can't get this to work ! + class Mul: boost::function { + inline double operator()(const double& a, const double& b) { + return a * b; + } + }; + + // If second argument of binary op is Leaf + template + typename DecisionTree::Node::Ptr DecisionTree::Choice::apply_fC_op_gL( + Cache& cache, const Leaf& gL, Mul op) const { + Ptr h(new Choice(label(), cardinality())); + BOOST_FOREACH(const NodePtr& branch, branches_) + h->push_back(branch->apply_f_op_g(cache, gL, op)); + return Unique(cache, h); + } + */ + +/* ******************************************************************************** */ +// instrumented operators +/* ******************************************************************************** */ +size_t muls = 0, adds = 0; +boost::timer timer; +void resetCounts() { + 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; +#endif + resetCounts(); +} +double mul(const double& a, const double& b) { + muls++; + return a * b; +} +double add_(const double& a, const double& 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); + + // 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"); + +// a.print("a: "); +// cnotb.print("cnotb: "); + ADT acnotb = a * cnotb; +// acnotb.print("acnotb: "); +// acnotb.printCache("acnotb Cache:"); + + dot(acnotb, "ADT-acnotb"); + + + ADT big = apply(apply(d, note, &mul), acnotb, &add_); + dot(big, "ADT-big"); +} + +/* ******************************************************************************** */ +// Asia Bayes Network +/* ******************************************************************************** */ + +/** 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; +} + +/* ************************************************************************* */ +// 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); + +#ifdef BOOST_HAVE_PARSER + 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"); + + 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)); +#endif +} + +/* ************************************************************************* */ +// 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); + +#ifdef BOOST_HAVE_PARSER + 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"); + + 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"); +#endif +} + +/* ************************************************************************* */ +TEST(ADT, factor_graph) +{ + DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); + +#ifdef BOOST_HAVE_PARSER + 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"); + + 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 + + // 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 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 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"); +#endif +} + +/* ************************************************************************* */ +// 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; + + // 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); +} + +/* ************************************************************************* */ +#ifdef BOOST_HAVE_PARSER +// 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 + + // 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); +} +#endif + +/* ******************************************************************************** */ +// Factor graph construction +// 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; + + 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); + + 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); +} + +/* ************************************************************************* */ +// test conversion to integer indices +// 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"); + + std::map ordering; + ordering[0] = 5; + ordering[1] = 2; + + 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); +} + +/* ******************************************************************************** */ +// 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"); + + { + // 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)); + } + + { + // 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)); + } +} + +/* ******************************************************************************** */ +// Test non-commutative op +TEST(ADT, div) +{ + 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)); +} + +/* ******************************************************************************** */ +// test zero shortcut +TEST(ADT, zero) +{ + 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); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testCSP.cpp b/gtsam/discrete/tests/testCSP.cpp new file mode 100644 index 000000000..51ddca098 --- /dev/null +++ b/gtsam/discrete/tests/testCSP.cpp @@ -0,0 +1,224 @@ +/* + * testCSP.cpp + * @brief develop code for CSP solver + * @date Feb 5, 2012 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST_UNSAFE( BinaryAllDif, allInOne) +{ + // Create keys and ordering + size_t nrColors = 2; +// DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors); + DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + + // Check construction and conversion + BinaryAllDiff c1(ID, UT); + DecisionTreeFactor f1(ID & UT, "0 1 1 0"); + EXPECT(assert_equal(f1,(DecisionTreeFactor)c1)); + + // Check construction and conversion + BinaryAllDiff c2(UT, AZ); + DecisionTreeFactor f2(UT & AZ, "0 1 1 0"); + EXPECT(assert_equal(f2,(DecisionTreeFactor)c2)); + + DecisionTreeFactor f3 = f1*f2; + EXPECT(assert_equal(f3,c1*f2)); + EXPECT(assert_equal(f3,c2*f1)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( CSP, allInOne) +{ + // Create keys and ordering + size_t nrColors = 2; + DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + + // Create the CSP + CSP csp; + csp.addAllDiff(ID,UT); + csp.addAllDiff(UT,AZ); + + // Check an invalid combination, with ID==UT==AZ all same color + DiscreteFactor::Values invalid; + invalid[ID.first] = 0; + invalid[UT.first] = 0; + invalid[AZ.first] = 0; + EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); + + // Check a valid combination + DiscreteFactor::Values valid; + valid[ID.first] = 0; + valid[UT.first] = 1; + valid[AZ.first] = 0; + EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); + + // Just for fun, create the product and check it + DecisionTreeFactor product = csp.product(); + // product.dot("product"); + DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); + EXPECT(assert_equal(expectedProduct,product)); + + // Solve + CSP::sharedValues mpe = csp.optimalAssignment(); + CSP::Values expected; + insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); + EXPECT(assert_equal(expected,*mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); +} + +/* ************************************************************************* */ +TEST_UNSAFE( CSP, WesternUS) +{ + // Create keys + size_t nrColors = 4; + DiscreteKey + // Create ordering according to example in ND-CSP.lyx + WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors), + ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), + MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); + + // Create the CSP + CSP csp; + csp.addAllDiff(WA,ID); + csp.addAllDiff(WA,OR); + csp.addAllDiff(OR,ID); + csp.addAllDiff(OR,CA); + csp.addAllDiff(OR,NV); + csp.addAllDiff(CA,NV); + csp.addAllDiff(CA,AZ); + csp.addAllDiff(ID,MT); + csp.addAllDiff(ID,WY); + csp.addAllDiff(ID,UT); + csp.addAllDiff(ID,NV); + csp.addAllDiff(NV,UT); + csp.addAllDiff(NV,AZ); + csp.addAllDiff(UT,WY); + csp.addAllDiff(UT,CO); + csp.addAllDiff(UT,NM); + csp.addAllDiff(UT,AZ); + csp.addAllDiff(AZ,CO); + csp.addAllDiff(AZ,NM); + csp.addAllDiff(MT,WY); + csp.addAllDiff(WY,CO); + csp.addAllDiff(CO,NM); + + // Solve + CSP::sharedValues mpe = csp.optimalAssignment(); + // GTSAM_PRINT(*mpe); + CSP::Values expected; + insert(expected) + (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0) + (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) + (ID.first,2)(UT.first,1)(AZ.first,0); + EXPECT(assert_equal(expected,*mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + + // Write out the dual graph for hmetis +#ifdef DUAL + VariableIndex index(csp); + index.print("index"); + ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/US-West-dual.txt"); + index.outputMetisFormat(os); +#endif +} + +/* ************************************************************************* */ +TEST_UNSAFE( CSP, AllDiff) +{ + // Create keys and ordering + size_t nrColors = 3; + DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + + // Create the CSP + CSP csp; + vector dkeys; + dkeys += ID,UT,AZ; + csp.addAllDiff(dkeys); + csp.addSingleValue(AZ,2); + //GTSAM_PRINT(csp); + + // Check construction and conversion + SingleValue s(AZ,2); + DecisionTreeFactor f1(AZ,"0 0 1"); + EXPECT(assert_equal(f1,(DecisionTreeFactor)s)); + + // Check construction and conversion + AllDiff alldiff(dkeys); + DecisionTreeFactor actual = (DecisionTreeFactor)alldiff; +// GTSAM_PRINT(actual); +// actual.dot("actual"); + DecisionTreeFactor f2(ID & AZ & UT, + "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); + EXPECT(assert_equal(f2,actual)); + + // Check an invalid combination, with ID==UT==AZ all same color + DiscreteFactor::Values invalid; + invalid[ID.first] = 0; + invalid[UT.first] = 1; + invalid[AZ.first] = 0; + EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); + + // Check a valid combination + DiscreteFactor::Values valid; + valid[ID.first] = 0; + valid[UT.first] = 1; + valid[AZ.first] = 2; + EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); + + // Solve + CSP::sharedValues mpe = csp.optimalAssignment(); + CSP::Values expected; + insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); + EXPECT(assert_equal(expected,*mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + + // Arc-consistency + vector domains; + domains += Domain(ID), Domain(AZ), Domain(UT); + SingleValue singleValue(AZ,2); + EXPECT(singleValue.ensureArcConsistency(1,domains)); + EXPECT(alldiff.ensureArcConsistency(0,domains)); + EXPECT(!alldiff.ensureArcConsistency(1,domains)); + EXPECT(alldiff.ensureArcConsistency(2,domains)); + LONGS_EQUAL(2,domains[0].nrValues()); + LONGS_EQUAL(1,domains[1].nrValues()); + LONGS_EQUAL(2,domains[2].nrValues()); + + // Parial application, version 1 + DiscreteFactor::Values known; + known[AZ.first] = 2; + DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known); + DecisionTreeFactor f3(ID & UT, "0 1 1 1 0 1 1 1 0"); + EXPECT(assert_equal(f3,reduced1->operator DecisionTreeFactor())); + DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known); + DecisionTreeFactor f4(AZ, "0 0 1"); + EXPECT(assert_equal(f4,reduced2->operator DecisionTreeFactor())); + + // Parial application, version 2 + DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains); + EXPECT(assert_equal(f3,reduced3->operator DecisionTreeFactor())); + DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains); + EXPECT(assert_equal(f4,reduced4->operator DecisionTreeFactor())); + + // full arc-consistency test + csp.runArcConsistency(nrColors); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp new file mode 100644 index 000000000..216d4b965 --- /dev/null +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -0,0 +1,226 @@ +/* + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 + */ + +#include +#include +using namespace boost::assign; + +#include +#include +#include + +//#define DT_DEBUG_MEMORY +//#define DT_NO_PRUNING +#define DISABLE_DOT +#include +using namespace std; +using namespace gtsam; + +template +void dot(const T&f, const string& filename) { +#ifndef DISABLE_DOT + f.dot(filename); +#endif +} + +#define DOT(x)(dot(x,#x)) + +/* ******************************************************************************** */ +// Test string labels and int range +/* ******************************************************************************** */ + +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; + } +}; + +/* ******************************************************************************** */ +// test DT +TEST(DT, example) +{ + // 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; + + // 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); + + // \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); + + // 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); + + // 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 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); + + // 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; + + // 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); +} + +/* ******************************************************************************** */ +// test Conversion +enum Label { + U, V, X, Y, Z +}; +typedef DecisionTree BDT; +bool convert(const int& y) { + return y != 0; +} + +TEST(DT, conversion) +{ + // 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); + + // 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