From f0d49f21f8a8047edb13e40af323ab15c2da5b61 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 30 Aug 2012 18:10:07 +0000 Subject: [PATCH 01/53] Added toolbox readme from release branch --- matlab/README-gtsam-toolbox.txt | 76 +++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 matlab/README-gtsam-toolbox.txt diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt new file mode 100644 index 000000000..6f74f9806 --- /dev/null +++ b/matlab/README-gtsam-toolbox.txt @@ -0,0 +1,76 @@ +================================================================================ +GTSAM - Georgia Tech Smoothing and Mapping Library + +MATLAB wrapper + +http://borg.cc.gatech.edu/projects/gtsam +================================================================================ + +This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ +library. + + +---------------------------------------- +Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) +---------------------------------------- + +If you have a newer Ubuntu system, you must make a small modification to your +MATLAB installation, due to MATLAB being distributed with an old version of +the C++ standard library. Delete or rename all files starting with +'libstdc++' in your MATLAB installation directory, in paths: + + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ + + +---------------------------------------- +Adding the toolbox to your MATLAB path +---------------------------------------- + +To get started, first add the 'toolbox' folder to your MATLAB path - in the +MATLAB file browser, right-click on the folder and click 'Add to path -> This +folder' (do not add the subfolders to your path). + + +---------------------------------------- +Trying out the examples +---------------------------------------- + +The examples are located in the 'gtsam_examples' subfolder. You may either +run them individually at the MATLAB command line, or open the GTSAM example +GUI by running 'gtsamExamples'. Example: + +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_examples % Change to the examples directory +>> gtsamExamples % Run the GTSAM examples GUI + + +---------------------------------------- +Running the unit tests +---------------------------------------- + +The GTSAM MATLAB toolbox also has a small set of unit tests located in the +gtsam_tests directory. Example: + +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_tests % Change to the examples directory +>> test_gtsam % Run the unit tests +Starting: testJacobianFactor +Starting: testKalmanFilter +Starting: testLocalizationExample +Starting: testOdometryExample +Starting: testPlanarSLAMExample +Starting: testPose2SLAMExample +Starting: testPose3SLAMExample +Starting: testSFMExample +Starting: testStereoVOExample +Starting: testVisualISAMExample +Tests complete! + + +---------------------------------------- +Writing your own code +---------------------------------------- + +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you +understand a few basic concepts! Please see the manual to get started. From 3b897cddc9603b38006fd2ba6fe6e803fda256c2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 19:58:33 +0000 Subject: [PATCH 02/53] Added eliminateFrontals function to FactorGraph, SymbolicFactorGraph, and GaussianFactorGraph - eliminates requested number of frontal variables and returns the resulting conditional and remaining factor graph --- gtsam.h | 5 +++ gtsam/inference/FactorGraph-inl.h | 52 +++++++++++++++++++++++ gtsam/inference/FactorGraph.h | 7 +++ gtsam/inference/SymbolicFactorGraph.cpp | 7 +++ gtsam/inference/SymbolicFactorGraph.h | 16 +++++-- gtsam/inference/tests/testFactorGraph.cpp | 30 ++++++++++++- gtsam/linear/GaussianFactorGraph.cpp | 7 +++ gtsam/linear/GaussianFactorGraph.h | 10 +++++ 8 files changed, 130 insertions(+), 4 deletions(-) diff --git a/gtsam.h b/gtsam.h index aebbea8b8..4aa1480df 100644 --- a/gtsam.h +++ b/gtsam.h @@ -787,6 +787,8 @@ class SymbolicFactorGraph { // Standard interface // FIXME: Must wrap FastSet for this to work //FastSet keys() const; + + pair eliminateFrontals(size_t nFrontals) const; }; #include @@ -996,6 +998,9 @@ class GaussianFactorGraph { size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; + // Inference + pair eliminateFrontals(size_t nFrontals) const; + // Building the graph void push_back(gtsam::GaussianFactor* factor); void add(Vector b); diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 66c44db52..6bd9c340c 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -23,6 +23,7 @@ #pragma once #include +#include #include #include @@ -85,6 +86,57 @@ namespace gtsam { return size_; } + /* ************************************************************************* */ + template + std::pair::sharedConditional, FactorGraph > + FactorGraph::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const + { + // Build variable index + VariableIndex variableIndex(*this); + + // Find first variable + Index firstIndex = 0; + while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty()) + ++ firstIndex; + + // Check that number of variables is in bounds + if(firstIndex + nFrontals >= variableIndex.size()) + throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); + + // Get set of involved factors + FastSet involvedFactorIs; + for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) { + BOOST_FOREACH(size_t i, variableIndex[j]) { + involvedFactorIs.insert(i); + } + } + + // Separate factors into involved and remaining + FactorGraph involvedFactors; + FactorGraph remainingFactors; + FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); + for(size_t i = 0; i < this->size(); ++i) { + if(*involvedFactorIsIt == i) { + // If the current factor is involved, add it to involved and increment involved iterator + involvedFactors.push_back((*this)[i]); + ++ involvedFactorIsIt; + } else { + // If not involved, add to remaining + remainingFactors.push_back((*this)[i]); + } + } + + // Do dense elimination on the involved factors + typename FactorGraph::EliminationResult eliminationResult = + eliminate(involvedFactors, nFrontals); + + // Add the remaining factor back into the factor graph + remainingFactors.push_back(eliminationResult.second); + + // Return the eliminated factor and remaining factor graph + return std::make_pair(eliminationResult.first, remainingFactors); + } + /* ************************************************************************* */ template void FactorGraph::replace(size_t index, sharedFactor factor) { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 5295a4c58..ed3e7d952 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -175,6 +175,13 @@ template class BayesTree; /** Get the last factor */ sharedFactor back() const { return factors_.back(); } + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. + */ + std::pair > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; + /// @} /// @name Modifying Factor Graphs (imperative, discouraged) /// @{ diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 00f3439a0..ff7a91ca6 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -63,6 +63,13 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::pair + SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const + { + return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); + } + /* ************************************************************************* */ IndexFactor::shared_ptr CombineSymbolic( const FactorGraph& factors, const FastMap SymbolicFactorGraph(const FactorGraph& fg); + + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. Note that this version simply calls + * FactorGraph::eliminateFrontals with EliminateSymbolic + * as the eliminate function argument. + */ + std::pair eliminateFrontals(size_t nFrontals) const; /// @} /// @name Standard Interface @@ -68,6 +77,8 @@ namespace gtsam { */ FastSet keys() const; + + /// @} /// @name Advanced Interface /// @{ @@ -87,9 +98,8 @@ namespace gtsam { }; /** Create a combined joint factor (new style for EliminationTree). */ - IndexFactor::shared_ptr CombineSymbolic( - const FactorGraph& factors, const FastMap >& variableSlots); + IndexFactor::shared_ptr CombineSymbolic(const FactorGraph& factors, + const FastMap >& variableSlots); /** * CombineAndEliminate provides symbolic elimination. diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 4743d1102..6121a7f99 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -21,16 +21,44 @@ #include #include #include // for operator += +#include using namespace boost::assign; #include +#include #include using namespace std; using namespace gtsam; -typedef boost::shared_ptr shared; +/* ************************************************************************* */ +TEST(FactorGraph, eliminateFrontals) { + + SymbolicFactorGraph sfgOrig; + sfgOrig.push_factor(0,1); + sfgOrig.push_factor(0,2); + sfgOrig.push_factor(1,3); + sfgOrig.push_factor(1,4); + sfgOrig.push_factor(2,3); + sfgOrig.push_factor(4,5); + + IndexConditional::shared_ptr actualCond; + SymbolicFactorGraph actualSfg; + boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2, EliminateSymbolic); + + vector condIndices; + condIndices += 0,1,2,3,4; + IndexConditional expectedCond(condIndices, 2); + + SymbolicFactorGraph expectedSfg; + expectedSfg.push_factor(2,3); + expectedSfg.push_factor(4,5); + expectedSfg.push_factor(2,3,4); + + EXPECT(assert_equal(expectedSfg, actualSfg)); + EXPECT(assert_equal(expectedCond, *actualCond)); +} ///* ************************************************************************* */ // SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index e2598b1ac..02f37915c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -47,6 +47,13 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::pair + GaussianFactorGraph::eliminateFrontals(size_t nFrontals) const + { + return FactorGraph::eliminateFrontals(nFrontals, EliminateQR); + } + /* ************************************************************************* */ void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fd1ec1f6c..51748b79b 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -132,6 +132,16 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + + /** Eliminate the first \c n frontal variables, returning the resulting + * conditional and remaining factor graph - this is very inefficient for + * eliminating all variables, to do that use EliminationTree or + * JunctionTree. Note that this version simply calls + * FactorGraph::eliminateFrontals with EliminateQR as the + * eliminate function argument. + */ + std::pair eliminateFrontals(size_t nFrontals) const; + /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); From 09558a146aa38817c93979e90fe87b0e882a1e72 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 20:13:36 +0000 Subject: [PATCH 03/53] Fixed error in unit test --- gtsam/inference/tests/testFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 6121a7f99..b2f9bc3f1 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -45,7 +45,7 @@ TEST(FactorGraph, eliminateFrontals) { IndexConditional::shared_ptr actualCond; SymbolicFactorGraph actualSfg; - boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2, EliminateSymbolic); + boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); vector condIndices; condIndices += 0,1,2,3,4; From 49a704c565f49df8e09668a63f0f57eb55c7df9a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 30 Aug 2012 20:17:27 +0000 Subject: [PATCH 04/53] Install MATLAB toolbox README along with toolbox --- matlab/CMakeLists.txt | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index b4e43d3d6..0be3863a1 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -1,13 +1,5 @@ # Install matlab components -# Utility functions -message(STATUS "Installing Matlab Utility Functions") -# Matlab files: *.m and *.fig -file(GLOB matlab_utils_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.m") -file(GLOB matlab_utils_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.fig") -set(matlab_utils ${matlab_utils_m} ${matlab_utils_fig}) -install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/) - # Tests message(STATUS "Installing Matlab Toolbox Tests") install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") @@ -21,6 +13,7 @@ install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION " # Utilities message(STATUS "Installing Matlab Toolbox Utilities") install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt From b6e9f7d3f7efebf1ac61a21e0af33ff20ea52dfc Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Thu, 30 Aug 2012 21:19:00 +0000 Subject: [PATCH 05/53] Added info on MEX_COMMAND for building toolbox from source --- README | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README b/README index 729c3fdf1..1a992292a 100644 --- a/README +++ b/README @@ -148,6 +148,12 @@ just the geometry tests, run "make check.geometry". Individual tests can be run appending ".run" to the name of the test, for example, to run testMatrix, run "make testMatrix.run". +MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your +shell's PATH environment variable. mex is installed with matlab at +$MATLABROOT/bin/mex + +$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB + Debugging tips: Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks From e5fe979ea524f3ef52bd621d9f661de943f61cef Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 31 Aug 2012 02:19:00 +0000 Subject: [PATCH 06/53] Fixed incorrect test case in testFactorGraph --- .cproject | 338 ++++++++++++---------- gtsam/inference/tests/testFactorGraph.cpp | 1 - 2 files changed, 178 insertions(+), 161 deletions(-) diff --git a/.cproject b/.cproject index 95c0f2a96..09c498789 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -823,6 +829,14 @@ true true + + make + -j5 + testFactorGraph.run + true + true + true + make -j2 @@ -985,6 +999,7 @@ make + testGraph.run true false @@ -992,6 +1007,7 @@ make + testJunctionTree.run true false @@ -999,6 +1015,7 @@ make + testSymbolicBayesNetB.run true false @@ -1134,6 +1151,7 @@ make + testErrors.run true false @@ -1179,10 +1197,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1267,10 +1285,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1605,7 +1623,6 @@ make - testSimulated2DOriented.run true false @@ -1645,7 +1662,6 @@ make - testSimulated2D.run true false @@ -1653,7 +1669,6 @@ make - testSimulated3D.run true false @@ -1845,7 +1860,6 @@ make - tests/testGaussianISAM2 true false @@ -1867,102 +1881,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 -j1 @@ -2164,6 +2082,7 @@ cpack + -G DEB true false @@ -2171,6 +2090,7 @@ cpack + -G RPM true false @@ -2178,6 +2098,7 @@ cpack + -G TGZ true false @@ -2185,6 +2106,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2318,34 +2240,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2389,6 +2375,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index b2f9bc3f1..09e75d7e6 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -52,7 +52,6 @@ TEST(FactorGraph, eliminateFrontals) { IndexConditional expectedCond(condIndices, 2); SymbolicFactorGraph expectedSfg; - expectedSfg.push_factor(2,3); expectedSfg.push_factor(4,5); expectedSfg.push_factor(2,3,4); From fc1a43e5855477cbd5018dc04d7cfe1b44b480f0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 31 Aug 2012 15:18:36 +0000 Subject: [PATCH 07/53] Reverting - Fixed incorrect test case in testFactorGraph (reverse-merged from commit 34e4c87523093f77f26a5c378763a715d56452b7) --- .cproject | 338 ++++++++++------------ gtsam/inference/tests/testFactorGraph.cpp | 1 + 2 files changed, 161 insertions(+), 178 deletions(-) diff --git a/.cproject b/.cproject index 09c498789..95c0f2a96 100644 --- a/.cproject +++ b/.cproject @@ -309,6 +309,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +343,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +350,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +397,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +404,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +411,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +426,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +519,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +535,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +575,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -685,26 +679,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -829,14 +823,6 @@ true true - - make - -j5 - testFactorGraph.run - true - true - true - make -j2 @@ -999,7 +985,6 @@ make - testGraph.run true false @@ -1007,7 +992,6 @@ make - testJunctionTree.run true false @@ -1015,7 +999,6 @@ make - testSymbolicBayesNetB.run true false @@ -1151,7 +1134,6 @@ make - testErrors.run true false @@ -1197,10 +1179,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1285,10 +1267,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1623,6 +1605,7 @@ make + testSimulated2DOriented.run true false @@ -1662,6 +1645,7 @@ make + testSimulated2D.run true false @@ -1669,6 +1653,7 @@ make + testSimulated3D.run true false @@ -1860,6 +1845,7 @@ make + tests/testGaussianISAM2 true false @@ -1881,6 +1867,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 -j1 @@ -2082,7 +2164,6 @@ cpack - -G DEB true false @@ -2090,7 +2171,6 @@ cpack - -G RPM true false @@ -2098,7 +2178,6 @@ cpack - -G TGZ true false @@ -2106,7 +2185,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2240,98 +2318,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2375,38 +2389,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 09e75d7e6..b2f9bc3f1 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -52,6 +52,7 @@ TEST(FactorGraph, eliminateFrontals) { IndexConditional expectedCond(condIndices, 2); SymbolicFactorGraph expectedSfg; + expectedSfg.push_factor(2,3); expectedSfg.push_factor(4,5); expectedSfg.push_factor(2,3,4); From 3948d70b206cd58c7fd971e2205318220c8b7a1d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 31 Aug 2012 15:18:39 +0000 Subject: [PATCH 08/53] Fixed iterator checking bug in eliminateFrontals --- gtsam/inference/FactorGraph-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 6bd9c340c..97a7fa1c6 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -116,7 +116,7 @@ namespace gtsam { FactorGraph remainingFactors; FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); for(size_t i = 0; i < this->size(); ++i) { - if(*involvedFactorIsIt == i) { + if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) { // If the current factor is involved, add it to involved and increment involved iterator involvedFactors.push_back((*this)[i]); ++ involvedFactorIsIt; From a058ecb692cc63f432c243dcdc711eca51f918c5 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 31 Aug 2012 19:22:07 +0000 Subject: [PATCH 09/53] disabled installing doc during creation of deb packages --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 85984d7e0..6349d440a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,7 +171,8 @@ 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_INSTALLED_DIRECTORIES ".") set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makestats.sh$") set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") From ab7594e8f084e710be63590813c0b2d9f2b84935 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Sep 2012 00:06:13 +0000 Subject: [PATCH 10/53] Made SPCG unit tests compile again, needed several fixes in iterative.h --- gtsam/linear/SubgraphPreconditioner.h | 18 +- gtsam/linear/iterative-inl.h | 2 +- gtsam/linear/iterative.cpp | 69 +++--- gtsam/linear/iterative.h | 17 +- tests/smallExample.cpp | 18 +- tests/smallExample.h | 2 +- tests/testSubgraphPreconditioner.cpp | 304 +++++++++++++------------- 7 files changed, 227 insertions(+), 203 deletions(-) diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 7b59735d9..8c1fcc837 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -48,6 +48,7 @@ namespace gtsam { public: SubgraphPreconditioner(); + /** * Constructor * @param Ab1: the Graph A1*x=b1 @@ -55,7 +56,8 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar); /** Access Ab1 */ const sharedFG& Ab1() const { return Ab1_; } @@ -69,23 +71,23 @@ namespace gtsam { /** Access b2bar */ const sharedErrors b2bar() const { return b2bar_; } - /** - * Add zero-mean i.i.d. Gaussian prior terms to each variable - * @param sigma Standard deviation of Gaussian - */ -// SubgraphPreconditioner add_priors(double sigma) const; + /** + * Add zero-mean i.i.d. Gaussian prior terms to each variable + * @param sigma Standard deviation of Gaussian + */ +// SubgraphPreconditioner add_priors(double sigma) const; /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(VectorValues::Zero(*xbar_)) ; + VectorValues V(VectorValues::Zero(*xbar_)); return V ; } /** - * Add constraint part of the error only, used in both calls above + * Add constraint part of the error only * y += alpha*inv(R1')*A2'*e2 * Takes a range indicating e2 !!!! */ diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 8a052a66f..5cc0841b0 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -122,7 +122,7 @@ namespace gtsam { // conjugate gradient method. // S: linear system, V: step vector, E: errors template - V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest = false) { + V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest) { CGState state(Ab, x, parameters, steepest); diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 6dadb2d72..d0a7fd551 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -22,50 +22,57 @@ #include #include - #include using namespace std; namespace gtsam { - /* ************************************************************************* */ - void System::print (const string& s) const { - cout << s << endl; - gtsam::print(A_, "A"); - gtsam::print(b_, "b"); - } + /* ************************************************************************* */ + void System::print(const string& s) const { + cout << s << endl; + gtsam::print(A_, "A"); + gtsam::print(b_, "b"); + } - /* ************************************************************************* */ + /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters, true); - } + Vector steepestDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters, true); - } + /* ************************************************************************* */ + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, + const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, + const Vector& x, const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - VectorValues steepestDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters, true); - } + /* ************************************************************************* */ + VectorValues steepestDescent(const FactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients, VectorValues, Errors>( + fg, x, parameters, true); + } - VectorValues conjugateGradientDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters); - } + VectorValues conjugateGradientDescent(const FactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients, VectorValues, Errors>( + fg, x, parameters); + } - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 5ffb366c2..384dfe814 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -31,12 +31,11 @@ namespace gtsam { * "Vector" class E needs dot(v,v) * @param Ab, the "system" that needs to be solved, examples below * @param x is the initial estimate - * @param epsilon determines the convergence criterion: norm(g) - V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false); + template + V conjugateGradients(const S& Ab, V x, + const ConjugateGradientParameters ¶meters, bool steepest = false); /** * Helper class encapsulating the combined system |Ax-b_|^2 @@ -127,21 +126,19 @@ namespace gtsam { const Vector& x, const ConjugateGradientParameters & parameters); - class GaussianFactorGraph; - /** * Method of steepest gradients, Gaussian Factor Graph version - * */ + */ VectorValues steepestDescent( - const GaussianFactorGraph& fg, + const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); /** * Method of conjugate gradients (CG), Gaussian Factor Graph version - * */ + */ VectorValues conjugateGradientDescent( - const GaussianFactorGraph& fg, + const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 100a4db4c..f81ab5528 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -421,7 +421,7 @@ namespace example { } /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { + boost::tuple planarGraph(size_t N) { // create empty graph NonlinearFactorGraph nlfg; @@ -458,7 +458,13 @@ namespace example { xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); // linearize around zero - return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue); + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + + JacobianFactorGraph jfg; + BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg) + jfg.push_back(boost::dynamic_pointer_cast(factor)); + + return boost::make_tuple(jfg, xtrue); } /* ************************************************************************* */ @@ -476,21 +482,21 @@ namespace example { JacobianFactorGraph T, C; // Add the x11 constraint to the tree - T.push_back(original[0]); + T.push_back(boost::dynamic_pointer_cast(original[0])); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); + T.push_back(boost::dynamic_pointer_cast(original[i])); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(original[i]); + T.push_back(boost::dynamic_pointer_cast(original[i])); else - C.push_back(original[i]); + C.push_back(boost::dynamic_pointer_cast(original[i])); return make_pair(T, C); } diff --git a/tests/smallExample.h b/tests/smallExample.h index 161c45da8..54c3a14c5 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -130,7 +130,7 @@ namespace gtsam { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ - boost::tuple planarGraph(size_t N); + boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 417643e78..60f737d67 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -35,30 +36,41 @@ using namespace gtsam; using namespace example; // define keys -Key i3003 = 3003, i2003 = 2003, i1003 = 1003; -Key i3002 = 3002, i2002 = 2002, i1002 = 1002; -Key i3001 = 3001, i2001 = 2001, i1001 = 1001; +// Create key for simulated planar graph +Symbol key(int x, int y) { + return symbol_shorthand::X(1000*x+y); +} -// TODO fix Ordering::equals, because the ordering *is* correct ! /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, planarOrdering ) -//{ -// // Check canonical ordering -// Ordering expected, ordering = planarOrdering(3); -// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; -// CHECK(assert_equal(expected,ordering)); -//} +TEST( SubgraphPreconditioner, planarOrdering ) { + // Check canonical ordering + Ordering expected, ordering = planarOrdering(3); + expected += + key(3, 3), key(2, 3), key(1, 3), + key(3, 2), key(2, 2), key(1, 2), + key(3, 1), key(2, 1), key(1, 1); + CHECK(assert_equal(expected,ordering)); +} + +/* ************************************************************************* */ +/** unnormalized error */ +double error(const JacobianFactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + total_error += factor->error(x); + return total_error; +} /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction - GaussianFactorGraph A; + JacobianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue + DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue // Check that xtrue is optimal GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); @@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph ) } /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, splitOffPlanarTree ) -//{ -// // Build a planar graph -// GaussianFactorGraph A; -// VectorValues xtrue; -// boost::tie(A, xtrue) = planarGraph(3); -// -// // Get the spanning tree and constraints, and check their sizes -// JacobianFactorGraph T, C; -// // TODO big mess: GFG and JFG mess !!! -// boost::tie(T, C) = splitOffPlanarTree(3, A); -// LONGS_EQUAL(9,T.size()); -// LONGS_EQUAL(4,C.size()); -// -// // Check that the tree can be solved to give the ground xtrue -// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); -// VectorValues xbar = optimize(*R1); -// CHECK(assert_equal(xtrue,xbar)); -//} +TEST( SubgraphPreconditioner, splitOffPlanarTree ) +{ + // Build a planar graph + JacobianFactorGraph A; + VectorValues xtrue; + boost::tie(A, xtrue) = planarGraph(3); + + // Get the spanning tree and constraints, and check their sizes + JacobianFactorGraph T, C; + boost::tie(T, C) = splitOffPlanarTree(3, A); + LONGS_EQUAL(9,T.size()); + LONGS_EQUAL(4,C.size()); + + // Check that the tree can be solved to give the ground xtrue + GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); + VectorValues xbar = optimize(*R1); + CHECK(assert_equal(xtrue,xbar)); +} /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, system ) -//{ -// // Build a planar graph -// JacobianFactorGraph Ab; -// VectorValues xtrue; -// size_t N = 3; -// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b -// -// // Get the spanning tree and corresponding ordering -// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 -// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config -// VectorValues zeros = VectorValues::Zero(xbar); -// -// // Set up y0 as all zeros -// VectorValues y0 = zeros; -// -// // y1 = perturbed y0 -// VectorValues y1 = zeros; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// -// // Check corresponding x values -// VectorValues expected_x1 = xtrue, x1 = system.x(y1); -// expected_x1[i2003] = Vector_(2, 2.01, 2.99); -// expected_x1[i3003] = Vector_(2, 3.01, 2.99); -// CHECK(assert_equal(xtrue, system.x(y0))); -// CHECK(assert_equal(expected_x1,system.x(y1))); -// -// // Check errors -//// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! -//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! -// DOUBLES_EQUAL(0,error(system,y0),1e-9); -// DOUBLES_EQUAL(3,error(system,y1),1e-9); -// -// // Test gradient in x -// VectorValues expected_gx0 = zeros; -// VectorValues expected_gx1 = zeros; -// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); -// expected_gx1[i1003] = Vector_(2, -100., 100.); -// expected_gx1[i2002] = Vector_(2, -100., 100.); -// expected_gx1[i2003] = Vector_(2, 200., -200.); -// expected_gx1[i3002] = Vector_(2, -100., 100.); -// expected_gx1[i3003] = Vector_(2, 100., -100.); -// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); -// -// // Test gradient in y -// VectorValues expected_gy0 = zeros; -// VectorValues expected_gy1 = zeros; -// expected_gy1[i1003] = Vector_(2, 2., -2.); -// expected_gy1[i2002] = Vector_(2, -2., 2.); -// expected_gy1[i2003] = Vector_(2, 3., -3.); -// expected_gy1[i3002] = Vector_(2, -1., 1.); -// expected_gy1[i3003] = Vector_(2, 1., -1.); -// CHECK(assert_equal(expected_gy0,gradient(system,y0))); -// CHECK(assert_equal(expected_gy1,gradient(system,y1))); -// -// // Check it numerically for good measure -// // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) -// // Vector numerical_g1 = numericalGradient (error, y1, 0.001); -// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., -// // 3., -3., 0., 0., -1., 1., 1., -1.); -// // CHECK(assert_equal(expected_g1,numerical_g1)); -//} + +TEST( SubgraphPreconditioner, system ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + + // Create zero config + VectorValues zeros = VectorValues::Zero(xbar); + + // Set up y0 as all zeros + VectorValues y0 = zeros; + + // y1 = perturbed y0 + VectorValues y1 = zeros; + y1[1] = Vector_(2, 1.0, -1.0); + + // Check corresponding x values + VectorValues expected_x1 = xtrue, x1 = system.x(y1); + expected_x1[1] = Vector_(2, 2.01, 2.99); + expected_x1[0] = Vector_(2, 3.01, 2.99); + CHECK(assert_equal(xtrue, system.x(y0))); + CHECK(assert_equal(expected_x1,system.x(y1))); + + // Check errors + DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); + DOUBLES_EQUAL(3,error(Ab,x1),1e-9); + DOUBLES_EQUAL(0,error(system,y0),1e-9); + DOUBLES_EQUAL(3,error(system,y1),1e-9); + + // Test gradient in x + VectorValues expected_gx0 = zeros; + VectorValues expected_gx1 = zeros; + CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); + expected_gx1[2] = Vector_(2, -100., 100.); + expected_gx1[4] = Vector_(2, -100., 100.); + expected_gx1[1] = Vector_(2, 200., -200.); + expected_gx1[3] = Vector_(2, -100., 100.); + expected_gx1[0] = Vector_(2, 100., -100.); + CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); + + // Test gradient in y + VectorValues expected_gy0 = zeros; + VectorValues expected_gy1 = zeros; + expected_gy1[2] = Vector_(2, 2., -2.); + expected_gy1[4] = Vector_(2, -2., 2.); + expected_gy1[1] = Vector_(2, 3., -3.); + expected_gy1[3] = Vector_(2, -1., 1.); + expected_gy1[0] = Vector_(2, 1., -1.); + CHECK(assert_equal(expected_gy0,gradient(system,y0))); + CHECK(assert_equal(expected_gy1,gradient(system,y1))); + + // Check it numerically for good measure + // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) + // Vector numerical_g1 = numericalGradient (error, y1, 0.001); + // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., + // 3., -3., 0., 0., -1., 1., 1., -1.); + // CHECK(assert_equal(expected_g1,numerical_g1)); +} /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, conjugateGradients ) -//{ -// // Build a planar graph -// GaussianFactorGraph Ab; -// VectorValues xtrue; -// size_t N = 3; -// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b -// -// // Get the spanning tree and corresponding ordering -// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 -// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// Ordering ordering = planarOrdering(N); -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config y0 and perturbed config y1 -// VectorValues y0 = VectorValues::Zero(xbar); -// -// VectorValues y1 = y0; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// VectorValues x1 = system.x(y1); -// -// // Solve for the remaining constraints using PCG -// ConjugateGradientParameters parameters; -//// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); -//// CHECK(assert_equal(y0,actual)); -// -// // Compare with non preconditioned version: -// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); -// CHECK(assert_equal(xtrue,actual2,1e-4)); -//} +TEST( SubgraphPreconditioner, conjugateGradients ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + + // Create zero config y0 and perturbed config y1 + VectorValues y0 = VectorValues::Zero(xbar); + + VectorValues y1 = y0; + y1[1] = Vector_(2, 1.0, -1.0); + VectorValues x1 = system.x(y1); + + // Solve for the remaining constraints using PCG + ConjugateGradientParameters parameters; + VectorValues actual = conjugateGradients(system, y1, parameters); + CHECK(assert_equal(y0,actual)); + + // Compare with non preconditioned version: + VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); + CHECK(assert_equal(xtrue,actual2,1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 832072c940fa866befd4e30bb1f097ba6e83c69c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Sep 2012 00:13:35 +0000 Subject: [PATCH 11/53] Fixed path --- matlab/gtsam_examples/PlanarSLAMExample_graph.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m index a65ea5980..9ca88e49a 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m @@ -15,7 +15,7 @@ clear import gtsam.* %% Find data file -datafile = '/Users/dellaert/borg/gtsam/examples/Data/example.graph'; +datafile = findExampleDataFile('example.graph'); %% Initialize graph, initial estimate, and odometry noise model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); From 2285b14c5f61d7604fb8edb16b56373db4fb6b92 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 00:39:02 +0000 Subject: [PATCH 12/53] Alternate matrix operation --- gtsam_unstable/slam/PoseRotationPrior.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index 60a29b3e4..30d3828fd 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -67,9 +67,9 @@ public: if (H) { *H = gtsam::zeros(rDim, xDim); if (pose_traits::isRotFirst()) - (*H).leftCols(rDim) = eye(rDim); + (*H).leftCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); // FIXME: avoiding use of assign with eye() else - (*H).rightCols(rDim) = eye(rDim); + (*H).rightCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); } return Rotation::Logmap(newR) - Rotation::Logmap(measured_); From 891fb2e038652c37dc5c9dbaea546e69da75b763 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 00:50:24 +0000 Subject: [PATCH 13/53] comments only --- gtsam_unstable/slam/PoseRotationPrior.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index 30d3828fd..9a56ec891 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -67,9 +67,9 @@ public: if (H) { *H = gtsam::zeros(rDim, xDim); if (pose_traits::isRotFirst()) - (*H).leftCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); // FIXME: avoiding use of assign with eye() + (*H).leftCols(rDim).setIdentity(rDim, rDim); else - (*H).rightCols(rDim).setIdentity(rDim, rDim); // = eye(rDim); + (*H).rightCols(rDim).setIdentity(rDim, rDim); } return Rotation::Logmap(newR) - Rotation::Logmap(measured_); From 55c9bdb3ac7aa121f061ec03ab0193b31c4b8913 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 00:58:05 +0000 Subject: [PATCH 14/53] Added notes on use of eye() and zeros() when using Eigen block operations --- gtsam/base/Matrix.h | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6902a4a88..e6a40f727 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -67,11 +67,30 @@ inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);} */ Matrix Matrix_(size_t m, size_t n, ...); +// Matlab-like syntax + /** - * MATLAB like constructors + * Creates an zeros matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros, + * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error. */ Matrix zeros(size_t m, size_t n); + +/** + * Creates an identity matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, + * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error. + */ Matrix eye(size_t m, size_t n); + +/** + * Creates a square identity matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, + * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error. + */ inline Matrix eye( size_t m ) { return eye(m,m); } Matrix diag(const Vector& v); From bd6cb53d5ee26eb53ac50fc042998fb56c00ec6f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 01:45:12 +0000 Subject: [PATCH 15/53] Updated README for make doc --- README | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README b/README index 1a992292a..8e74f61e4 100644 --- a/README +++ b/README @@ -72,8 +72,8 @@ will run up to 10x faster in Release mode! See the end of this document for additional debugging tips. 3) -GTSAM has Doxygen documentation. To generate, run the the script -makedoc.sh. +GTSAM has Doxygen documentation. To generate, run 'make doc' from your +build directory. 4) The instructions below install the library to the default system install path and From d8e0a9589039128b963bdc00cefa28b19ce4a2ff Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 03:06:08 +0000 Subject: [PATCH 16/53] Small bug-fixes for managing linearization points --- gtsam_unstable/nonlinear/LinearContainerFactor.cpp | 2 +- gtsam_unstable/nonlinear/LinearContainerFactor.h | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index fa44d3e93..e6c49b318 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -205,7 +205,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& orderin /* ************************************************************************* */ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place - return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor)); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 9affcc29e..a8f7cd399 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -23,6 +23,11 @@ protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; + /** direct copy constructor */ + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const boost::optional& linearizationPoint) + : factor_(factor), linearizationPoint_(linearizationPoint) {} + public: /** Primary constructor: store a linear factor and decode the ordering */ @@ -105,7 +110,7 @@ public: * Clones the underlying linear factor */ NonlinearFactor::shared_ptr clone() const { - return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_)); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_)); } // casting syntactic sugar From 3522d09c4eb573797f34744c450a5276f08ce759 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 03:06:09 +0000 Subject: [PATCH 17/53] Adding options for making deb packages --- CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6349d440a..3df8ab232 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -161,7 +161,7 @@ if (DOXYGEN_FOUND) add_subdirectory(doc) endif() -# Set up CPack +## 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") @@ -172,12 +172,16 @@ 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 ".") -set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makestats.sh$") +#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error +set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") 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 +# Deb-package specific cpack +set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") + # print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") From d3b5c463edd4c946b518bbf8d40b92ccc45701d9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 3 Sep 2012 17:22:09 +0000 Subject: [PATCH 18/53] Fixed initialization bug --- gtsam_unstable/nonlinear/LinearContainerFactor.cpp | 9 +++++++++ gtsam_unstable/nonlinear/LinearContainerFactor.h | 3 +-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index e6c49b318..b5701fb13 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -38,6 +38,15 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza } } +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const boost::optional& linearizationPoint) +: factor_(factor), linearizationPoint_(linearizationPoint) { + // Extract keys stashed in linear factor + BOOST_FOREACH(const Index& idx, factor_->keys()) + keys_.push_back(idx); +} + /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const JacobianFactor& factor, const Ordering& ordering, diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index a8f7cd399..324556622 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -25,8 +25,7 @@ protected: /** direct copy constructor */ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const boost::optional& linearizationPoint) - : factor_(factor), linearizationPoint_(linearizationPoint) {} + const boost::optional& linearizationPoint); public: From 61b6c72363e7908c0e0029507e7b6f7b0658214c Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 3 Sep 2012 17:56:22 +0000 Subject: [PATCH 19/53] Added PlanarSLAMExample_Graph to the gui --- matlab/gtsam_examples/gtsamExamples.fig | Bin 10561 -> 10933 bytes matlab/gtsam_examples/gtsamExamples.m | 9 ++++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/gtsamExamples.fig b/matlab/gtsam_examples/gtsamExamples.fig index e51c23468d21978f6cdd6c9f8d5d4650085db138..ca48437dae84bc997488acd393909553f61e90f9 100644 GIT binary patch literal 10933 zcma)iXHXNs7A}Z@B7#z-BZ~CiJ1D)2(tDTQTL=V1DbkztB1L*fIs~NGfb<$5^h5%L zPC}md-uLJIduMib&+N{eJu_#%J>Q;PErpMo3U8T3c=?&N6h87eIJw$$GwZrq`#N}d zyGb&uXlfgYi*hr+^R~D4wYOvTaF=A(@^EMVVDHJyEW|7zBq_o#Da6k#$S)ws{QoPA z960|ITs1Ayf7*9;9Gt*hgb~<-PK@Fw>$2aUrTQlnxDNlFGO2gLNtcOM=qSyL!Hw?i29ysGFdwuir#@~6AT zo$DNiy<(En146xMU!E6-%heyqi!$<5t2aB zZ>G;;Hd*=EE$(9pR^nxbBjI=3F(y>ca%|Y#1$f4I-uu#*%6e(CKNT!SP`RnRlsJ;} z`YkPATMkIlu_S+j#%3wCdrqVB^)KVDmoL~I*;%W4U{NZZX7uNrJw+rzVdj6TjIP9; zQax*;Ry$hxbWQuMXD=ga%a@ofeCf(MCa;GnlUJFyNOp;Wn!nAk)#M3 z&=-ak%}=%y;Nd8^C9;f3>3MGQaVkKiXSYqO`+MEW?PsN^<_dOqRr=3ad!5#iXq(kH z)hfa3RDNU@)z8xg(%#qoeOCKP7%oV6AcMAj?kwKZ&9lfQ+pfafP)xh)>1Cc%mWu2R@Sf<8 zD`U0|y_S;y7en#`!QsEJOYu@*&ReXQ*T z?fUT3?Lx(g*q5liM?)YB&7lGse%WS_8NG}}JJUPp{+Y}xToM7K`Mkik*AeT7PfGbu z`xfRavw`(cfWEGOcWtK(o_sJiGnRCge@9C3AJpBS+8)PXabgU~-2t=3Dttp#&q(vzjr+GSj;yiBxf zYNAaZ0UOBfAmn7Qhu8xg^F7Kln#DvXw$%n7 zcF0%+4Ln#rZ>HV2$~=7bYiAHN?)6<~k;^l6<9;c1N2)&!uIW55 zq(CS+h1{QcN;V{M$)>5>=>qJs%Uy@Y<-Z=3OE!F|6jAy^j`@&Xol;#w8xZr6r``&| zZ}YpHqQ3QBmuGl%(j+k0Ums;7YX0QLEVKMS z`#)G2pR|;^bxF}%XimQ(nhY)V^?+S!Bn4A9I@DcONd5Hq^eK9F{3+X;&4CvrKJ{Ol z2=WHq;`$Afv>BO|cuivtnJ*q#_iTNpqx9r(8__3kPM14sgt*tk5QsHk_F0{B*REPN zbnOgaF_IISe}zK7+`EbU>^BNQ5f<$CTkQw60x6$gDjjy%uGBx-eIj~F7_OYYh+d_5 zc7mM4XY1Z1ZKCV>AWiH2xJt2~C<@lDT`hVvu<+O;=(0O#vRu&GF(SeAr$4&M3zcag z>{`A{Y%qb~T1eOImJ~+q?^06j@pHgSqBDMa)hOs#o4fB64~>vFJ@YAbRL&4))T@4H zljl54%FXoWdFv-i7kwc!gObL0Sn0;%4y)RXDWoZt>D@F!1&}{pZ0W;i@wcRJ!>6lLS}|R=oHQ3DCZsB)Tl}&AAXY9MX^ZG|rG=0-lmw zG&YbL43Wt5v9P3)vp}JbbR@|R{ddmL3UF=d3~iwGr|+vOf6up8SL4?+Di#epMf1&W z^Ci1;y2YR0WfHA#tN=S&vYgp0&n}})g`Z4dmQE#n{bkp0zhI7S!DvlV(K7QBlFm9D zqw2e>Ca59(c+YQa0u~I0C0p^Sw=NrM>cx(6UHHIug*K3Uz3pHtctQu58PpDb@i(C- zcIsIClGxB&9#DSRb-NS?-Pa(E|mZbz^e>|ToNKUWUILWrl&qBMT5>A{#B`slSR`Qc=w%Qlqn+9>-kRz4a9q@Z*H0y)$@Wk}AfjVHs_1j$vp!uC8h*KE98@+p zZQqpF{IlTH;s!-d5_^fKLa03RyW9?M{Fw8PycB}_Onxy{>9Mg(y6@8myG;12_Z7?T zm?WDUBq$XB9;wL;N z4Iq$lWD=98g^Aoi9ds};zIYxWdll7e)tu&Kj8%pwT*r4_i+I5ZRP$xsdqNvf!eM@T9L4p!7Kt37vGKHEq4_)KzuaCt!kB-F$*+*LH} zQREFVGfVQ<4jutIq?)YnU&apm-npNm$WF{(w1JSh=%T{fz+wn2{^*n-FSsV?mETC$ z>G2k_0(FYihsF0u?!@%E3k7*el#eE3OS516|Mqc_1 zRkhOnXeYI#m*{u1Y}n5Y<1vfFrh|hEyD$esl=IPGdO1nnzs0U09CZnZ3yW zi&i78I0_|-+N@=BD`U@iIb$+S19Iz_OnjMoWwk4QZp2Y-tkn3)D~FoH+B}YmL+8#a z>!?8fY-Z~89Ka>ZzZ~2j6PT>1wfUO*)katZ8F^dzOyzx6MeX4)M?jwzG2h{Nm)M0x zcj(1YcikB9bibWcuDx`9ttA+fVd~+;N4i^Zvw51c;(*h8^8g1A-JLJj&s=z)1W`+5 z7P!}P0?OJ0Buxc!CeANJ_b{p-Q5@%*7L}7Eblbip>(fV$a6k$4WX* zr49fn7fV6%Bt*QFdozfSv(Wth1()V;%d!feKfiuFAAkeDfnwFW9wK>Vx;2=pAo|d^ z1gkOHr_ZvI`WQs^eh=*kx-LCzw_W90x?s-YrFNv&NJ`=q)^>+3|M=CkXMLK0_`9@q zS6X)cOW{=GWV*d!feZao+2|C2A^1$M_t^Kc zme2THq<1m1ld|g+XWLqZ%jhFNs6_c%^Sp_zEs4xn zv}ohvFRawYiw7Smg27%%n_c|A#Pq7HRzIE7h2^EA z_)c-WKi~N)xs>e`yE;CHI?`VIomvtd*9#QF1mxir@_L8RCok}U?^~kjmJiUi3; z{f*!_{|_p5a%Z{YyWXK@wV2qAj)zz<4%dtu&lGfe8_6|>T{L`Acb1&*3@Qy`=_sA_ut3;7Z-y|v}L>!0eEeuxr0 z^PN^{wN}9z)d!Z}4C0xUsB&;#w*|=ZO(>bj(a1;COAYg9Q414+(KfN|%X_;jX1;Z| zI(++E)u9eCI-Y#rPtnh}>*uGc$`07_xNh#Hzbe`ry>wsC%^+0p&*ix*P=d;Tx7=%7 zGnL8N4A9w^O?cNdz(v{!4OT|EOFU7Tk_|GYlQmkj+`RS|xuU;v0a4lj@WV9(>wMDB zEap`qI`ft{8!+&~_-8TbWl|K`-rqMpzD|cdwT`ksGYE_5lmOWOkvBty|B$!*?{5e2 z6pEiG($>D&nEPN#w8Z)B-4FKcOzW$LJn=l^0LOXnZAJ0GZR$_kgQ;S$c<^T43-P1) zg3Ss4)%83ImyEYBmoqdSp{;k5T|HSrsErqI55Bod%%#l8Lq@(o1+b>-!H9o6z5!ac zG(7q3qA}tUGermQ8&?h_6?(~6bt;&=@Z0CWps=ckh@>ygHQd-P?g{rJ$6tgsUWvLO zm<6ftJYN|8M0LH^u`pWU)#dS}#PKhA!;ryE*Y9njgOin?^yHJh`2I+ib@ic?ZMvQh zh>3+*(BuK#eK*=%Ka}&R+^k9kI`2<;g`RnJpFCpNXZtKR)01J|o@p-edNbk#Vo%yx zbO)sWx(}4{X)U~7CCB8Y(uLov-*4$Fe~H}|gk(*h(ztdt&>ssl4R8bixq(Ei;yxN} zGZNmPrkdrk{!@Lt+}`9xYbHwXOj;8xOFBD&Eo`0BowZ!7j{aaTv-z!7$_jkT=1z?x!FI|wy$uXWV1{af z1ZL$q$-`isM>Lby!J;0`ggg>q_+L6wj0+2Gh4NRqX}w&=GP5Y02UzL8kj4sP`N;!nMLwYqjWnp=!{Mmn6>l=yWH)Gb z;|wWOMs%Ikl%95<0>gzlCkid>AYBKaYXbVz&FgKbxu-LBS@E^&wOQ3&O-;4VI=;k9 zX5NOXl2L2klwGt<$BR+He);B*|0How38HbF<^oT{>=qUYG9c%+j$~N=bjN8sl#R}b z!i4ETAJgKsdd5DAp?t*iGZe5+XME!;vYEGV{~Cyw$+UMrjLdo=7jf*@&{+_yw+>Z9 zr$61(N)hA60M7o-BKv{X9vzKhm>{`qv2z!~w~oO*=RmbR)|fzo79IV9T(P=ESw7uYanQKSNdH~z@BHsPdree&$%Us0 zY1w}|(oV+vYZQE3P7I8|exe6@(9`efd?2kW-kR%ITL_96@2nKay%nvA_nQ)XY1#r<*Z?0ymM65{6cBK1ezk%BF8eyq z6_VHFGnFghvxm?e=J47X>w-pBoX^TQB6L&sDk7A{AL4l2XICf^CkN`g8{_djYt zwDd0ZeCEmQGd2BhgMupq{jyod z&8;Vo>t?g^KBA4R4_de_D~DQGxnGqIyAV&-f8j=P;W)EjYSRGIu5A_F{`ce7`mt*BG= z40DxGE9{#x7o`~Q z(%ewa4v0C!f$2~2O8PktD`EB@iyK{?IJ0UKZn`VDvLDr@f#!T zv;FS`I>X<{^t~&nNpotdaql|1X|stoVRUg5-nf{vsT?wy*Ay#NNPp1Vz^hhcUs(gN zpZ{5L`XWr}D*eO&wLcV@UR|o4=)|EjN|tHcYDo?dNSHPj(N_)4%vU4 z8y8*XjD+IHZ=RL%4&0RNb|h(dbm@3ByU}V!#naiFtxs1}QyZ|wDscg&zbrz_PZi{f z2UNtWsq|w-YqJ~UqXgXbqwgQ8F|7bW$A!8y54Nc;*V*^LZzKVvN1)7m@qa7dKSZKp zf|GT?szGm}pkIM(l%*MpF@_C7?rA>#*{#&md{ln99!x=}@MN*bkp0NF7~QjII+n5f z6o>}ej7&nu#7LUNUeYzc5gh0qL;-kz78LyL=cfd(ClZPGhHwdksl)f@XYm>eaLbQN zH|=Xqc*8y<@rLjYpJNxG^_+X1Ugmm*Hr=&)_NGdLWcTBv+I{(#Q+N0)A#+#T>^E`- zXI|WgG84h4;1DQ|!j;XkY<`2_UE@a%PF{|9fVR58Dt~hg=SDdrBDA}ZQ$2*;X9bmx zkV*u$jymR(mj-E{Pob74qyxCV-rRhdC8(`X>rAF@Y=fQjpI22j@`D)zR=VOi{Z|HP z=ecWZw#%Go-k7C1>BWc$@wmS)PgF}CdO@#;ItQifU|$bo6+c6ZZ@8zm&(T39JCNVY zGiQ#!SdOzB+=MNPou$hg+|=WJjomjxS0;S1-AOtTXv)Vu0d+b=T0*`-r9KVb8>Nfq&k%aFTsMMlZ9(JVDz#ZD0ERITgJ-S8Ysq zXkmBz1M1O@U-Argx4TZclA}EYh2zI}%u?c5rFeO~9f@#R?`!=+54F->zt3Lj7Xq~( zc+MpCqC$Sx7jz1yR&-SK@nO)?a+G1H#Qn9#+w`f z50@N0wGA^3N=&4Oy{vn+M>)i_`@#{J=mrD_i|BUF_9#0L`L>51FF*YRIU`3s`Kw9Y zrfC$xTy{Y1T zPrq5#dxKoZYfBT!MU~O{s$6L2s_oOS-g2RDhsKE{@gW+k?>VFjMfOJpb ztnhxMVTyuQZY~ui8dq$o5VW)3uefIfsh|46#HNGe?szfWR!VxjEvzalh4*6YKjFN> z2^jScH{HQ#$wS*v={Ma_96V!xDi54(D3;4D_c*Df*X<0>7k1x!I>Q)>gi7HYuuwi& z-{t!8L<64@{8|a zG4x<;@eLqCll*|VIl^mLI)7OtVwO^G#(e6KQ|LjOg+TR4T^ z4_zXWwM%@Cm3p^i)xyhWimtB&B}(MNqOoI+8q4;$kS}c$IwdnaEsvr;K4{(TDb~{nJ3(302>`&Z4K=?8{4w*t=$od_F6O zX8emzChW3_?N>1km&>_5{17HVR>s2b|K_8nTB@^ z511%J55{4oBmGfQviI6>5L&Xnf3BF2C9RtSiRFoem!w&ztywWib$)1uf2U92qHj_;jtBt&ormw>FmskMv+^^l=h@nVVBPLjKR-<;-V5gUk+O~6Po~{HuC|oNtQ)^sX9B$r>38ellKU$V zJ(A>AiQZ7MWpG5}Fha;ewx8b)awomXFA+?@uWBGjoYY`e=z}SeLDZRw;jTNZ%`nji zG)CMFcl7wsP3su&pC^By*4Des_S)UIHR~r{-!Ty7ZwM8L@L69u3;0C6`%J=*Y+8dV zfTsxI`wz+29n>HElgSJ4I@D&ydl^wz;_H&X15bhmI(}cng5!h|iW6W5(7L9uZioD7 zOZs6<5tEVSCqmS}S4rv9ix|Hzr)DVH6V7%qroedgFPy-o0f10`v10hJ)i*Z;e!dXfJF3Xa8kmbr*Z| zz5F%1%}0<@N&mdfBXC$dU)!}z4vOylBHmglHoBv_VjR|>x)iLV3cO-)4^Bcc!BR$YA9`IXmS z28IAU1V2yo-A|`M0cj2gpToJJWYho_Bnh3lE*sYnsIz(G@{M7tx7b_&|oAvjAhzg-~*&UqHbFKQ)92ZBkhSnLf0Xz@PI0?kqWsCvUQzsC!@5n)-N>jlnGiex~H|Z zaM2eEW^$Jmyu=McNkNL6Lg`-%4Rcc0Ufs(J7RUvl^M1k*xkOJ$Yg6ZcU!y-;=+%j_ zm$KrTe9afJ1OmtS`56J)!($m^Wb%u_x3NY~ARS(cqJXFp(EJ@tkm=arVU> z({mHk@and##NF{QT*K(Daq#O|Hh)BHli6fE^U6P zEaD~pwA#z+1-VFB8I)6Hj zIK`FEzKm?fy4%Ll9|Q$Auz?%kd* zExdF#JrPxiVcan*i`CDT<>k#OuCD(e`y6eGEuLb{W4KeXvX&cs6gJWfz9IHzxry}7)&7_2o#jvb{wE5}^@N-FI6S`Ja2 zu`QL7?(JZLY=j2(iDVLnVKoC>OY@K{y)z6pn1o|-;|>@zROwO{sa>TzYEUPA|KCET z2Jg(A9nJ$;W`7(TQRY1y8!CcFR$tF(!ZXR$Bf@af3NkYa46|}9O^7K8vuwkL>0ii; z68`icNRd?#_KTBG1?FKQ6o6{X+FX+wyrUD4ScvL`#;966AB1m=mseF&TS*XtH!5u* z98j+?8JR`8ts$Qokd?IarBJv7yj1^%v@tjS=7Wt6HYWybi5&ZC$Y}R^D;@OE9X*O5^{rS&!T*;r56twiK z56(Fe@dP+@!s@C2`JI^f@8}H7{cEJ5-OvAwL8>4$_5RD;E6<0Qs^#3ARNO1((dxQf zmCZ!V15b^-i4&%6J#jDT!&&`d)5GjMl^wXFuj2Ee<8hd6AQef>L9xmXS9N9ed&c~& zx4V@Y#UxJRF{mGVKgMLw$5ZZEeAdk^$$vTo0=$eTPz&o5vjP{}bnAXKd*NeE%hELV zr`}n1@9l0a3qeg5Q;C_R<{*SS7^$=G*e#qb3=1Etu=qe3mma#ivltsa%hnO3%68SH zM5_fm2jnXz#v(4=X>`aqNsWt6G2~Kug2(*Fg2w{KLKfD!au8aW-w<7#7!BzQQW_6| z4Tr8km9e*Jarm>W%ts{5N4(5OjLb)}%tw^+b5&vGi#YG=@QOBY6uTcs18|}NkD>uj zo?VDCA2G#?}SjV%=nl*5M2*SYnL*zKd;nMkMTp_89we-$k+qr#IfX{Qm%cvlX`h literal 10561 zcma)iXEYp8*EJCYi4rw>Z;9S}i{2tc?_Gi+dYd7lMkh#=QKR=>hv;=g7rmFkU@#cW zo9B7IZ>{g=cYd6E&pQ9^U1#mR*FM?`23iV=j3T`JjM@qYJdQ4|4sRIWx!QO;0zSJ* zGQLwXQr8g`e#5Br*}=x!!JZM|F3D)%e@vAZtVYR|LkMq30+z-GO@A9U^3z&%4wy>3~olI#t}BXLQbV&Ts5XV3<)>o z&W1IUjN23$B_;Hiy>ylg!w1r7O(<=Ub1{A3#*~JhneqpNd`r_SV@Ok%kX`!fQiMFs zdH>m7z}^xRvVVn&S0z!D0_>FmexCv~$b89eYyGD#OTv3W%2v$fg^kVHIS+9A4#~Azg-vf_ zw9;ZQK?^oWKTkB#WP?^5wb8_BPXG z+DI|SaXgF{P6p7uUFy_{hJDt*cF&@|drd^Q0-pN?nb$-I1q7HK(G^2J+~F333<8Y~ z!D+zaOFj+sugg!xAfvnZX%J^u$t4&q(B;&BJD552$u2i=E$X}eeS_y!EM!n&yTEh0 z)rD51n%XHIWwT2LD$~VBr;?V^yla=5YcS(9gZAX2N}tT78c{_|8&()#?HOc8+tqAs z4iC)X2Sj#Fv6(lo>CeYmi+)`BoZ*Qpv`tu$X9?Xk8onVX_V^+V9Eh_bxee33m__Mf zG1M|tR*&<(eV>t*TYGncM8Us4muu=?33(hiMUae@ z5wro!#2`Sc`w6qCA5LPq!GsQ;ILm|8W&_ue67C(hd1>>~RQJtMf!jkYqmT0DMDrA< zYi9Ui0-twm>1BF(tJoUrMj+73d5ER%dbFm8O1t_z&4tTXx}ycGD|bd?;X)B*&sUy! zdq3gz&u<5swf(OmJaqb>)%R^p>nfYJ^DM@Dl`IpE$)j#$i zvnW<>Vq5*jqSAeD7wHc87!u!Itg@Co#IdgP+Kki8iKpljg!2>1oX=tE+EQbO^@O_g zxhN91O6=JT>J$4#$Uek|+*wHOi6<@-T^))|57uOapxxlAj-Bo4ofW`myW2EkFU)&S z!2tiZp%63al4VKBPjLjzD?`=vG3YTXqBY&2IO&|rHY2Uw>flnZ8oGc}kw12;bH>J7 zKlLGYS5z|nAGQkKj)yZLcAgD_1Ty2~<{g6l(TCRP^UZ1saye2 zFe31HnfSPUw0s12c^4))x<}x8jxu%(>#H^Tashq)t)YU*32s1d_1dZ{5^(~-`T@j! zhkdp0#07~_1c)CY+DFKE55I$_igmG?dzD-@J1xCq;C5JiwE+)(^&(5g6?Pu;UGKs<-Q z#&zFOz=}VS@#NOZH2z}7RJTuSE@Sp(hAao2z}8ey^eMwJUI5Aq(*dzs?Y_H-l@tfO z4b@@20VM>QW&B+DA}Ys|z?^$A5Wf9q-5G9R;m-#99ZyhG( z8v5@*)QirZNfXL+#jQT59}}}BvyV%Q*5+l;%v6)Uq4eJ%I@{-0Y_kXohl zlYHaa5v=xm(oqoAjsU_Zvc%wBtAiMMb4S|dB|JEK(Nc}}S7h(!Xh-?P6Pe;a*W z{7HAfWu%9PbeOL-i>5sS$8i=`lbDxRS4LJ& zSXIRdPDn)ff+ffzT((tPq*a@rp!bF~ijTx)Jb{C*J)?mD!-RV*p{g=s$uF_4&N^Lh zt$Cq65h&4XnI^i=N2t+N9J8V%p?kNhI^Rt826XuPC8>)TMY3UY+~Ts33#a7BaNsUQ za+AbfoXo>A73v##M8fm>8fd!eRci3%C(_G8;pw{WqRm)5-br^<4|QVSjX%m8AMx`| zk=93M&BKP;35I=h%?^JtiB3X=M2gZ?BMrah7BI_7gQL^)kKm(0Rva80@veIh$*0+b zk$s|zy(Zy<_s!vbM#<`qN#$hOJ&oYNFn-JPUN;gCE(<+j0&gkMH>l)t&sBp}OhC*< zP5+kxb?Wu|tw6)*ESqei?R(6@+q771i8us`VZU%=FaY9DPCBoAcr-X`Lpe%WwwZ`= zRi1a=GRU-Xu!GIlWgYd0Xnl;~wFW(`zlS@DR9sihHs=&|#7?Rk@L$$pK;$yk3whS|u3rcxDP(v1z! zqRJ~L)AghB4iJ8Ou8_%qWRvq{cn{p!PR#%!p9bl*Pu`2`_59R|D~{B+%cSf=Fpv3c zwRcWR)bJs|d7ig(41ammu7-DSStfJ*dAn{3-n|yHxtbIRl+n$N0p6cq!;m|HlwGo{ zPJ9a9WsGm&F9-XgD?KKz{R-F$}0$aKU%wxSg zKIJ%h*8xe}qf0xaoXw4T=(d6fUK6O&K6nsy>iAzYw8LN9J|a$7)Z{8aEaX6ct-L zXR|JQpEzwGkO7brwo_r>`fa5g2^8;+Z3sr>dS_Hu>))VE1isR{e`){&p=ANf7Q$Exkiz!NY5nyTyk0jH>VSM(x;7t#k`pr z&;CDz?rygVQGG;O_dYjaeMHo~L|clTrRH36@AB|PXrA_N;_lst-kJk|wbRzXv#Yh4 z`yZ+RxAXAAyx@>BbFFciz%!$BtOl>V#y=XqbNH&XRUSk?89I->C@(yIZ{kLMVc4nI z!9@_tmEy^HEW|ME@Em7RZYl5L-zu=0vk7?khe_~Re=-AD<_Tv~22%r(JGWJ0zdCz9 zTAlk$YO|)qfyf{)-mL-~w=kP(A=EP07NR(BJP04Nne`Og<_iOxVdh^Sk+9=?B? z1dU!X){Y}nCacbQ*aZB+j<;4Ee*2}CgjtT7EPl1`4J_gKbYoKR-k#)R`zI%j!1lkV z)Qg*W>>pVL*!`c#cnS|vCD3YJh9-v#Q1Mwlr^Sv};G<^yQ~vm)th&s>F|b#pQ@v!b z>lAr(Rbd1VnOi#^bi3Qr=)U9@JfQG?xitHgj4-1Nw<2iYdaf?1B|x7fNW^&;|3{3S ziL(itbM|6mvGOO)w#ZwmB^r91e=5q`_eJd|u8uo-`3I);zG{cfVHeJweC^}F@3Won zCQn>D!gHeEN+quaC;+}+niDk$Cr?m{qd7YqKQ$`|)Fpn%b2B|yelJ4@9=Z<~788T5a5k#BNB7DAb23m{y*iB5kHVt{W5cK776MGtbuUmunH2AKmu zG+nYTYr-9d@n`F}e~U#xat}*pR-g8b5bNYs;&%%KC|;{s$&plY#_sNHqL)JPCXS-_ zn$*ySFP0kl9D{}6fL6hsTjzgpnjbOYVw`Z?6a?O#M!f-a>hcEWbbM5$UtaofGDa;L zt!-h8=bN&_RKmAVbA^~PnYLJoQha3YAY9B;5&1GhZ}?-Bw`|EV-MAlHuPOG_N!2*y zWiEpQK)Rv3a0oZt+ViY;mU%n;zD;Z` zxAQE3IXG`Ll~G`3pp!VG=v*4BeYxc1EUZpP5W zrM2I8Xh}Kf+A*BBnkeF)A+HG?KA%3pNl-ByQAgwX=cxe6Gh(HRb6jYc_hXk1&5UF46{YGMSR1CGiVC%QHyP7qX z^I|~H10-^)_VGk44kyEB)^F!(tC1@3Rzj+9)6RPzZ#I1S^y0tIhLTP)UB_cO*#=7Qv>?e5XkKKy;f++Ol1w@d)48}q)w5A z!P)VHPb%7@ZQ9kzIS|RVBZRGWn^eSm*5|t#>2yW`C!?kw2J30w^zJwk%I-)-o;mhS*{j+TI%1ls7|l zm#ofF{h+Fhr+{ulC6S`cmeiY+rKO9mwGyG(J9X@}Fsj--P<6}H?`d^4QS#Z?YZC&if1!Ssle zB!5rZ@p)(1EtMGQx!r>)rM=AhwBC`yfm!!+4zdCSdCFzCDq@4P$tJ%DR%6 z;n?&dLwekppAqG0ns4wMQc~EcB$+upc{B6mcDaw*8n58CE49w=>qg}`xB^z#T{#3whr!93}9j?sJw%jWBoM-zTMp5OW_4m3SV5Rd<0@xBICU?wDE0OSWIHOtmjczc#TSY(6CIm{09$MnNMu*Nj6K z{%GrD-g%@hI%F$kQu^sQTJWq{8r?s#$LlUYS@8EGOKgOEts)e6{I{yt?4I8d+J zHW`7Yo)OYygw4wd8LtHo%zRj%0Lb$R?(GJ}o8<%`|5zg|FTCd#dXmaizPptn<}=hv z1N?BZkp(3c-Q^?p<*8Y??^i)?Ir?i$rBQ{aB5ZwCQGnK~GaXZ+k}G}(`bz>?*A*BR*Yc)vJYfnS;jBVu<>~|ZE`oT}d zI|%wW;2FEu*=_r|UQik2Bpo|<>;)NG+rTz+V>kd$u7_2XXi@s4t-h@D!TaMl;pMID zHEEh+@nW(VIp?Tp&XNt=tJlw0)33&6d*+iI-d7%rT$kWvTC%Qdf7IRbE(<1rOd%J z4GlXwDs^a(xAm4rk}Qi}YS?vjGlzy78XGjZnBy3{j~{J!NcL!yPNLQRUXCkbI`rF> zU?!7}T(bN}Q{>1K_*Cg~;!=}vc9L}5Qf#K^^44BOB#NB3+`qcXxcgJ-fI+-S#kspi z6*pU`om-~q_wp+Cqfu@BZHZ8lRaItFo;K0>8vFRUK>2U1v*VG%?eQ+&W}HfMtJRKCra;ckz!$+g>vl$*K(=c3)D@gWYf*Qu;Q zNw<%#1s;O_I}`A(WGMRQ@uH0DJ@A_JUx#O(YLXQmpne@N`pso)a!;m=6N@z?cvlKo z5q$g-iFL!X3t3iL2Q?=(g59Lf1_w~!xYU9cn$dLbZHa+=`y{cqyr>Gsk!tz6!-Q;D zQ8IS8hZbr=KYYvo=&P1;quQD`8F+G@7s>a8E7A_`livplt!i~>EOiFilDarN%bz}LL`5Sq-fqF9I zT{eiu{jtvy46(EU$tiaeETCn3LFmY|GzZ5dqWDJIk9%q0HjYK9qRgfvfx%vZV?*_o z(NgE}R^Z>{TY>ULJyK{X&iNAeqcBh3S0V>$!zVAa2u#T)hicWap7WNy8kAFx1;vtr zIQt=mw{JYMS77_tdUD_P_YCDK$j_@@Eoy|5mk*{=FP0mqF^n}y>ty{n1@YOx2LO?D zt^$0Fax_I(AKx}agt*)VYkA)F)%5(~*i)vZeS)MPKAC>eBXFU?`GNW7?fI*vg| z(gyO#*9dg<@=cX^B3IOCV| zD}Q^;{eXIy%m(&Fz>C^&S}FY2pLHNt0fq}nTML^w}PDPko!iuURfp>@J z>0dl&%@|S`!4Mb40c$;QZMQ8u!#t=`*UWlx!U=}gQ`@nI)C{w84_;&#>h;M$FfMc~ zJpT5yyJ(dHT|x}3W0BtrKI?hgXi9RJe0p<=yykhDV&Vf*ye45y%)5T=$)-GFZY>f0PU zD%@P6&nr|!^8(G54|M}x?92E0Ip!@up26J@sv{@j$!5~fv+O=`9a`My#g{{!%0$N- zv~26SPvVL%Q zcSGlAArKHlURE0ZWlB0$s3o#(LEQgQ6fZ7Gss%IL{!fqMBzQQvFBB+NQ!lgU12?s% zen0C-MBc<^R%T%qeko;@W? z)ymi7az_Ht8(N28*@}wHhM6v1I#YVF&mZ_dkVwIq zXw1noMXkQn&zML4mpNJfWVJrxCNF>bH?CXd%<7@9y`STcoCrnc;I#fs4EFaim}m?; zjexoS1zfo^a#MQhB7mtSc3KpEEOI4BNUE!$@3@-n=i)lwOo!9y%0E=x_XD_dKMOT` z%l~(Kb%+yJU7(NcVq&0%t0oos$~{RVQcnKkm3Tk(T{j?d{E#>An0YAS36A-GU+@Jir*BHT03|XC z96F~`CF#R9TcLvXwuH+4sG318x;q?bN`b`P*V@|Boai19Jc9!;&bj9AHeXjd2&kC# zA~EZ2NvBQ$s%EcupP7vCD|WX}sY`50>h9mBruFUUV0t7uqnh7e|3Jk7tQ@JD7rKbW z6?Jkk%-LU4MGkkh$vZt1$gNnl05Ah5I_;zv;XAksiS6A%xcqAALeG;5og_I?6P*{^ z0yQL&mY=1FGBY9By(TTqcXW$75?Tqq7BFLP`9YHoYNX0M>u(2N;LJ>+8IcFyFo!E(k?Sa|im z_-SHQ5K9`~!^WfEU&`Qbw>t}{TO0XAm_(%K#%<3~{CQqNozES;nY7dN1Vh5M4{YAn zTnF`P`Nw5VhOETp4;AppIiXFgjnf~e_CHT9o(C0{e{0_{v2^Ry9nj_7$34!htmw5? zoBkkT;CQo?_5Cb0r(ZXH07@n~qtyq@mEBM!q{{iC`#j?3TXgzDnzo+$vlzXQ2DJ{@ z03#67&dMi73jB@cxTz{RsXV5=l3R1TP3>lxrHB%gYoi1s7Oh?`?#v5>=L8E%^t68J=2weAav8EASh5>espe zz5-oqA82GEp*fpT?(3}Uf%}S`tCD)F8|yW4-`qxRjhiveEvPB(O}E2)N1yPkC}kN| zQJ3=aAynb}w?)e-hR@xUGsy8A*oBA4`d1S5sp;Q(M>BSZN+tQ!zULn5Tl6D2693iPs2Brsi1E>oI)e3CQO-NQlng za%``?sW~ZcW}_x~I1g@W>!;kf!1iqnuqpHN9E<#gWQABVyA?X%y|duMJJJ*AZ2s_E z)%hhQU%~QQ?iw?z58J6CAhnPEZY`z#qK&Lnsrkw!n(5CJ_x6)ZOGguGsr!U_stBJ| zFwimR##h74uBrB-46SNa;=Q&AKc?9 zebNqZJNHR8OaR;n(;+7s$j!qK*A;trOQH}*BP?X{D~3I93Rx;xPi%L33FQG=*34O&UviZi7Mx5=`rr@-xCuPRmP6N26b`q z>N^56_i*y*<#l5(=i3flZj?Pm3$V8Bg=965zuI6s=mHsAtkqJ&eo9V?jExBzfBiA4^svPN06}cw=pwm_zLB3wX?Y1S-e~j@Cp926xNLOlGGx} zx_}`xnTdrVG>j;;|EbpVDq|DkC#T0=UY;*>rWK#=n3NzI9@e^$r+!cf28MNLoQ<_Q z!%WCZp5R-RGTcy%AUN`-o9ObVY#DEgUSuRbf{Ljxutt+SIzNoR)@S0Ku(|{HV(CY0 zC569%cpsHN2rlS`-`#cb!mM`Y85{#`WitlV25aBE;VLiMPv5Q>xlwd?aQqmEV;Z0x zaC0Sca}`GG4Arss{Q_x{%(V^7tRTP`^i{TPVvaHKLz1k`zOJ>N`vhf?UDD$#htXvG z>i6LK4tY%;hj&Q7l+G-?(dv3o8yrHA{<P!rGcQEcC4Xh>i<7Y-~D-q%73bL`j!Yu3bwklM)K^Ifnl2hs2pAriSzh+D#+LI&7-?F;TtJg1muGO_+zJg$7aSGt{SvzS_hnbQSvOTzEl6;Aa@{Yk z$W^n_H%STXb_R2?ctt6Y@2k7V$Qb46g;scmn&>NsA$J1CvI7-*x2l5J;*#WQ#x zxGv zRufU#5R}jsl~ICgX)YeUDt!XEI5_SOa7T)FPwi5o86cdJN6J@@x7gFgCJitHjhE@B ztdw~Vz{?s00c!l-1$ds#miyTbLa7_qknwA0C?Q`Gaima#JnP{>9COfvrGCfdCDgH&3-%5zKD5#*!yzy zR|@f%TWt(4_~sS(S=i)R%y&XW%<{S-g6Q1KNS5i#JUWU(KEhRlpXtu%{41mLIEQ~G zk(39=F>dR)9EO>|N&bI$-Z#wuY|EV(OX>8<6Vuhuap>4`Byfs9VRHy%C5w&z@bj%_G!=NTfhs^cn+NCJ`CBk`*!$**oTJL2bcH?hw+G)@km>K)-1F%D73T%QxA?&u!jA175A+S z#@jC3=s*VItE5e<=ba0F Date: Mon, 3 Sep 2012 18:00:26 +0000 Subject: [PATCH 20/53] Removed deprecated unit tests from testGaussianFactorGraphB and moved SPCG-specific ones to testGraph and testJacobianFactorGraph --- .../linear/tests/testJacobianFactorGraph.cpp | 92 +++ tests/testGaussianFactorGraphB.cpp | 548 +----------------- tests/testGraph.cpp | 43 ++ 3 files changed, 146 insertions(+), 537 deletions(-) create mode 100644 gtsam/linear/tests/testJacobianFactorGraph.cpp diff --git a/gtsam/linear/tests/testJacobianFactorGraph.cpp b/gtsam/linear/tests/testJacobianFactorGraph.cpp new file mode 100644 index 000000000..e756d83e7 --- /dev/null +++ b/gtsam/linear/tests/testJacobianFactorGraph.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 testJacobianFactorGraph.cpp + * @brief Unit tests for JacobianFactorGraph + * @author Yong Dian Jian + **/ + +#include +#include + + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, gradient ) +//{ +// GaussianFactorGraph fg = createGaussianFactorGraph(); +// +// // Construct expected gradient +// VectorValues expected; +// +// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 +// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] +// expected.insert(L(1),Vector_(2, 5.0,-12.5)); +// expected.insert(X(1),Vector_(2, 30.0, 5.0)); +// expected.insert(X(2),Vector_(2,-25.0, 17.5)); +// +// // Check the gradient at delta=0 +// VectorValues zero = createZeroDelta(); +// VectorValues actual = fg.gradient(zero); +// EXPECT(assert_equal(expected,actual)); +// +// // Check it numerically for good measure +// Vector numerical_g = numericalGradient(error,zero,0.001); +// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); +// +// // Check the gradient at the solution (should be zero) +// Ordering ord; +// ord += X(2),L(1),X(1); +// GaussianFactorGraph fg2 = createGaussianFactorGraph(); +// VectorValues solution = fg2.optimize(ord); // destructive +// VectorValues actual2 = fg.gradient(solution); +// EXPECT(assert_equal(zero,actual2)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, transposeMultiplication ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph A = createGaussianFactorGraph(ord); +// Errors e; +// e += Vector_(2, 0.0, 0.0); +// e += Vector_(2,15.0, 0.0); +// e += Vector_(2, 0.0,-5.0); +// e += Vector_(2,-7.5,-5.0); +// +// VectorValues expected = createZeroDelta(ord), actual = A ^ e; +// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); +// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); +// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); +// EXPECT(assert_equal(expected,actual)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, rhs ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); +// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); +// expected.push_back(Vector_(2,-1.0,-1.0)); +// expected.push_back(Vector_(2, 2.0,-1.0)); +// expected.push_back(Vector_(2, 0.0, 1.0)); +// expected.push_back(Vector_(2,-1.0, 1.5)); +// EXPECT(assert_equal(expected,actual)); +//} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 20972add9..c6ec69d4e 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) { } /* ************************************************************************* */ -//TEST( GaussianFactorGraph, error ) { -// Ordering ordering; ordering += X(1),X(2),L(1); -// FactorGraph fg = createGaussianFactorGraph(ordering); -// VectorValues cfg = createZeroDelta(ordering); -// -// // note the error is the same as in testNonlinearFactorGraph as a -// // zero delta config in the linear graph is equivalent to noisy in -// // non-linear, which is really linear under the hood -// double actual = fg.error(cfg); -// DOUBLES_EQUAL( 5.625, actual, 1e-9 ); -//} +TEST( GaussianFactorGraph, error ) { + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + VectorValues cfg = createZeroDelta(ordering); -/* ************************************************************************* */ -/* unit test for find seperator */ -/* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, find_separator ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// set separator = fg.find_separator(X(2)); -// set expected; -// expected.insert(X(1)); -// expected.insert(L(1)); -// -// EXPECT(separator.size()==expected.size()); -// set::iterator it1 = separator.begin(), it2 = expected.begin(); -// for(; it1!=separator.end(); it1++, it2++) -// EXPECT(*it1 == *it2); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(6,2, -// 10., 0., -// 0., 10., -// -10., 0., -// 0.,-10., -// -5., 0., -// 0.,-5. -// ); -// -// Matrix Ax2 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 10., 0., -// 0., 10., -// 0., 0., -// 0., 0. -// ); -// -// // the expected RHS vector -// Vector b(6); -// b(0) = -1; -// b(1) = -1; -// b(2) = 2; -// b(3) = -1; -// b(4) = 0; -// b(5) = 1; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(6)); -// //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} -// -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(4,2, -// // l1 -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(4,2, -// // x1 -// -10., 0., // f2 -// 0.,-10., // f2 -// 0., 0., // f4 -// 0., 0. // f4 -// ); -// -// Matrix Ax2 = Matrix_(4,2, -// // x2 -// 10., 0., -// 0., 10., -// -5., 0., -// 0.,-5. -// ); -// -// // the expected RHS vector -// Vector b(4); -// b(0) = 2; -// b(1) = -1; -// b(2) = -1; -// b(3) = 1.5; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(4)); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} + // note the error is the same as in testNonlinearFactorGraph as a + // zero delta config in the linear graph is equivalent to noisy in + // non-linear, which is really linear under the hood + double actual = fg.error(cfg); + DOUBLES_EQUAL( 5.625, actual, 1e-9 ); +} /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateOne_x1 ) @@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll ) EXPECT(assert_equal(expected,actualQR,tol)); } -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateAll_fast ) -//{ -// // create expected Chordal bayes Net -// Matrix I = eye(2); -// -// Vector d1 = Vector_(2, -0.1,-0.1); -// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1); -// -// double sig1 = 0.149071; -// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); -// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2); -// -// double sig2 = 0.0894427; -// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); -// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3); -// -// // Check one ordering -// GaussianFactorGraph fg1 = createGaussianFactorGraph(); -// Ordering ordering; -// ordering += X(2),L(1),X(1); -// GaussianBayesNet actual = fg1.eliminate(ordering, false); -// EXPECT(assert_equal(expected,actual,tol)); -//} - -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, add_priors ) -//{ -// Ordering ordering; ordering += L(1),X(1),X(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactorGraph actual = fg.add_priors(3, vector(3,2)); -// GaussianFactorGraph expected = createGaussianFactorGraph(ordering); -// Matrix A = eye(2); -// Vector b = zero(2); -// SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma))); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, copying ) { @@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, matrix ) -//{ -// // render with a given ordering -// Ordering ord; -// ord += X(2),L(1),X(1); -// -// // Create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// Matrix A; Vector b; -// boost::tie(A,b) = fg.matrix(); -// -// Matrix A1 = Matrix_(2*4,3*2, -// +0., 0., 0., 0., 10., 0., // unary factor on x1 (prior) -// +0., 0., 0., 0., 0., 10., -// 10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry) -// +0., 10., 0., 0., 0.,-10., -// +0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1) -// +0., 0., 0., 5., 0., -5., -// -5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2) -// +0., -5., 0., 5., 0., 0. -// ); -// Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5); -// -// EQUALITY(A,A1); -// EXPECT(b==b1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, sizeOfA ) -//{ -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// pair mn = fg.sizeOfA(); -// EXPECT(8 == mn.first); -// EXPECT(6 == mn.second); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { @@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) GaussianFactorGraph fg2(CBN); GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); EXPECT(assert_equal(CBN,CBN2)); - - // Base FactorGraph only -// FactorGraph fg3(CBN); -// GaussianBayesNet CBN3 = gtsam::eliminate(fg3,ord); -// EXPECT(assert_equal(CBN,CBN3)); } /* ************************************************************************* */ @@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering) EXPECT(assert_equal(expected,actual)); } -// SL-FIX TEST( GaussianFactorGraph, getOrdering2) -//{ -// Ordering expected; -// expected += L(1),X(1); -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// set interested; interested += L(1),X(1); -// Ordering actual = fg.getOrdering(interested); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize_Cholesky ) { @@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// // create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ord); -// -// // optimize the graph -// VectorValues actual = fg.optimizeMultiFrontals(ord); -// -// // verify -// VectorValues expected = createCorrectDelta(); -// -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { @@ -585,48 +345,6 @@ void print(vector v) { cout << endl; } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup) -//{ -// // create a test graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(X(1)); -// size_t x1_indices[] = { 0, 1, 2 }; -// list x1_expected(x1_indices, x1_indices + 3); -// EXPECT(x1_factors==x1_expected); -// -// // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(X(2)); -// size_t x2_indices[] = { 1, 3 }; -// list x2_expected(x2_indices, x2_indices + 2); -// EXPECT(x2_factors==x2_expected); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, findAndRemoveFactors ) -//{ -// // create the graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // We expect to remove these three factors: 0, 1, 2 -// GaussianFactor::shared_ptr f0 = fg[0]; -// GaussianFactor::shared_ptr f1 = fg[1]; -// GaussianFactor::shared_ptr f2 = fg[2]; -// -// // call the function -// vector factors = fg.findAndRemoveFactors(X(1)); -// -// // Check the factors -// EXPECT(f0==factors[0]); -// EXPECT(f1==factors[1]); -// EXPECT(f2==factors[2]); -// -// // EXPECT if the factors are deleted from the factor graph -// LONGS_EQUAL(1,fg.nrFactors()); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother) { @@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, variables ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Dimensions expected; -// insert(expected)(L(1), 2)(X(1), 2)(X(2), 2); -// Dimensions actual = fg.dimensions(); -// EXPECT(expected==actual); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, keys ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Ordering expected; -// expected += L(1),X(1),X(2); -// EXPECT(assert_equal(expected,fg.keys())); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, involves ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// EXPECT(fg.involves(L(1))); -// EXPECT(fg.involves(X(1))); -// EXPECT(fg.involves(X(2))); -// EXPECT(!fg.involves(X(3))); -//} - /* ************************************************************************* */ double error(const VectorValues& x) { // create an ordering @@ -674,38 +363,6 @@ double error(const VectorValues& x) { return fg.error(x); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, gradient ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // Construct expected gradient -// VectorValues expected; -// -// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 -// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] -// expected.insert(L(1),Vector_(2, 5.0,-12.5)); -// expected.insert(X(1),Vector_(2, 30.0, 5.0)); -// expected.insert(X(2),Vector_(2,-25.0, 17.5)); -// -// // Check the gradient at delta=0 -// VectorValues zero = createZeroDelta(); -// VectorValues actual = fg.gradient(zero); -// EXPECT(assert_equal(expected,actual)); -// -// // Check it numerically for good measure -// Vector numerical_g = numericalGradient(error,zero,0.001); -// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); -// -// // Check the gradient at the solution (should be zero) -// Ordering ord; -// ord += X(2),L(1),X(1); -// GaussianFactorGraph fg2 = createGaussianFactorGraph(); -// VectorValues solution = fg2.optimize(ord); // destructive -// VectorValues actual2 = fg.gradient(solution); -// EXPECT(assert_equal(zero,actual2)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, multiplication ) { @@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph A = createGaussianFactorGraph(ord); -// Errors e; -// e += Vector_(2, 0.0, 0.0); -// e += Vector_(2,15.0, 0.0); -// e += Vector_(2, 0.0,-5.0); -// e += Vector_(2,-7.5,-5.0); -// -// VectorValues expected = createZeroDelta(ord), actual = A ^ e; -// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); -// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); -// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); -// EXPECT(assert_equal(expected,actual)); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, rhs ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); -// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); -// expected.push_back(Vector_(2,-1.0,-1.0)); -// expected.push_back(Vector_(2, 2.0,-1.0)); -// expected.push_back(Vector_(2, 0.0, 1.0)); -// expected.push_back(Vector_(2,-1.0, 1.5)); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 TEST( GaussianFactorGraph, elimination ) @@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_single2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createSingleConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "yk, x"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createSingleConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, constrained_multi1 ) { @@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createMultiConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "zk, xk, y"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createMultiConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ -SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); - -// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// g.add(X(3), I, X(4), I, b, model); -// -// map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[X(1)].compare(X(1))==0); -// EXPECT(tree[X(2)].compare(X(1))==0); -// EXPECT(tree[X(3)].compare(X(1))==0); -// EXPECT(tree[X(4)].compare(X(1))==0); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, split ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// -// PredecessorMap tree; -// tree[X(1)] = X(1); -// tree[X(2)] = X(1); -// tree[X(3)] = X(1); -// tree[X(4)] = X(1); -// -// GaussianFactorGraph Ab1, Ab2; -// g.split(tree, Ab1, Ab2); -// LONGS_EQUAL(3, Ab1.size()); -// LONGS_EQUAL(2, Ab2.size()); -//} +static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); /* ************************************************************************* */ TEST(GaussianFactorGraph, replace) @@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace) GaussianFactorGraph actual; actual.push_back(f1); -// actual.checkGraphConsistency(); actual.push_back(f2); -// actual.checkGraphConsistency(); actual.push_back(f3); -// actual.checkGraphConsistency(); actual.replace(0, f4); -// actual.checkGraphConsistency(); GaussianFactorGraph expected; expected.push_back(f4); -// actual.checkGraphConsistency(); expected.push_back(f2); -// actual.checkGraphConsistency(); expected.push_back(f3); -// actual.checkGraphConsistency(); EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//TEST ( GaussianFactorGraph, combine_matrix ) { -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Dimensions dimensions = fg.dimensions(); -// -// // get two factors from it and insert the factors into a vector -// vector lfg; -// lfg.push_back(fg[4 - 1]); -// lfg.push_back(fg[2 - 1]); -// -// // combine in a factor -// Matrix Ab; SharedDiagonal noise; -// Ordering order; order += X(2), L(1), X(1); -// boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions); -// -// // the expected augmented matrix -// Matrix expAb = Matrix_(4, 7, -// -5., 0., 5., 0., 0., 0.,-1.0, -// +0., -5., 0., 5., 0., 0., 1.5, -// 10., 0., 0., 0.,-10., 0., 2.0, -// +0., 10., 0., 0., 0.,-10.,-1.0); -// -// // expected noise model -// SharedDiagonal expModel = noiseModel::Unit::Create(4); -// -// EXPECT(assert_equal(expAb, Ab)); -// EXPECT(assert_equal(*expModel, *noise)); -//} - -/* ************************************************************************* */ -/* - * x2 x1 x3 b - * 1 1 1 1 1 0 1 - * 1 1 1 -> 1 1 1 - * 1 1 1 1 - */ -// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) { -// typedef GaussianFactorGraph::sharedFactor Factor; -// SharedDiagonal model(Vector_(1, 0.5)); -// GaussianFactorGraph fg; -// Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// fg.push_back(factor1); -// fg.push_back(factor2); -// fg.push_back(factor3); -// -// Ordering frontals; frontals += X(2), X(1); -// GaussianBayesNet bn = fg.eliminateFrontals(frontals); -// -// GaussianBayesNet bn_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.), -// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.), -// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// bn_expected.push_back(conditional1); -// bn_expected.push_back(conditional2); -// EXPECT(assert_equal(bn_expected, bn)); -// -// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); -// GaussianFactorGraph fg_expected; -// fg_expected.push_back(factor_expected); -// EXPECT(assert_equal(fg_expected, fg)); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother2) { diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 93ecd6dbe..71a075ccb 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -105,6 +105,49 @@ TEST( Graph, composePoses ) CHECK(assert_equal(expected, *actual)); } +// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// g.add(X(3), I, X(4), I, b, model); +// +// map tree = g.findMinimumSpanningTree(); +// EXPECT(tree[X(1)].compare(X(1))==0); +// EXPECT(tree[X(2)].compare(X(1))==0); +// EXPECT(tree[X(3)].compare(X(1))==0); +// EXPECT(tree[X(4)].compare(X(1))==0); +//} + +///* ************************************************************************* */ +// SL-FIX TEST( GaussianFactorGraph, split ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// +// PredecessorMap tree; +// tree[X(1)] = X(1); +// tree[X(2)] = X(1); +// tree[X(3)] = X(1); +// tree[X(4)] = X(1); +// +// GaussianFactorGraph Ab1, Ab2; +// g.split(tree, Ab1, Ab2); +// LONGS_EQUAL(3, Ab1.size()); +// LONGS_EQUAL(2, Ab2.size()); +//} + /* ************************************************************************* */ int main() { TestResult tr; From 35d188e36dbe14e3608b3847a7e82ae016959603 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Sep 2012 18:01:24 +0000 Subject: [PATCH 21/53] Moved testIterative.cpp back to GTSAM, made the first test compile/run --- tests/testIterative.cpp | 194 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 194 insertions(+) create mode 100644 tests/testIterative.cpp diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp new file mode 100644 index 000000000..88eb222bd --- /dev/null +++ b/tests/testIterative.cpp @@ -0,0 +1,194 @@ +/* ---------------------------------------------------------------------------- + + * 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 testIterative.cpp + * @brief Unit tests for iterative methods + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +//#include +//#include +#include +//#include + +#include + +using namespace std; +using namespace gtsam; +using namespace example; +using symbol_shorthand::X; // to create pose keys +using symbol_shorthand::L; // to create landmark keys + +static bool verbose = false; + +/* ************************************************************************* */ +TEST( Iterative, steepestDescent ) +{ + // Create factor graph + Ordering ord; + ord += L(1), X(1), X(2); + JacobianFactorGraph fg = createGaussianFactorGraph(ord); + + // eliminate and solve + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + + // Do gradient descent + VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? + ConjugateGradientParameters parameters; +// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; + VectorValues actual = steepestDescent(fg, zero, parameters); + CHECK(assert_equal(expected,actual,1e-2)); +} + +/* ************************************************************************* */ +TEST( Iterative, conjugateGradientDescent ) +{ +// // Expected solution +// Ordering ord; +// ord += L(1), X(1), X(2); +// GaussianFactorGraph fg = createGaussianFactorGraph(); +// VectorValues expected = fg.optimize(ord); // destructive +// +// // create graph and get matrices +// GaussianFactorGraph fg2 = createGaussianFactorGraph(); +// Matrix A; +// Vector b; +// Vector x0 = gtsam::zero(6); +// boost::tie(A, b) = fg2.matrix(ord); +// Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); +// +// // Do conjugate gradient descent, System version +// System Ab(A, b); +// Vector actualX = conjugateGradientDescent(Ab, x0, verbose); +// CHECK(assert_equal(expectedX,actualX,1e-9)); +// +// // Do conjugate gradient descent, Matrix version +// Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose); +// CHECK(assert_equal(expectedX,actualX2,1e-9)); +// +// // Do conjugate gradient descent on factor graph +// VectorValues zero = createZeroDelta(); +// VectorValues actual = conjugateGradientDescent(fg2, zero, verbose); +// CHECK(assert_equal(expected,actual,1e-2)); +// +// // Test method +// VectorValues actual2 = fg2.conjugateGradientDescent(zero, verbose); +// CHECK(assert_equal(expected,actual2,1e-2)); +} + +/* ************************************************************************* */ +/*TEST( Iterative, conjugateGradientDescent_hard_constraint ) +{ + typedef Pose2Values::Key Key; + + Pose2Values config; + config.insert(1, Pose2(0.,0.,0.)); + config.insert(2, Pose2(1.5,0.,0.)); + + Pose2Graph graph; + Matrix cov = eye(3); + graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); + graph.addHardConstraint(1, config[1]); + + VectorValues zeros; + zeros.insert(X(1),zero(3)); + zeros.insert(X(2),zero(3)); + + GaussianFactorGraph fg = graph.linearize(config); + VectorValues actual = conjugateGradientDescent(fg, zeros, true, 1e-3, 1e-5, 10); + + VectorValues expected; + expected.insert(X(1), zero(3)); + expected.insert(X(2), Vector_(-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); +}*/ + +/* ************************************************************************* */ +TEST( Iterative, conjugateGradientDescent_soft_constraint ) +{ +// Pose2Values config; +// config.insert(1, Pose2(0.,0.,0.)); +// config.insert(2, Pose2(1.5,0.,0.)); +// +// Pose2Graph graph; +// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); +// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); +// +// VectorValues zeros; +// zeros.insert(X(1),zero(3)); +// zeros.insert(X(2),zero(3)); +// +// boost::shared_ptr fg = graph.linearize(config); +// VectorValues actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100); +// +// VectorValues expected; +// expected.insert(X(1), zero(3)); +// expected.insert(X(2), Vector_(3,-0.5,0.,0.)); +// CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( Iterative, subgraphPCG ) +{ +// typedef Pose2Values::Key Key; +// +// Pose2Values theta_bar; +// theta_bar.insert(1, Pose2(0.,0.,0.)); +// theta_bar.insert(2, Pose2(1.5,0.,0.)); +// +// Pose2Graph graph; +// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); +// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); +// +// // generate spanning tree and create ordering +// PredecessorMap tree = graph.findMinimumSpanningTree(); +// list keys = predecessorMap2Keys(tree); +// list symbols; +// symbols.resize(keys.size()); +// std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); +// Ordering ordering(symbols); +// +// Key root = keys.back(); +// Pose2Graph T, C; +// graph.split(tree, T, C); +// +// // build the subgraph PCG system +// boost::shared_ptr Ab1_ = T.linearize(theta_bar); +// SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar); +// SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar); +// SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering); +// SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); +// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); +// +// VectorValues zeros = VectorValues::zero(*xbar); +// +// // Solve the subgraph PCG +// VectorValues ybar = conjugateGradients (system, zeros, verbose, 1e-5, 1e-5, 100); +// VectorValues actual = system.x(ybar); +// +// VectorValues expected; +// expected.insert(X(1), zero(3)); +// expected.insert(X(2), Vector_(3, -0.5, 0., 0.)); +// CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From edef9851dcdb3cc1879aae7d941ff9797ca82d9a Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 3 Sep 2012 18:17:16 +0000 Subject: [PATCH 22/53] Fixed case issue for PlanarSLAMExample_graph --- matlab/gtsam_examples/gtsamExamples.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/gtsamExamples.m b/matlab/gtsam_examples/gtsamExamples.m index 39ba79203..094329329 100644 --- a/matlab/gtsam_examples/gtsamExamples.m +++ b/matlab/gtsam_examples/gtsamExamples.m @@ -146,7 +146,7 @@ PlanarSLAMExample_sampling function PlanarSLAMGraph_Callback(hObject, eventdata, handles) axes(handles.axes3); echo on -PlanarSLAMExample_Graph +PlanarSLAMExample_graph echo off % --- Executes on button press in SFM. From 3d5220140278d42e156da62a34f3e0e33bf8d120 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 3 Sep 2012 18:22:13 +0000 Subject: [PATCH 23/53] Adjusted spacing for the buttons in example gui --- matlab/gtsam_examples/gtsamExamples.fig | Bin 10933 -> 10949 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/matlab/gtsam_examples/gtsamExamples.fig b/matlab/gtsam_examples/gtsamExamples.fig index ca48437dae84bc997488acd393909553f61e90f9..70e22957a18c26de22db0bae2900c464c5902124 100644 GIT binary patch delta 10299 zcmYj#byU;=(>0BfDy5{fh;(;@pnx=pbayxGPe4JsMQ~y14r!zoq`Ny7Sh^NiYJr9C zdA|31&-vz$Idjg;e|PSjd#6FKS1(b9NKl-gUz}IqH9s#Of07n+k~tp0&4KYhfvKua z`k$)k4F-lk_{b3IMk7LA#JcD)weUycZ$>?N4Q6-pGNTaz1YS@RBPwhuVVwJkUClBW z>b@oB@fa5SO3L?SrgZNYpH(Eut4yXh1o=;*Ut9H`)2Xh~aeZK4tXb|P%~_M8XF%N~ zZQjMFJjqS(eJ%*FT{OSJWwFs+WG?koO7(Rr*KgON$XiaEU_U?;Yu$ikb*WsAglHXb zav2g6b^ax;7HKfEWS7`r)^PZ4Ql{jskSq0T9lYrOHT7C&$aPAeJURctN@@qw(`Yt4 z7$sHH`3P}HCB&CPIM`@LLFf`ZBeZKbB|!cK|9a($H=Tfk#JdI7tZ1Qx6w0Rt63vwX zL0AoXw8G9Z3se+`Ju$8ux=pVho0&WhSyqa^FeV7s$bzQrQgvW4oZ90xlCNVy=_g8f zr-}TZi%GWc>?4y-g?E~#EybSDI$G^1&$+?%*<$^r-pvF_Rp%T9hPgC({BY5b|9Mdt z9a!!3+-4r|QBG|)BuW2BjMnRqM~oj#nH5hynu5M0-+rIsNqbP*H%kK*Ubv}2wQflZ zA9Zg(?L$*RKJ-;}x8kqMV=FAYUz{H6oIu-vcF#0otW^7Uff$M6DE5UikBcTFja? zG@{>Bk@2;ZI89Ogl~=Ete!Xh?ac?V-ZpH{VknMbWev`%zV%kTH71i zOy_jQL>@eltBUOb^5U)ohc*RDh~;EfAEW(OP75GES!D(zC=@T_4+sO**O(m|Tm6|m z1F)0HmW;0z4R^8XFSQDqr)^pe{-owaPXxlZ3O|ECwJ{^xJ{em&mY~7pRgRz?xluTp zP?o2$dXN$~d<>L9vqZ1QdMC^*opbeb9SDWY&QZM+FCbR(P}qDnA_yZE*rHk%wxC-& z6fR9UwT#PQDT)o`igZ`XUzDy3UF(*i1ae#8%dMO37vY1h8&Xpk9Sp{&>l~YFZc_E$ z#t8<@-Ie_zslg(H4l=BRO&^}s_)2YhI)*YRhH`d@ud6HmMGz9nyVP4g31;t?9PuaE zEafdQb9fJRvZLKKy31umv$fn1d!7J$$bndc-Q(%j%+I48EjAI*OvD_}SXc#}Qt z#$dwVhijZ|6;r(Vh`fW3>x9gwNNbKG*EsXeb)_fU=LrK4LIUP%S=x=PL(#Vk<@0;= zRb=?AmZn4*A}YKONK5_)UC3E3I)C%@YzY1GRyrN=>Z$vWfZekA=LtGI8S(<>&QBVP z^k=uiSLe!OYY%aKSuHJ;PO**v-MRE81Q)DK@+PmP`G=FSq~i}hBpAw^0aE#dyDibA zF7p93N{-j91)Co&a#uy%f8&-q>o%b@H{QH#(+q}D`-hHZLJTSwuawbzH7LnNvS&g) zpp0QKZQXpRT34aHI->g!sS>5PUgx7p zv_aF#66RuuoHWMfKt%je_vbM8DYM&b?|(lOAdD@*AG$}b%;kLjK;Xbn3 zIC!Cx@CY1W!Tg&39tfEi^}rt^eqyo}2|o`9(O-S}{+)y8-Mg)>>#_|%4|Vu`7O94e znI51jrbsUxKbBM(y^(Bc0q;IyVPHHy9vO~mlF5++Tnu5k+B`UMZ0&DYUl0WHolSfX z)4~ol&1xWZ3i?3<^aVFeg)xmFN z-`i$+TOnLx(E7?}7rplTdadoc(FJ3|V*H87zJ0j<4~cr~aiXbu+94kCY74Awa^e-f zaDvW+ByCk4ZYb|ZtR|U*Ib1e?!|rMB2bQ67;RMny?e?(`0&gGm`tNs}|% z(+b_)_S_%dE&f#XhzxdT7HWR}UhK;Lr>5u4i^-iM|B2ibunES1r>ci@q2t&zo%6d) z*3{}Hr&$#!*qlf0HwQB2HRR_76!C9F8WQJcLfhsjZIigT^hr}y2ncfH%Rxt zjvsM?85u*`w@+qz;b12RDg;&}71@P- zrie0F3forAgx#M9>JR+3$R#`i@6gGe%JaFK z0hd}&4(X;Y+X>a7YP$%NsQ}%Wp4pcMRFGji0kj(H;*5HSJ%`v`=SP`Rc0Or;TIKo! zj3pkLwC*w~iJbR6@tl0GC=S(%wCNmCb&eU|#Xgp>4Q>>^`cgJEy`Wo;DiN>k;qLje zpCLpM9!Y+fXjn{rhCLkV-9z`eUwM)wy_Ts}RPG=|)kIsc?m;>TRfFq@evhaNK7r44 z1I54h<5Ifmc;a1(czl#JR}UB88=NSlTwW;~y7?8XU7cvPZzHqPLG!plS}k3H60Gn% z`Pl8E4=#pw?T7Emvo`;>orqQQ#6WTaK-=tes-@5WN&+(hFA#-PW}Gua>I=x}WyJT^ zYWm{pj(sBmNQkQ6<1a~R`k-|B)=L*D;A-8bB`J7tnfYYm9dDCW|J}q2`r?qRF)04{ zTtI5!Eo%MP*9`Mh)s3Ij5$BbS*>h0;BMf%-{ts0l%@HSCiPEg+_8JZZpv8L`FZ%IFbK7pCF5XXxf%V z4X!L7sfh6qvekUh%^jl65mh>Ey;0cp*qU(0AwrS*-&}5 z*{);Ud))@CS;L=__&a|YeAq7JlF5Rg&)@K>6Fs=tvtnBZhch_U8JOl^P zZbbgU`GLXVYwdHv%s!pL@XMp6CNoY*d7lX0L82Q#q#NaSDtd4D)YS(S0fW_!+2lNKmLJteoZzPW-R+6cCQUi_)$01a z60r3GrR-V%F>m9H$SzyX%GfE}1NAkauX`VX{5`%Vc23mf*E=VM`9bv6<1jFs({{br z$600phy`AJ5mz>Gzk-N>FCL09B7ckA|1WKS&?=EvWuR4S7vne|o-zK*V_EDSU?X75 zvk2sG9jrQO@vsv>@zu_G&yVQ>+S9Z9RT{qXBx|wweAyS-G5F6FKV6V3GXAiX%!qmU ztZOSr>jxEXgCoJlJ6d}Ty=?X`K#sc z%KNzBL~6?8jD_)K4>Jb>ok!ZeyqTdo72AK#(g$UV8!wJUV7F(hcRAey4zkMTvqh<> zM~Q(jVe1(@MmK(ErWb$A2ZekhO8**07Re=wJjzXELq#keo;y5jd}Z*X3j%+lOFmoN z{P)oVc5_cA;ddMsW3xjYtYP727rpwW$CV@&|7=fRlkh@#w>1VX+nJ!|f%#R4i@b** zWqtmYBgFe%$=z8?;wCH)c)`;p@xj>7G5_-K!P9~|x;;VCB+OT|<$U-ZaRaeK?&cu&t9bVNWdXm0?-b zD7lU=@T=zM?&li)xib_7Jn1=iuc6y$Z08tUOWag})@Fynjvnu&0UmS{pMN=Q7V?Z? z5ylIATgxVwT1+4g4MxDilp8d5(k_u*JU(V@`ysTV`dB6e-rGu#)1oFF_ZFyFVP(US@o_ z7qhmcoO>d7_t&r1nW0)&=yywzR^B&1$e+OP@DF5drX066!NMyi=6DhnYUxxZ+erM< zaCG32PR8OwMZ+bj*uYP9O0F1lIsyt!+5(lB+Nx^RV?0fuscyi2c`?PZ*sO-xpOA-n zv*+;hl=#)xJ@M!k+m&7#zq3AqhxUq|52tjSar_Fxd1-&SCzY>Su7G};aC4W_zDA)w z8mcDH#b^tU%-A=Pj9}Y0J`?r(ujgLSMbSu5Q_;o5=n`RV@)jx?okf}M9vkcqZqy%FP(rLru_JM$ zK23Ezs7;vRCO|KfuMT;@BdT1SCujSg8G~UjVd!IE{hql-d|qp80)+=P21!n*3bgY4MC-?N(fa-+F6I zcfTNy%T-$m9e}Je&JVX4rSUtFRY89=;gNI7O>acp%6h||n$B#pcP(9%3Ig0Rt+g^6 zpND{lZ`XLOp|Or~U+-~^gc}k^c|H9%{`LGxDp`;J?oFg>c`vazPsCL>dm&J1*E3v) z{M{1sh{gok*lQrHLiGz`MxW&Cu*eo_o;Y`O8XJEQc9=;N_nnF$IscqpV4R`P(!rxt zr<}+X^AY2PaUQ!wtb66!NtAi?(#^Yc1#K3f*;e?*C={}xu4_9wKr8S2lBR-t&^Fhk z+fx%iDvN$Z2Tv~}ko2|v(iVEW_Ib+ZAf2`N_(y{^jO&Au`iN#;Al)lRY3owL&56ucq*~7W?8|NUKVn=B9`|=7!L#G<&dw0M z{Mj;Cw2eyv4dmzoFIE_&IH`n)v!^lzK%Qd^#Mjw8*6ke*Tnv5r8C2d+c7IZ&NkE&) zY8sG;UK0Q})m)ZN5+|&^BVs6V()0>$%?yf{Xn+PX1xjJ1Tbu5_-EJ>S%7&Cd&)fTf z%5+?}Y|o^a7dyu+9o`Q6@l|%xdFl9hG~WXs|MTAZk7F~?berZS_iJ_ilvorBB(9Gz zJY`ILhW`Lws#> zS3>=F&%v9~7N3-JmiYkamK{`jc^sks`kTh^PdS%Y*O#fz4p0MrR-v(HnW^JVHoryL z+TwBA8u!|wq}#lI_{mV5PEqqv0FEw_;oP?^2b|_)9|y&M;r2CmhPmsv&(r%+5euIL zm>HlYXF@Y<^x?kKG=kQMov-k0q|a~zMM+HOBEI6Zz>e4w4iV)dEB zVVjV&c=q&#HhH7`$H^xagup@-Sp?I0sssypqomOV-0Efdliz;=Q;lu0oQLsH%e`gh zgqGhbQ#$;B2Ch~|4Wpg+)FbTwkkHss)o;MVI%TUc#qpekb^PGzm+mXv7 z=4C?eEIR%NtGoQz;evDP&(}^SfZkTy8aeB})6$jSZbxUAo&8fMl0=~P=&MvXE{PTH zaNZQYW6ufa_-E=>n(-agx)Gv-h^rAK{emND#R4|xMQ3n`mZ`S+-JxzlPi_4y3s1~~ z{nt`Nq^L(%D)2dhu-%{Rxv6oJsKvp0Uq-qI9{QY6N~1{1#UC~D+PCkFR3`v0PK3u7 zm!Yo>C4FDfXV4-EaRX$PB0}!x5Nq#;iv^?+IamD~G@Wg{*D@eYr5g_K^2pWjm6U^r zczAWaj2pNu_safoCz3(p?`j?N#QFZdo^Ff=uK_!^Tgoi3g9VvvD2Z*1U#1zKIq$}h zkY6H`_OOIY6*waCo<7j<&|mu;SwOFGu<^~$yUk**`>|y{6^L!5?>Lz4{;etnp6Py5 z8kxp&>8?%6{@;|)LjNFsX>+&?8DC~_1K*+_?lewbU!K0DR+pl+%bIU)E6>a?bxUcv zvEJTSHIZyGK8ih4?&|BRui@T&`s&ayU0anuuW z;rq`Q@LO!9YQCJsXTZdiLj_EzFl-s{Vev@YV-tbNE`=GiY0Y^jZ>j*@x^hJhUIX z%|(pDg$3lJ*OC+Jo59|Q##_)kA#dZKJla-=WIvOp;0+PBhVnmAX`ywyrh3b(T*dEz zDE5*Szogvtrgxo&%0v1CE{p0N9s# z7Cz%SSE_GS#8Jr>RIVS_Fxdy&>hfM_*6N!j&Ke4|@ToPd z|JSeJkB*~exDZea_U?8xXt0NuaV+~kgX>4`C8S^Ej$ZF76QnV|cC~=TSw1+0<@-S& zBKOl(ZzmeT{9w(MnBZHbAptSqo_kB8^i2qhT3d9MM?RQlXzVE9t~~T4#e`VI5dHyr zGe-RVHh1f}h+-<56_S4Qm&G%?zRsU^$a_K31~^+e=e@|=LPn6QPtw9EJ?YWab6yqT z>HNKIn)I+aXvrU>*Y|0>_+47+>Pes7mHG3{l2z$K-c}v3@TrYpR{}tL8OS+fG9D@q zz9y`{`-AA!ItMDY;D%E{iPxLJT?b~^NpEe4q?OH;O{uo-7j5cGaH?gah5ey!o7O4} zeY=0~<}!E1VO*~kA$h*a1_{te+!5=N7`GEZuH=#L?7}r@nv2+uDNP49f#1!I(U2mWU+!8(s;qN;_03AT(roI~R_^adryrw|AIaz*$)17| zcIsp$CXsg80}zxz{pFYPa!Z*rehGm81TPl=o)R00?d-f+x$ejY3UOuSyaW}Be!bRw9*NV@Yo z((s~Y?3czTN@*4N+A)5>j+CH~?d~DpW2T@g@;YD?!08y=gj~{Y3#Qum)phcU1>(48 z(Te*e>d2_NG>H-8-G*M-5`r0*sYAz-9CnL6515vJhmN%(!jIPP&FnF$nZ}=9q_n$X zgs7C^aFfLX!nyv2nQ0^q7pmNbHg8Fa| zNF5+tEMR@;Bx^Sqq%Lc+wT{`@(AKXKf0j7^97zCA@QfAc=ojvQO<~OJ*YhOrV@Xv( zjGHjT>+XB*k9JeFE5uKU7KY6m#+J^&x=)i_Vpe&CI>~fj2i)oMM;BSzNM%$3u~@MhbUfC&@(yBhq=VqE6QHd8tlPBRKqi`?6m$d0yr{ z{p&`O<_FJvw8zlipvTYMsDK27-BU10MHA{AI}74@K$!BLRtOM@8mY;)$Wo9YVZb7? zzU*%<^mEw}{MaFi_j2&R5H(?n4B&-2ZG{WT!dj4NsACxn1tVX|3am}o;|VjkDy5(k z1BDjpG+)25Cxu=k5I7f!YN4jJK~y>OAq>{|hY00RM$bb7S(%Jwk;rMiT7Y3an#RD0 z0p4PT`$+^Seb&4l54Gc(*q-ob>PI}O|10K-6?X3{o5Z&UTMY zMH>J9BS|~%5fd&^77J7=;XHX?g-`+NP@K3CeT=Tj|4Q$*h9Jdc-tz9}7-Fog2kf41 z>rLG*at~Q}1(SCF+GhYI_&&VBF>UJI81P-^DFPj^m`_lu)W?XQFwC1Z(TOUX+BPel zzglwfy}9&9f82E#aymKmcD*4y zq*eRVA~iT@n|8b8P<*##OgldD4%Nx{LTo~Q(e3ILkD2Amn%xja_f)VH_DeRWmsLqo zSUFzkJgKAVRzHjo+m%TMTEE)xyZ#oCI|Hwxz5r2jO1j1;l3yO%yH;Pii9)tqxp`|q zufn}nm(Kl4(mcO`B{6}QuZIKEwXZ~%x_sI~hTjAYp8e;56kHISLs3CsS2_#H8%awO zi~BRFzE}NP{T>&HHOcj_8SEES{T|Rg{5h(nE=eSUPct~g@;|flP~tW@dM>Or7Jv(H zjmd8Z>(1FHGGG_pIp3-SY;=6;N6Xxjxp&{M!iITWmt`P!K=I|7DZEj7?l}Xn0+b!* z)6HZZj5we}RIatBU}z{N+?t`=rT_9nBgUQc|HbU)-k^<;mR%;8^!GuF5yw{4}npI$v{LO|hFmT;_NU z>1hG2oe3!Db|gDQ^^a8LOG(NVD(kXG#sVa1Gi}JW(}eZ zD&A{7DkxGEDhFGj)UW?hGMw4+mOkcNJ1Ow};(lDnJ5ev1c7X$T(!@Xvc_t4_g#APsPG#0y-_dd7bHunq51JW#HFl4 z!7QvEOLiYfce(V>5c@-iipt)2OyAeI@CL9KQiDk79ek%%W&TWzD`qM^*UVlO@w1UfHM^fs3E=Kdmj5m)tv zz}2rfVrCyc*k(gPscLh`LKry^YZ>%#R9Uc3iSTQedn6; zT@F>>V|%8h%!L!_QLhViSf!oHdx@b>bdDjO`D7;P5Skb}4W%_{H@x$BXz@ z_c+I_^!6G7)6uVK;R1IQe}w6L+YeG(IRSRFUu_@Y{h48rWboMB~S9!RlCCJ+YUx- zixyWEkrwsTzUIF|Y_CInchJ8i7yYNG1`i(Ay|ufB&RBX_-*faNnUh#VU+lmCqd0sZ zxIm2Yq(-{u_Rq_JWwt*m=3;@p1IKe*zH4m0^f9Z#yib5&ETq9ieReyKWUB|V|N!dsFdwRuOUS4L7i}igzMYx}OebMs7(a9>I%;q?CdNf8iD~uqo z2FRj4?j|?JHhJHkewa)DBTxLv&2HhK!b^bHud*bCV2u5Tns?&YvP>JVb5zf}f(Z+V zzJ5{&Z?oJ>DMVl^c zOZnQt`b&(=uzflv*@EYq&v!pyuWn;ro4wMeu=N4t?Tlp>@V<$$%6DfWCrs5| zEC=4Guul;01{>1M*Qa~K%}d^G1#6_@*76$*2WTf6?i z@pEDaQ;%kR?=x$_nudybBVuFi*ITgf3 zQKf^GbQEQ(WLFIY9Jk)}NEWND&j;}19-VEAS`H=r_{7}ceTfXFs8uSe)cJFN9axyl zdw6o4V(SbszYvO4eZxL-z>8+9xGL*7Cb;ge^+%e#9^Mm)9>5PFLt>5%8Bfq?dH|R-B8GckxA1O zXAR{YJ?2%2*U_7i?DGMihSm<5N=>$E@{!<>_SISTDrt{P)wm|$LUnF~Cw_T`-NCnr zxfYxm8CXN;vw4sL*-w!&ce>CRoaCBgcgkoWgOFB>%(C-Mpb$=6a_C_WXhz;++Q}*A z9Vqzs-E-V|D|(CvzEzLbbr2P1)k)%^pN|T#9}h!u*P;ThD@Yo&?}p&`DRb=QKu>~S zh+6fc=p@Jw3FCFx9lpAU1>2YeXfCvpgW9^5|BlgHyW(L#cv}vArG~!lP*+xUjn7i| zbTn0UOfr$xur#$ylK+#C&r_|gofOuQ6{KY;7ZE3|yQa)g0siL>`C5?hG)8*M-3s^i z8`Tj0q*zoht3{Mp1_xu*J};7%{>ja6{889zkeq{uvbKY&DkwXNqhYj585<+wqb7SD JJx4m`{{gkx0DAxc delta 10287 zcmV+~DA3o%Rkc-+KMXTEH846eFd#B8F*1=+BavVRe;z3S000000003=O%DJ70G22K z0C=42Sif%@Hx#BN*>%#`unQDWGifPe1BNAcNtVLrIBD!6j*ZyPRw&eol33p_9C=RE zzaV2q{|%iw6&*Wf?9d@YQv^j}6hRSW>{z}dsk@_P>m)mN8~6d#yT{`rA3suj-*Flt z^rAS1eZqSH)}$4yAt|Xy}v06?*>G5(ZOERkwy^u0SP%J z4A~v(G(+NvS{4wSVkd4fEy^uXbR4BVaZD%;GNbGkZAT$F>OXP9JV36fdXP_F6AS$R znMJv9(DR;E_Kf)1^(bd3C9A&Uhes-09P}!_f1(a~fL+q7Sob(-lW?tv+q$?8kDrY-;GQHgSA^R-gWQjsExY!?M&rEB!+0UzGk;>6c3Xru1v2|4{l*r4N<< zOX)XC|E=^}rQiL>^jjm&H^w{;jd|*MpH?pM&&D+@{u&33i^fUgrg7A`YMeFhrf|YQ1BWna z-c{D3;5PBNwE^Q6GvgMK-)iOZD`fl%2sl9=zf#7pggoCPdGh_Vfe+aCcD7eGDtgNK z#y@VpTj*q-kd7PjdIf3G`?h3BT~WGhjoR-n$oOOH_g5fO*7qP^+txFX$u;XVfAbu~ zlP6IYXRvMTI1Oy?ndA1G4@zmxgTe1$VmN+0rR?jX+}mBhb9XZU#>kL^8GnQw+DvF; z%)iHn>tFAuA4L(Ik?xavN518_O}&<{+WLY;hqS3EZb%%0ndZA;hWujNCagz$-4=Bc zUaS$6#FSC5?GUbLH0v@O=so;ne-UapzP%^jOFx+GHr^*q;QO%!51u0KgVsY!zx8-I zHrjeTK*@KLoc2?ae7gh@>og)Rrxy4@6u9E61KvcG`@X;+)J8joz;|-Y!;GgOo(d!S zl}7Zte$H{8w;vkOZ>o>qFz5dJK99_JbuRf({SeB>H@-A*1^YC7!_7JFfBWJ&gZ9x! z#<*b1b8P8M{C|3P{CTSHdeZ0VMc#WwRG!MGy7xk@n1_^y%)2wnwnSM~|6}zU-Ny~$ zphV1zA@k-4#(!Q+#-AB5KD2*a96esSA4A{g@;xH~4l(at)iL66>LpWXMV<}8*43(- ze6ZhoRZw=lz^+CttO{O$f1T>a%Fg|j+tr6()RvZTeVa#3F=n3gcP!KI7^7HeRyNxh|fYhqM&te^YbX*{p1xRF0ipkdR^C#e>7hjguj+_Kicmg zR6d{= z68xW`;r6BGwV=&Ui?fKQ_Q~<_=x?+SDlb%IUbuC^dEsElywJJK3ulrSD(93J(&t~l z_^^Q&*g5mUW0Et%!e-ICzxav;3<4&h5+b$m( z+(|aGZ+G7By?O7=dv7-a0A1!4;4V5M5Qex#r_C#a6s3qti`HwHL`6vfdrYGW@MEl2 zQ{)mNGFERQg5Z?-Y&`@JwdBQr%W;-Z*DrAEgD&eueXEElsHkB~8t-+fhBQR528>pT ztlC?cH`EeUf3~oI)UqPg{Dr+jIPuP@k{9Zy>q4l9IO~Jof+i7KtmqYFYTgVx1b-Tk&4yjDmHYmarW{98$1r<Vvd?a?3GGS+fj$5X7{IhZM zm{v4wsO1b+%6&B_&DeI(&m!@&8K~G^_ZseoJ$P=>=bERiSmoy$y ztV3d*YR3;OAJP%GXccQ!Gh9}rvW?@V-q++MNzq%+vy&e~(CkNgp39?Ob1z=)q_(e& ze{{XMu#>vi6TJ59+;*yd9y+YIbAG)&kb2vDMD>=>cTW1v3ihIOoAowV##M=E%N>_m zzs@?>Syns6eL0LF-ODmri_PXqMYY?eyW>+l}3qsn=J# zHlAGio;tmoy*Ls3mqS= z@D97L09dU1_p{$u9Lv7K|BnB^zvGc$FG{!BS4>ii$}9DAF3p+NBuvd-TfRCjySvF9(v!=^P}$fe@}kj zaV+}||9{5O^&P(e00960?3m9_6G0Tm2Z{w0bWM!Gs7WtM8%fHK3yFuelpjWEAQlKG z>$F|U!ghDFJ5}K1kAoye!%6W^An|5QG+w+IJ$mqF3d2^I@8@@hhgKkwUp58 zB%9f{-S?e0Z{Ey)QUNr8C;;Fjf4({z0r2C#1z-5K*p69t19O?ro4|3)4&rO3pk$Dw zDgdBGO*G$`k!0)cuBy))8qA2raaMpmPLsnpx0rDR%Qj{%bw`7Hubqp- z&#fo1JIvc}jm&S&R?pAN?~orhURpoIuiwRwjic3iKKF;TV6>ERS^q}sj`)qc_%(6k z21uShZoYo8^EC4Hhi~KYf3wZVshz~dq4rM^-^iP_2P|I}C_YMX~Z3akf z()cRPC%b=2*{AK6O?nv`XbQ?wb^_+8p4)t!>V9{lF>zH(hwm%VEl3$vLAok4I+fI6 zvH3x<4nH@Smv_Ok-{Wgikq|v2NE^n{vCjtZQI*rxf|8A^vZ@1me|*vRdqA3C z*@~*?ak(vtSzD*yMaR$S5L=TLMA_JXt~srv5A0n>a;{~su8%!=z8%@R+|%;%!gl0( zTjP~SXSa&`dBtbze+dqH?7EWoZ~H2%1CqZyL^4e4s*23t4N-$SM0mxitsqGeY1#7U z{JwL|DSrVjf5Z+K3_UEX87PMnv5C>xK=@{FcTW#YB~c-Z$8`H{@$(Y!T^P83@0|Ri zpsskM&*P2i@dllHt$8DL$h`6H;qR&Q@$XgRjlMeMjlL@Je+I9^_m${Ymp58G-T)qN z(D*&xs4w0~arq*4sk|XW1?m+5hsPVvyg}z&Yu=bVWZw97|L4#RdA%auI05#qFR{(S z+{KL}#T!oF5qqQAaaBu)@7;6{oHu+zu?|-~-l$97ST2({raj)M3U3HC;*DjO_@=ph z5xYd*NDh|?e;OWVICBPF+gfwR$RTsatG64Wxes3)Im2yVvFDtCEnJNMmA`wSy#46L z71fBp96vfP?V~O_2hR;>y#Fb{(dLG~00030|Lj;zj1)x_u9?|gU|DdDKUoq?Ruf@~ zm<2tsMoITDEbQv8i#yBkBWA7XuAM2V>1w;HVTX7yf0`f{m799@B8gWM!-;s2L`~o% z7bD()gAxb+C zEco9=e>QX=qYj98i(w0qkOfPgWiU*w#X7W1g8capf%g1;3GS$&wV@Hj>wJoTi%Pr%?=C&POCPJxe^uW?2HS~F@erAi!5uZ`!p7t>Y}f?3 zW7{j(^&pup7iSkskdP`gPHx}Xp|!K-VeyXa?bzY_rKk9{LGerQ(4tneK|ME=r^P=a z*-oVQsigO?GA_q0&6eiMg5Lr<@^G;o;kLag$=)W#j{#*p1wRt}{3$Q}w=sOH-!+gB ze;?W3D1U_BM2ep5$A(b8_isUCdN=dR`uXsex-XOT`jmM`MV!99@zrN{Khl0b^^3EUB<3jQnc4C$Ydq!s)6G8J6!PLtpvx+d zU~>W1B714yGYI0}GuZl`>F9IrsU7v5e`YsnUwYV}7IP?(>}1J9p9&*>9$s3Ptm7-t9XQJ&6X0Xeuba{c=0e^_P zIH<<~m6?OBT3i3HiyYZx(Y6UTpiuF}VS2PE>-t*MY4CJAgUq&_i4;2niXFk*e|Q-Y zx3jJ#tz!Yi*HP3p^8wy?fHzj~?+qnhg6|Se3GRO92WFg zN&n${L4RsKf9G}mw_iC~lX_O%6VP=ndfyt~b9HF`sg63=Ro;7yc2h@We?Rh^%jI%@ zT(13#-ns<;zW^)IZ7t5<(Bkha%Kz6_7_ah?o@#%kc_O-^WvvH)NPQQR$MlLU0n3xU{72LIvfgwqZe>eM@p=Gw3e)eu|)X%eX zn|q7@qalBykl4?X8d}FWSx`c=0{gLR6WDx}x3-pS(ySqM?4$!-`_J3W}E45~8q9zf8v#rvP_ASSi;ibyj<_KwHiV~qD*dfGZ*L`F*vf(*5JLHiLw zs%)R>rXMo zV@-m1myv_T{R3TRofLcTeX&jzhD= zD-W{Ydq(qaISWV5K6zUkK3LaZ;P`em_Z;_Ye-&bS1%XR4uXC01Ue!p?Pt{-f-s8X4 z%YrxG_K=4X=ItiFbZP0nu4g``mj|L7MSA}L00960>{mfg6G0T7mO{h>L8B3m8m=bZ z)Kgmu*eVc1f1xOvm|^#^oj9FYcW2dd;zT%j;umP*#gjMVjlbZX7&z*UpWw`P+ufZM zYnMVH*_TXb-?#hCyuO)v^WGRj=!$GP)RNIa&&ZYydL@%m;|!q*`Fvltu?xa$OUP@9 z&Kv8&%SLz)Ga)al@iLLt!gJ_WpsT(L4#AXcfmvjXe+w)KXSD~dagR`c<6aMR)h~br z%&f3Y?lk;CBlhn0IqtPUSAE6cY?in-#m%Z{Qb3V$FVZ@vyutHEV!RQ=H#dD=8J&D? zbl$jZQ>ZOR`P(~qYUj>ze)4g>@CY5}CxSQ8zL#COm|Zy};DhkbWe>Ze`+OXQ=X^VJ zx1jjje-r(klN*8lO8((hMudyvGS8XN@fmEqu^jF(v&1QPd@OtlB{*|SxY0RsXwI}i z|8~y7E-Z2rqTE_VLt3^JnMFavViN>dRfZ=WTLWU%MTnKTL$)N2X^|3(HGm1UKp4b= zgcOCX5mVG7*dlf;!ca_}ahF8GmM_aee2ijMf8h{j#kmzGz_A^Wza6Xwjvz+VI_Y~) z2pP@zdp2*h2Q4{(1%$eiS)ia}GoITl8gXmNn3)u6#3dVq64CTAp!N#4iSZPgo$rWP z0XO2jw#|-b3$oGiS5?mIPf?b>rY(QB?+YA$hE}lV04b|3II~#bC|0y>Dt(|v?7Z@= zf2Hxg@qB?aD$Jl>WY4_lf~|MKN?ovG7c9`1!J z`KR@`(2X}1t<7C#?2A9wDV`mcoFqLP}Wb5w=Kja;nHWEV?sc)dpL-%0$H4dzXf zH!S^n^6k^_!+c?Hy0SO@c|V#r^lJw~XM4W%d`addiFgLKPgS3sH?P+|9z}6~D*deo z9(0+vpFH%e*736B=V?8h*25|9{CI<`hyQi2DH(1(Jnh%RyJs< zW$#|#Ha_^Mi4TB)paL(x_$EI^e=q`%#ssmnP>Mw;rIgmcG*$(F5}h;Mvv+syUH9Gv zLz?)VY|ovubIzPObLQ+!$0>^P7=Ma94`7^9s!Va(!0Td$zKkVJO-sTneMc=@CmQenVnSpSYhb4EMKa|wk0ehrN?Uw&9H-D$D9^i|Gl!Zk=vZ%d{x}>&hCb zWwV&qJ0AO#ZS!ho?9^Ap&}fZCRhwqofMrypvTSK8nXZocy{T38f4D{)S(0sK8ne?v zMm4j+Fj?$nns)m!yMZP>!9$;|lxefAZoKOGxMGM}9&2qh-u2XCs>WK?OgE)Ll>)%A zE=jFrs;zou)uyTO7=T6hmGD;c=f;MWtffwmX(><6Y|J6sltWgNLsp$b=Cs#RJx(oW zooX~@n$l{r*K5>Re>08C`4FWp#QCJcL-O#D57gi^WMJs$f zgxBAw@j*YM#78%*7FCNju9~uqhcRwdi2ejgAKH9>?x0W^0^%0--6-b2COUc#>VKI2 zI7uJcmQ#lFiF=Vxzesr|hu?*KCl#faM2T0iGj5b~!dGYI!H->%Z zhC=;N@`xKE%KnniOZ;3CIZs+xJPu_#Y@V{ecy4(VdQtLudy1xLj&Tb)kN#KdIabc; zQS|bSOF`>jRM`2s+-U>Ha}JGH>$-F9YkXnUZm&|9-KkC+_)41KM~*tZ3F@xrKq1ci z1=@Yo_u?q|e?M08aV+UQ%zN>_ecu2<_hQ*Tp0P6fvQ#N5E-8(APD(M`Ftn{I-l!hz ze!ZY7N_{AE+Vhp{JCA&~$!DGurFl;D&hy#KJa;RF`T2y?2Ck_|yO9^Nxa07{^sB0} zFuxvm+PsV^cG|=m)hw>9;-_gDCR~(hQi(M)EpAb$e~(F%#uyw@^Ynj`n15iIX@*2<$GBOqDHj(?c z9!hf>eL>I2iI;V2#aK}Hie-Y?fXgrbc6u3)1*bLM|<{Yl%bRp0V z#(mz=xtuPNJ!^n2k$3BXI?3^4fLnQwj~wG6uOsFa#vkPvXMB%se7}1&P`lHn_JZ zf6n)y{e^Z%+r#esar`9$L*(q|fFGQEyAZg}LF4b(pW<{UeEe$_oVv+@MxgBYW4?ur zkNJr0pgap2ALo1SEZ|3S{v6<^!1?dp1Gtnmp6rjThwC|8pojeRC*b?6@p1n0`ePo+ z>mMW!TL)Z!IsX1QW%T4Y%k#v%$MSC zPXKOb^~boS5ANbLNIp9Q7|QA=^ZWMzcgc2vTjc#sK;O9YmHEBj00UWe7H_gK)3ro$PU)u9RdSnp z2;hbrS6Lt9x_=$e6@OALaMd0E!!1D9L*t1YzyG88dV@?8=nS-jamPHK*)@aH6;jb% z&gm*i?zK4eW$FC`00960>{eTB6jd0WZGjYZp->7n-1Y@2+OTbrNTBw#6ugB7q(u`n zHq+hdc3?WQ?9M{L2*wwE(G*O$7z4ieK;na33Yd6%QXhr36o0k!expDEg`yF~^Pio6 zc6w%q*=_Je`+b@I{pZa2&wu{Q*&_&mn?!JsI~fMaf&GBjwZOxPe?(28Vr~Ji?|2&3v^E*ENPV0aCI+$~Z^H|LMGjx)#I=-a5W?SDW|ClE} z|4#4uJRGY5b~)C+E&km8`8f0Y#Jb1v537JJW`8>Mx0^h9{sd)@ZGXI7?2mcvb15&H z^@IKI)EB(Oc=G$5!_ewjU%ZowARK?)WFx~c8M^}*AW?=svT_U1Zg2PDM#^EbieZ?1 zap@f8uE_Rx%8bv0R{Y{h~eW zuUj_(-Dcd;9~^hGgZsz2zH=MUPgXGWky95=Qudm0<>zBu!wlWzRyHuPeldT8bKTnV zcW4*o5IJ=c&`TC~Eu!pnw1eyTx))P+krST-x=GQ?z#jAWINdtJc<_0|y2AM5dyF%V zqmJX~->`T4=8SL?L+9%dMj^li%^EOrabTzQw^&H3k*wGZGOw?fkel zJ)$WVJJx+pbBWt@<2~SQPeB>X&&rxu+`pyI7FE`xLITfG@2r3C*3x8zB3euhR%kTw zW@X{`73)M@#!uUMq(I>d3 zYvcQeM#z;Xin4z`TQEXidMx9qcjsPV=hNZLguUz}_N;k@@9Q%sRtFHj22oeU+K8s< z@$sk>7HON)jH9mVvMOQO3Ux6x07Mb~#S!Z(-x3#kBJU0xT~H@0 z#(%rC$o6Ph48-Gg*u}pf7cbDGbxR7%^6NL&Zj%CffwYr`C`kKaSxAqH4eJ}lhOi<< z^Ow>#lZvZ0lvh^Bk$@uAVB0P&uuUxp#bl6ZblEaNnE3f7s+zjHL5tz};g$!4)eg?H z#%C7yorix~s;t;p1^VNHtsZLAB>`H5Qlx*hc%2U_)&~{qEvb$8{H5{2^?JP} ze&eGOKbw`oO3Coi5fcv5d%NxXP4@yNkM{{$0bh)FRGyCoC z@7tN*t6w)9@dOg8&p#HCuyic~aTZbvaln6{M3^FVEa1=5oID230ssc}qg6lJ_{>HN z$1aY=?B+AGdttJ3m$7r1>{>uG@Nmp#8ESL^%2H|$W(40I6JK)~zAh7=m*ZKYq2bs} zII_3J;`$oRXP-kA_}gv#4CkdJ!Bi!(iDwk-?|-u&+xc}iw!mPB*?Ze+i-VWrE&YF( z#}d^*aopBdXTCNLlS>(uFlMr>Ea4N)>rVEAX`1 zcoU=fdA=Lm=^1K!b!j(v_l*1Mh7_nmsz+R4OeKs&+JbZ*Cs|P9S%_0IGTuL1!GAuR z{}}e?HPtVZB$WM;@Z3yz+<$L)aA*j|V>F-A-)Zkp<#=5dUKj9lf7EIh;&y*NcWr(Y z`PFCg3jlOQ;Z=~9WPbLLs7smxlc=PBmRn!O<}Ze|^p(L-UpAUpL`gb;9_TJg>2=Yc zp&HChEyJ85BdxD@R)!d=tdGUmCB4MrqH=o&N~{Mmd5O(}x4*yc{su9@D>;%E{i9MG z#4z&+JF|Z}ZO^%MrpQDz#_)dyP?C@FqNj^|=KA#5)*)(J@H5<9IQ{H4-Rt1GzAj+2{*Z^Io&Po1fY1vz%SC?Y%zcr}y9Ay!`nzcs=;z+5GmWV8_qMpsSu0{&!>SOuJWs68#yI?u?dZnZ=i|c=DG@pGARp38- zy)tCvFL3mVqgQ~VR|>o*N3W!8{$jXVy`ul^8Zo+0zOoK#tLsX7B~)SG9l*M8hHZt} zP#&?aYS(wz`sCb3+Q@&`kEa}c;^-4cpA>jcjy_4){KasU`Xr|JbVL@SGf>Ucp-voq z@;80b364ge^c=B1`S$ho#jacL%IcF4sQve~kf6SWsLx2zW2HMhz`7sL-hOz&HJAs0 z|4esmrJe60z|kFI{f_Q9S-RsO-$nL5?@p^w$yTpC{r~^~|NoQgC#HW2r{gcS&ivMA zU;O9g-T0sz_Wa*`WnFns*fYEDSlKjTr9H^p31fU>`?cUYn#M1${gBqQrs>8-1v}jXqd8h2cH{NgopyeaPX)3iJstlndzd`(h(&CJy(vp(=JR?(Tn}dx802?$)ld~= Date: Mon, 3 Sep 2012 18:30:34 +0000 Subject: [PATCH 24/53] Fixed installing .svn directories in matlab toolbox --- matlab/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 0be3863a1..16cfb501a 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -2,17 +2,16 @@ # Tests message(STATUS "Installing Matlab Toolbox Tests") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN "*.fig" PATTERN ".svn" EXCLUDE) # Utilities message(STATUS "Installing Matlab Toolbox Utilities") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}") message(STATUS "Installing Matlab Toolbox Examples (Data)") From f16a981a27a55637d8cb53d0107a9e478aca23ee Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 18:32:49 +0000 Subject: [PATCH 25/53] Added first pass of script to automatically generate self-contained precompiled matlab toolbox --- CMakeLists.txt | 1 + package_scripts/toolbox_package_unix.sh | 43 +++++++++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100755 package_scripts/toolbox_package_unix.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 3df8ab232..5c1c5cdab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -175,6 +175,7 @@ set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSIO #set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") 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 diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh new file mode 100755 index 000000000..44d0dac87 --- /dev/null +++ b/package_scripts/toolbox_package_unix.sh @@ -0,0 +1,43 @@ +#!/bin/sh + +# Detect platform +os=`uname -s` +arch=`uname -p` +if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then + platform=lin64 +elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then + platform=lin32 +else + echo "Unrecognized platform" + exit 1 +fi + +echo "Platform is ${platform}" + +# Check for empty directory +if [ ! -z "`ls`" ]; then + echo "Please run this script from an empty build directory" + exit 1 +fi + +# Check for boost +if [ -z "$1" -o -z "$2" ]; then + echo "Usage: $0 BOOSTTREE MEX" + echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost." + echo "MEX should be the full path of the mex compiler" + exit 1 +fi + +# Run cmake +cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="$PWD/stage" -DBoost_NO_SYSTEM_PATHS:bool=true -DBoost_USE_STATIC_LIBS:bool=true -DBOOST_ROOT="$1" -DGTSAM_BUILD_UNSTABLE:bool=false -DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true -DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true -DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true -DMEX_COMMAND="$2" .. + +if [ ! $? ]; then + echo "CMake failed" + exit 1 +fi + +# Compile +make -j4 install + +# Create package +tar czf gtsam-toolbox-2.1.0-$platform.tgz -C stage/borg toolbox From 9497a233ce9805f729b9495bcedf288a6dd400f2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 18:36:05 +0000 Subject: [PATCH 26/53] Cleaned up all remaining commented-out unit tests from the old linear rewrite - removed or reenabled as needed. --- gtsam/inference/tests/testFactorGraph.cpp | 59 ------- tests/testGaussianFactor.cpp | 184 ---------------------- tests/testGaussianISAM.cpp | 27 +--- tests/testGraph.cpp | 59 +++++++ tests/testNonlinearFactor.cpp | 4 +- 5 files changed, 62 insertions(+), 271 deletions(-) diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index b2f9bc3f1..bfbe716d4 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -60,65 +60,6 @@ TEST(FactorGraph, eliminateFrontals) { EXPECT(assert_equal(expectedCond, *actualCond)); } -///* ************************************************************************* */ -// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x1", "x3"); -// G.push_factor("x1", "x4"); -// G.push_factor("x2", "x3"); -// G.push_factor("x2", "x4"); -// G.push_factor("x3", "x4"); -// -// SymbolicFactorGraph T, C; -// boost::tie(T, C) = G.splitMinimumSpanningTree(); -// -// SymbolicFactorGraph expectedT, expectedC; -// expectedT.push_factor("x1", "x2"); -// expectedT.push_factor("x1", "x3"); -// expectedT.push_factor("x1", "x4"); -// expectedC.push_factor("x2", "x3"); -// expectedC.push_factor("x2", "x4"); -// expectedC.push_factor("x3", "x4"); -// CHECK(assert_equal(expectedT,T)); -// CHECK(assert_equal(expectedC,C)); -//} - -///* ************************************************************************* */ -///** -// * x1 - x2 - x3 - x4 - x5 -// * | | / | -// * l1 l2 l3 -// */ -// SL-FIX TEST( FactorGraph, removeSingletons ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x2", "x3"); -// G.push_factor("x3", "x4"); -// G.push_factor("x4", "x5"); -// G.push_factor("x2", "l1"); -// G.push_factor("x3", "l2"); -// G.push_factor("x4", "l2"); -// G.push_factor("x4", "l3"); -// -// SymbolicFactorGraph singletonGraph; -// set singletons; -// boost::tie(singletonGraph, singletons) = G.removeSingletons(); -// -// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; -// CHECK(singletons_excepted == singletons); -// -// SymbolicFactorGraph singletonGraph_excepted; -// singletonGraph_excepted.push_factor("x2", "l1"); -// singletonGraph_excepted.push_factor("x4", "l3"); -// singletonGraph_excepted.push_factor("x1", "x2"); -// singletonGraph_excepted.push_factor("x4", "x5"); -// singletonGraph_excepted.push_factor("x2", "x3"); -// CHECK(singletonGraph_excepted.equals(singletonGraph)); -//} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 0097ec49a..3fe92c50d 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -66,33 +66,6 @@ TEST( GaussianFactor, linearFactor ) EXPECT(assert_equal(expected,*lf)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, keys ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactor::shared_ptr lf = fg[1]; -// list expected; -// expected.push_back(kx1); -// expected.push_back(kx2); -// EXPECT(lf->keys() == expected); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, dimensions ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // Check a single factor -// Dimensions expected; -// insert(expected)(kx1, 2)(kx2, 2); -// Dimensions actual = fg[1]->dimensions(); -// EXPECT(expected==actual); -//} - /* ************************************************************************* */ TEST( GaussianFactor, getDim ) { @@ -110,62 +83,6 @@ TEST( GaussianFactor, getDim ) EXPECT_LONGS_EQUAL(expected, actual); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, combine ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get two factors from it and insert the factors into a vector -// vector lfg; -// lfg.push_back(fg[4 - 1]); -// lfg.push_back(fg[2 - 1]); -// -// // combine in a factor -// GaussianFactor combined(lfg); -// -// // sigmas -// double sigma2 = 0.1; -// double sigma4 = 0.2; -// Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2); -// -// // the expected combined linear factor -// Matrix Ax2 = Matrix_(4, 2, // x2 -// -5., 0., -// +0., -5., -// 10., 0., -// +0., 10.); -// -// Matrix Al1 = Matrix_(4, 2, // l1 -// 5., 0., -// 0., 5., -// 0., 0., -// 0., 0.); -// -// Matrix Ax1 = Matrix_(4, 2, // x1 -// 0.00, 0., // f4 -// 0.00, 0., // f4 -// -10., 0., // f2 -// 0.00, -10. // f2 -// ); -// -// // the RHS -// Vector b2(4); -// b2(0) = -1.0; -// b2(1) = 1.5; -// b2(2) = 2.0; -// b2(3) = -1.0; -// -// // use general constructor for making arbitrary factors -// vector > meas; -// meas.push_back(make_pair(kx2, Ax2)); -// meas.push_back(make_pair(kl1, Al1)); -// meas.push_back(make_pair(kx1, Ax1)); -// GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4))); -// EXPECT(assert_equal(expected,combined)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, error ) { @@ -186,47 +103,6 @@ TEST( GaussianFactor, error ) DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, eliminate ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get two factors from it and insert the factors into a vector -// vector lfg; -// lfg.push_back(fg[4 - 1]); -// lfg.push_back(fg[2 - 1]); -// -// // combine in a factor -// GaussianFactor combined(lfg); -// -// // eliminate the combined factor -// GaussianConditional::shared_ptr actualCG; -// GaussianFactor::shared_ptr actualLF; -// boost::tie(actualCG,actualLF) = combined.eliminate(kx2); -// -// // create expected Conditional Gaussian -// Matrix I = eye(2)*sqrt(125.0); -// Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = I*Vector_(2,0.2,-0.14); -// -// // Check the conditional Gaussian -// GaussianConditional -// expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0)); -// -// // the expected linear factor -// I = eye(2)/0.2236; -// Matrix Bl1 = I, Bx1 = -I; -// Vector b1 = I*Vector_(2,0.0,0.2); -// -// GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0)); -// -// // check if the result matches -// EXPECT(assert_equal(expectedCG,*actualCG,1e-3)); -// EXPECT(assert_equal(expectedLF,*actualLF,1e-3)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, matrix ) { @@ -328,66 +204,6 @@ void print(const list& i) { cout << endl; } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx1,kx2; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 1,2,3,4; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse2 ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx2,kl1,kx1; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 5,6,1,2; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - /* ************************************************************************* */ TEST( GaussianFactor, size ) { diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 7ec4e5317..ea91eda5a 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -47,7 +47,7 @@ static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sig static const double tol = 1e-4; /* ************************************************************************* */ -TEST_UNSAFE( ISAM, iSAM_smoother ) +TEST( ISAM, iSAM_smoother ) { Ordering ordering; for (int t = 1; t <= 7; t++) ordering += X(t); @@ -76,31 +76,6 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) EXPECT(assert_equal(e, optimized)); } -/* ************************************************************************* */ -// SL-FIX TEST( ISAM, iSAM_smoother2 ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// -// // Create initial tree from first 4 timestamps in reverse order ! -// Ordering ord; ord += X(4),X(3),X(2),X(1); -// GaussianFactorGraph factors1; -// for (int i=0;i<7;i++) factors1.push_back(smoother[i]); -// GaussianISAM actual(*inference::Eliminate(factors1)); -// -// // run iSAM with remaining factors -// GaussianFactorGraph factors2; -// for (int i=7;i<13;i++) factors2.push_back(smoother[i]); -// actual.update(factors2); -// -// // Create expected Bayes Tree by solving smoother with "natural" ordering -// Ordering ordering; -// for (int t = 1; t <= 7; t++) ordering += symbol('x', t); -// GaussianISAM expected(smoother.eliminate(ordering)); -// -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* * Bayes tree for smoother with "natural" ordering: C1 x6 x7 diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 71a075ccb..14655a249 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -148,6 +148,65 @@ TEST( Graph, composePoses ) // LONGS_EQUAL(2, Ab2.size()); //} +///* ************************************************************************* */ +// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x1", "x3"); +// G.push_factor("x1", "x4"); +// G.push_factor("x2", "x3"); +// G.push_factor("x2", "x4"); +// G.push_factor("x3", "x4"); +// +// SymbolicFactorGraph T, C; +// boost::tie(T, C) = G.splitMinimumSpanningTree(); +// +// SymbolicFactorGraph expectedT, expectedC; +// expectedT.push_factor("x1", "x2"); +// expectedT.push_factor("x1", "x3"); +// expectedT.push_factor("x1", "x4"); +// expectedC.push_factor("x2", "x3"); +// expectedC.push_factor("x2", "x4"); +// expectedC.push_factor("x3", "x4"); +// CHECK(assert_equal(expectedT,T)); +// CHECK(assert_equal(expectedC,C)); +//} + +///* ************************************************************************* */ +///** +// * x1 - x2 - x3 - x4 - x5 +// * | | / | +// * l1 l2 l3 +// */ +// SL-FIX TEST( FactorGraph, removeSingletons ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x2", "x3"); +// G.push_factor("x3", "x4"); +// G.push_factor("x4", "x5"); +// G.push_factor("x2", "l1"); +// G.push_factor("x3", "l2"); +// G.push_factor("x4", "l2"); +// G.push_factor("x4", "l3"); +// +// SymbolicFactorGraph singletonGraph; +// set singletons; +// boost::tie(singletonGraph, singletons) = G.removeSingletons(); +// +// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; +// CHECK(singletons_excepted == singletons); +// +// SymbolicFactorGraph singletonGraph_excepted; +// singletonGraph_excepted.push_factor("x2", "l1"); +// singletonGraph_excepted.push_factor("x4", "l3"); +// singletonGraph_excepted.push_factor("x1", "x2"); +// singletonGraph_excepted.push_factor("x4", "x5"); +// singletonGraph_excepted.push_factor("x2", "x3"); +// CHECK(singletonGraph_excepted.equals(singletonGraph)); +//} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index b1717a79b..c8824749d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -73,8 +73,8 @@ TEST( NonlinearFactor, equals2 ) Graph::sharedFactor f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); -// SL-FIX CHECK(!f0->equals(*f1)); -// SL-FIX CHECK(!f1->equals(*f0)); + CHECK(!f0->equals(*f1)); + CHECK(!f1->equals(*f0)); } /* ************************************************************************* */ From 43f8613ec521cdfa13769bee6e77c26128ff7a06 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 18:57:05 +0000 Subject: [PATCH 27/53] Fix in testDataset --- gtsam/slam/tests/testDataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 2df2228af..aae3fc5da 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; TEST(dataSet, findExampleDataFile) { - const string expected_end = "gtsam/examples/Data/example.graph"; + const string expected_end = "examples/Data/example.graph"; const string actual = findExampleDataFile("example"); string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size()); boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash From 21d9d8aa0cabb393225cc2295baf9b4ab7ae3822 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 19:25:27 +0000 Subject: [PATCH 28/53] Print ordering with multiple entries per line for more compact printout --- gtsam/nonlinear/Ordering.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 9f99948a4..b445af506 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -44,10 +44,23 @@ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const cout << str; // Print ordering in index order Ordering::InvertedMap inverted = this->invert(); - cout << "key (zero-based order)\n"; + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) { - cout << keyFormatter(index_key.second) << " (" << index_key.first << ")\n"; + if(index_key.first % varsPerLine != 0) + cout << ", "; + cout << index_key.first << ":" << keyFormatter(index_key.second); + if(index_key.first % varsPerLine == varsPerLine - 1) { + cout << "\n"; + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if(!endedOnNewline) + cout << "\n"; cout.flush(); } From af652b0e04c5fcb8a3e44c659ec124477b20f090 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 3 Sep 2012 19:43:08 +0000 Subject: [PATCH 29/53] remove simpleSPCG reorg SubgraphSolver add unit test for SubgraphSolver --- examples/Pose2SLAMwSPCG.cpp | 12 - gtsam/linear/JacobianFactorGraph.cpp | 15 +- gtsam/linear/JacobianFactorGraph.h | 4 +- gtsam/linear/SimpleSPCGSolver.cpp | 226 ------------------ gtsam/linear/SimpleSPCGSolver.h | 98 -------- gtsam/linear/SubgraphPreconditioner.cpp | 4 +- gtsam/linear/SubgraphPreconditioner.h | 9 +- gtsam/linear/SubgraphSolver-inl.h | 80 ------- gtsam/linear/SubgraphSolver.cpp | 178 +++++++++----- gtsam/linear/SubgraphSolver.h | 69 +++++- .../SuccessiveLinearizationOptimizer.cpp | 7 +- tests/testSubgraphPreconditioner.cpp | 4 +- tests/testSubgraphSolver.cpp | 106 ++++++++ 13 files changed, 316 insertions(+), 496 deletions(-) delete mode 100644 gtsam/linear/SimpleSPCGSolver.cpp delete mode 100644 gtsam/linear/SimpleSPCGSolver.h delete mode 100644 gtsam/linear/SubgraphSolver-inl.h create mode 100644 tests/testSubgraphSolver.cpp diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 240361db7..36f2552a1 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -54,13 +54,9 @@ // for each variable, held in a Values container. #include -// ??? -#include #include #include - - using namespace std; using namespace gtsam; @@ -110,14 +106,6 @@ int main(int argc, char** argv) { parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "simple spcg solver final error = " << graph.error(result) << endl; - } - { parameters.iterativeParams = boost::make_shared(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp index 0ca3b8667..70c57e8df 100644 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ b/gtsam/linear/JacobianFactorGraph.cpp @@ -115,7 +115,20 @@ JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gf return jfg; } - +/* ************************************************************************* */ +JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg) { + JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); + jfg->reserve(gfg.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr & factor, gfg) { + if( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor) ) { + jfg->push_back(jf); + } + else { + jfg->push_back(boost::make_shared(*factor)); + } + } + return jfg; +} } // namespace diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h index cb27b507a..1164652c3 100644 --- a/gtsam/linear/JacobianFactorGraph.h +++ b/gtsam/linear/JacobianFactorGraph.h @@ -67,7 +67,9 @@ namespace gtsam { void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x); - /** dynamic_cast the gaussian factors down to jacobian factors */ + /** dynamic_cast the gaussian factors down to jacobian factors, may throw exception if it contains non-Jacobian Factor */ JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg); + /** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */ + JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg); } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp deleted file mode 100644 index fa0fee82f..000000000 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ /dev/null @@ -1,226 +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 - - * -------------------------------------------------------------------------- */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -/* utility function */ -std::vector extractRowSpec_(const FactorGraph& jfg) { - std::vector spec; spec.reserve(jfg.size()); - BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { - spec.push_back(jf->rows()); - } - return spec; -} - -std::vector extractColSpec_(const FactorGraph& gfg, const VariableIndex &index) { - const size_t n = index.size(); - std::vector spec(n, 0); - for ( Index i = 0 ; i < n ; ++i ) { - const GaussianFactor &gf = *(gfg[index[i].front()]); - for ( GaussianFactor::const_iterator it = gf.begin() ; it != gf.end() ; ++it ) { - spec[*it] = gf.getDim(it); - } - } - return spec; -} - -SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) - : Base(), parameters_(parameters) -{ - std::vector colSpec = extractColSpec_(gfg, VariableIndex(gfg)); - - nVar_ = colSpec.size(); - - /* Split the factor graph into At (tree) and Ac (constraints) - * Note: This part has to be refined for your graph/application */ - GaussianFactorGraph::shared_ptr At; - boost::tie(At, Ac_) = this->splitGraph(gfg); - - /* construct row vector spec of the new system */ - nAc_ = Ac_->size(); - std::vector rowSpec = extractRowSpec_(*Ac_); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - rowSpec.push_back(colSpec[i]); - } - - /* solve the tree with direct solver. get preconditioner */ - Rt_ = EliminationTree::Create(*At)->eliminate(EliminateQR); - xt_ = boost::make_shared(gtsam::optimize(*Rt_)); - - /* initial value for the lspcg method */ - y0_ = boost::make_shared(VectorValues::Zero(colSpec)); - - /* the right hand side of the new system */ - by_ = boost::make_shared(VectorValues::Zero(rowSpec)); - for ( Index i = 0 ; i < nAc_ ; ++i ) { - JacobianFactor::shared_ptr jf = (*Ac_)[i]; - Vector xi = internal::extractVectorValuesSlices(*xt_, jf->begin(), jf->end()); - (*by_)[i] = jf->getb() - jf->getA()*xi; - } - - /* allocate buffer for row and column vectors */ - tmpY_ = boost::make_shared(VectorValues::Zero(colSpec)); - tmpB_ = boost::make_shared(VectorValues::Zero(rowSpec)); -} - -/* implements the CGLS method in Section 7.4 of Bjork's book */ -VectorValues SimpleSPCGSolver::optimize (const VectorValues &initial) { - - VectorValues::shared_ptr x(new VectorValues(initial)); - VectorValues r = VectorValues::Zero(*by_), - q = VectorValues::Zero(*by_), - p = VectorValues::Zero(*y0_), - s = VectorValues::Zero(*y0_); - - residual(*x, r); - transposeMultiply(r, s) ; - p.vector() = s.vector() ; - - double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ; - - const double threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); - const size_t iMaxIterations = parameters_.maxIterations(); - const Parameters::Verbosity verbosity = parameters_.verbosity(); - - if ( verbosity >= ConjugateGradientParameters::ERROR ) - std::cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon() - << ", max = " << parameters_.maxIterations() - << ", ||r0|| = " << std::sqrt(gamma) - << ", threshold = " << threshold << std::endl; - - size_t k ; - for ( k = 1 ; k < iMaxIterations ; ++k ) { - - multiply(p, q); - alpha = gamma / q.vector().squaredNorm() ; - x->vector() += (alpha * p.vector()); - r.vector() -= (alpha * q.vector()); - transposeMultiply(r, s); - new_gamma = s.vector().squaredNorm(); - beta = new_gamma / gamma ; - p.vector() = s.vector() + beta * p.vector(); - gamma = new_gamma ; - - if ( verbosity >= ConjugateGradientParameters::ERROR) { - std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl; - } - - if ( gamma < threshold ) break ; - } // k - - if ( verbosity >= ConjugateGradientParameters::ERROR ) - std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl; - - /* transform y back to x */ - return this->transform(*x); -} - -void SimpleSPCGSolver::residual(const VectorValues &input, VectorValues &output) { - output.vector() = by_->vector(); - this->multiply(input, *tmpB_); - axpy(-1.0, *tmpB_, output); -} - -void SimpleSPCGSolver::multiply(const VectorValues &input, VectorValues &output) { - this->backSubstitute(input, *tmpY_); - gtsam::multiply(*Ac_, *tmpY_, output); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - output[i + nAc_] = input[i]; - } -} - -void SimpleSPCGSolver::transposeMultiply(const VectorValues &input, VectorValues &output) { - gtsam::transposeMultiply(*Ac_, input, *tmpY_); - this->transposeBackSubstitute(*tmpY_, output); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - output[i] += input[nAc_+i]; - } -} - -void SimpleSPCGSolver::backSubstitute(const VectorValues &input, VectorValues &output) { - for ( Index i = 0 ; i < input.size() ; ++i ) { - output[i] = input[i] ; - } - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, *Rt_) { - const Index key = *(cg->beginFrontals()); - Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); - xS = input[key] - cg->get_S() * xS; - output[key] = cg->get_R().triangularView().solve(xS); - } -} - -void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, VectorValues &output) { - for ( Index i = 0 ; i < input.size() ; ++i ) { - output[i] = input[i] ; - } - BOOST_FOREACH(const boost::shared_ptr cg, *Rt_) { - const Index key = *(cg->beginFrontals()); - output[key] = gtsam::backSubstituteUpper(output[key], Matrix(cg->get_R())); - const Vector d = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()) - - cg->get_S().transpose() * output[key]; - internal::writeVectorValuesSlices(d, output, cg->beginParents(), cg->endParents()); - } -} - -VectorValues SimpleSPCGSolver::transform(const VectorValues &y) { - VectorValues x = VectorValues::Zero(y); - this->backSubstitute(y, x); - axpy(1.0, *xt_, x); - return x; -} - -boost::tuple::shared_ptr> -SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) { - - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - FactorGraph::shared_ptr Ac( new FactorGraph()); - - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - bool augment = false ; - - /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; - else { - BOOST_FOREACH ( const Index key, *gf ) { - if ( connected[key] == false ) { - augment = true ; - } - connected[key] = true; - } - } - - if ( augment ) At->push_back(gf); - else Ac->push_back(boost::dynamic_pointer_cast(gf)); - } - - return boost::tie(At, Ac); -} - - - - - - -} diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h deleted file mode 100644 index 994815fa4..000000000 --- a/gtsam/linear/SimpleSPCGSolver.h +++ /dev/null @@ -1,98 +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 - - * -------------------------------------------------------------------------- */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - -struct SimpleSPCGSolverParameters : public ConjugateGradientParameters { - typedef ConjugateGradientParameters Base; - SimpleSPCGSolverParameters() : Base() {} -}; - -/** - * This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10. - * - * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into - * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. - * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. - * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ - * - * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it - * with the least-squares variation of the conjugate gradient method. - * - * Note: A full SPCG implementation will come up soon in the next release. - * - * \nosubgrouping - */ - -class SimpleSPCGSolver : public IterativeSolver { - -public: - - typedef IterativeSolver Base; - typedef SimpleSPCGSolverParameters Parameters; - typedef boost::shared_ptr shared_ptr; - -protected: - - size_t nVar_ ; ///< number of variables \f$ x \f$ - size_t nAc_ ; ///< number of factors in \f$ A_c \f$ - FactorGraph::shared_ptr Ac_; ///< the constrained part of the graph - GaussianBayesNet::shared_ptr Rt_; ///< the gaussian bayes net of the tree part of the graph - VectorValues::shared_ptr xt_; ///< the solution of the \f$ A_t^{-1} b_t \f$ - VectorValues::shared_ptr y0_; ///< a column zero vector - VectorValues::shared_ptr by_; ///< \f$ [\bar{b_y} ; 0 ] \f$ - VectorValues::shared_ptr tmpY_; ///< buffer for the column vectors - VectorValues::shared_ptr tmpB_; ///< buffer for the row vectors - Parameters parameters_; ///< Parameters for iterative method - -public: - - SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); - virtual ~SimpleSPCGSolver() {} - virtual VectorValues optimize () {return optimize(*y0_);} - -protected: - - VectorValues optimize (const VectorValues &initial); - - /** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */ - void residual(const VectorValues &input, VectorValues &output); - - /** output = \f$ [A_c R_t^{-1} ; I] \f$ input */ - void multiply(const VectorValues &input, VectorValues &output); - - /** output = \f$ [R_t^{-T} A_c^T, I] \f$ input */ - void transposeMultiply(const VectorValues &input, VectorValues &output); - - /** output = \f$ R_t^{-1} \f$ input */ - void backSubstitute(const VectorValues &rhs, VectorValues &sol) ; - - /** output = \f$ R_t^{-T} \f$ input */ - void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ; - - /** return \f$ R_t^{-1} y + x_t \f$ */ - VectorValues transform(const VectorValues &y); - - /** naively split a gaussian factor graph into tree and constraint parts - * Note: This function has to be refined for your graph/application */ - boost::tuple::shared_ptr> - splitGraph(const GaussianFactorGraph &gfg); -}; - -} diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 91a98a263..b33adbc38 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { + Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { } /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 8c1fcc837..b109368af 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -40,7 +40,7 @@ namespace gtsam { typedef boost::shared_ptr sharedErrors; private: - sharedFG Ab1_, Ab2_; + sharedFG Ab2_; sharedBayesNet Rc1_; sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 @@ -51,16 +51,11 @@ namespace gtsam { /** * Constructor - * @param Ab1: the Graph A1*x=b1 * @param Ab2: the Graph A2*x=b2 * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, - const sharedBayesNet& Rc1, const sharedValues& xbar); - - /** Access Ab1 */ - const sharedFG& Ab1() const { return Ab1_; } + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); /** Access Ab2 */ const sharedFG& Ab2() const { return Ab2_; } diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h deleted file mode 100644 index 28b2caaac..000000000 --- a/gtsam/linear/SubgraphSolver-inl.h +++ /dev/null @@ -1,80 +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 -// -// * -------------------------------------------------------------------------- */ -// -//#pragma once -// -//#include -// -//#include -//#include -//#include -// -//namespace gtsam { -// -//template -//void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { -// -// GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); -// GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); -// -// if (parameters_->verbosity()) cout << "split the graph ..."; -// split(pairs_, *graph, *Ab1, *Ab2) ; -// if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; -// -// // // Add a HardConstraint to the root, otherwise the root will be singular -// // Key root = keys.back(); -// // T_.addHardConstraint(root, theta0[root]); -// // -// // // compose the approximate solution -// // theta_bar_ = composePoses (T_, tree, theta0[root]); -// -// LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! -// SubgraphPreconditioner::sharedBayesNet Rc1 = -// EliminationTree::Create(sacrificialAb1)->eliminate(&EliminateQR); -// SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); -// -// pc_ = boost::make_shared( -// Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); -//} -// -//template -//VectorValues::shared_ptr SubgraphSolver::optimize() const { -// -// // preconditioned conjugate gradient -// VectorValues zeros = pc_->zero(); -// VectorValues ybar = conjugateGradients -// (*pc_, zeros, *parameters_); -// -// boost::shared_ptr xbar = boost::make_shared() ; -// *xbar = pc_->x(ybar); -// return xbar; -//} -// -//template -//void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { -// // generate spanning tree -// PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); -// -// // make the ordering -// list keys = predecessorMap2Keys(tree_); -// ordering_ = boost::make_shared(list(keys.begin(), keys.end())); -// -// // build factor pairs -// pairs_.clear(); -// typedef pair EG ; -// BOOST_FOREACH( const EG &eg, tree_ ) { -// Key key1 = Key(eg.first), -// key2 = Key(eg.second) ; -// pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; -// } -//} -// -//} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 9b37d9a4b..14a0cc174 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -19,103 +19,171 @@ #include #include #include - #include #include - #include using namespace std; namespace gtsam { +/**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : parameters_(parameters) { + JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg); + initialize(*jfg); +} +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) + : parameters_(parameters) +{ + initialize(*jfg); +} - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); - GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) + : parameters_(parameters) { - boost::tie(Ab1, Ab2) = splitGraph(gfg) ; + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1)->eliminate(&EliminateQR); + JacobianFactorGraph::shared_ptr Ab2Jacobian = dynamicCastFactors(Ab2); + initialize(Rc1, Ab2Jacobian); +} - if (parameters_.verbosity()) - cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; - - // // Add a HardConstraint to the root, otherwise the root will be singular - // Key root = keys.back(); - // T_.addHardConstraint(root, theta0[root]); - // - // // compose the approximate solution - // theta_bar_ = composePoses (T_, tree, theta0[root]); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, + const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) + : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); - VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + initialize(Rc1, Ab2); +} - // Convert or cast Ab1 to JacobianFactors - boost::shared_ptr > Ab1Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab1->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab1) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab1Jacobians->push_back(jf); - else - Ab1Jacobians->push_back(boost::make_shared(*factor)); - } - - // Convert or cast Ab2 to JacobianFactors - boost::shared_ptr > Ab2Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab2->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab2) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab2Jacobians->push_back(jf); - else - Ab2Jacobians->push_back(boost::make_shared(*factor)); - } +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters) : parameters_(parameters) +{ + JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2); + initialize(Rc1, Ab2Jacobians); +} - pc_ = boost::make_shared(Ab1Jacobians, Ab2Jacobians, Rc1, xbar); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) +{ + initialize(Rc1, Ab2); } VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -boost::tuple -SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) { +void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) +{ + JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); + boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); + VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2); + pc_ = boost::make_shared(Ab2Jacobians, Rc1, xbar); +} - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2) +{ + VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + pc_ = boost::make_shared(Ab2, Rc1, xbar); +} + +boost::tuple +SubgraphSolver::splitGraph(const JacobianFactorGraph &jfg) { + + const VariableIndex index(jfg); + const size_t n = index.size(), m = jfg.size(); + DisjointSet D(n) ; + + JacobianFactorGraph::shared_ptr At(new JacobianFactorGraph()); + JacobianFactorGraph::shared_ptr Ac( new JacobianFactorGraph()); + + size_t t = 0; + BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { + + if ( jf->keys().size() > 2 ) { + throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + } bool augment = false ; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if ( jf->keys().size() == 1 ) augment = true; else { - BOOST_FOREACH ( const Index key, *gf ) { - if ( connected[key] == false ) { - augment = true ; - connected[key] = true; - } + const Index u = jf->keys()[0], v = jf->keys()[1], + u_root = D.find(u), v_root = D.find(v); + if ( u_root != v_root ) { + t++; augment = true ; + D.makeUnion(u_root, v_root); } } - - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if ( augment ) At->push_back(jf); + else Ac->push_back(jf); } return boost::tie(At, Ac); } +SubgraphSolver::DisjointSet::DisjointSet(const size_t n):n_(n),rank_(n,1),parent_(n) { + for ( Index i = 0 ; i < n ; ++i ) parent_[i] = i ; +} + +Index SubgraphSolver::DisjointSet::makeUnion(const Index &u, const Index &v) { + + Index u_root = find(u), v_root = find(v) ; + Index u_rank = rank(u), v_rank = rank(v) ; + + if ( u_root != v_root ) { + if ( v_rank > u_rank ) { + parent_[u_root] = v_root ; + rank_[v_root] += rank_[u_root] ; + return v_root ; + } + else { + parent_[v_root] = u_root ; + rank_[u_root] += rank_[v_root] ; + return u_root ; + } + } + return u_root ; +} + +Index SubgraphSolver::DisjointSet::find(const Index &u) { + vector path ; + Index x = u; + Index x_root = parent_[x] ; + + // find the root, and keep the vertices along the path + while ( x != x_root ) { + path.push_back(x) ; + x = x_root ; + x_root = parent_[x] ; + } + + // path compression + BOOST_FOREACH(const Index &i, path) { + rank_[i] = 1 ; + parent_[i] = x_root ; + } + + return x_root ; +} + + } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 60104bbe0..04b1cb127 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,9 @@ #include #include +#include #include +#include #include @@ -25,31 +27,86 @@ public: SubgraphSolverParameters() : Base() {} }; + /** - * A linear system solver using subgraph preconditioning conjugate gradient + * This class implements the SPCG solver presented in Dellaert et al in IROS'10. + * + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into + * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. + * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it + * with the least-squares variation of the conjugate gradient method. + * + * To use it in nonlinear optimization, please see the following example + * + * LevenbergMarquardtParams parameters; + * parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + * parameters.iterativeParams = boost::make_shared(); + * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + * Values result = optimizer.optimize(); + * + * \nosubgrouping */ class SubgraphSolver : public IterativeSolver { public: - typedef SubgraphSolverParameters Parameters; protected: - Parameters parameters_; SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object public: + /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); + SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters ¶meters); + + /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); + SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + + /* The same as above, but the A1 is solved before */ + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; protected: - boost::tuple - splitGraph(const GaussianFactorGraph &gfg) ; + void initialize(const JacobianFactorGraph &jfg); + void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2); + + boost::tuple + splitGraph(const JacobianFactorGraph &gfg) ; + +public: + + // a simplfied implementation of disjoint set data structure. + class DisjointSet { + protected: + size_t n_ ; + std::vector rank_ ; + std::vector parent_ ; + + public: + // initialize a disjoint set, point every vertex to itself + DisjointSet(const size_t n) ; + inline size_t size() const { return n_ ; } + + // union the root of u and and the root of v, return the root of u and v + Index makeUnion(const Index &u, const Index &v) ; + + // return the root of u + Index find(const Index &u) ; + + // return the rank of x, which is defined as the cardinality of the set containing x + inline size_t rank(const Index &x) {return rank_[find(x)] ;} + }; + }; } // namespace gtsam diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index af3f70ddc..9d85f26fe 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -60,11 +59,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ } else if ( params.isCG() ) { if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams)) { - SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); - delta = solver.optimize(); - } - else if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { + if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); delta = solver.optimize(); } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 60f737d67..b0a008478 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -120,7 +120,7 @@ TEST( SubgraphPreconditioner, system ) // Create Subgraph-preconditioned system VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config VectorValues zeros = VectorValues::Zero(xbar); @@ -197,7 +197,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) // Create Subgraph-preconditioned system VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); + SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config y0 and perturbed config y1 VectorValues y0 = VectorValues::Zero(xbar); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp new file mode 100644 index 000000000..04e0554e5 --- /dev/null +++ b/tests/testSubgraphSolver.cpp @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSubgraphSolver.cpp + * @brief Unit tests for SubgraphSolver + * @author Yong-Dian Jian + **/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +using namespace boost::assign; + +using namespace std; +using namespace gtsam; +using namespace example; + +/* ************************************************************************* */ +/** unnormalized error */ +double error(const JacobianFactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + total_error += factor->error(x); + return total_error; +} + + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor1 ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + SubgraphSolverParameters parameters; + SubgraphSolver solver(Ab, parameters); + VectorValues optimized = solver.optimize(); + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor2 ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + + SubgraphSolverParameters parameters; + SubgraphSolver solver(Ab1_, Ab2_, parameters); + VectorValues optimized = solver.optimize(); + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor3 ) +{ + // Build a planar graph + JacobianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + + SubgraphSolverParameters parameters; + SubgraphSolver solver(Rc1, Ab2_, parameters); + VectorValues optimized = solver.optimize(); + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From e9eb96a40855258b4cc2c06308f363cca244748e Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 3 Sep 2012 21:50:22 +0000 Subject: [PATCH 31/53] wrap iterative solver, subgraph solver, etc. fix the matlab spcg example --- gtsam.h | 41 +++++++++++++++ gtsam/linear/ConjugateGradientSolver.h | 32 ++++++------ gtsam/linear/IterativeSolver.cpp | 50 +++++++++++++++++++ gtsam/linear/IterativeSolver.h | 17 +++++-- gtsam/linear/SubgraphSolver.h | 2 +- .../SuccessiveLinearizationOptimizer.cpp | 4 ++ .../SuccessiveLinearizationOptimizer.h | 3 +- matlab/gtsam_examples/Pose2SLAMwSPCG.m | 15 ++++-- 8 files changed, 138 insertions(+), 26 deletions(-) create mode 100644 gtsam/linear/IterativeSolver.cpp diff --git a/gtsam.h b/gtsam.h index 4aa1480df..5b21f726f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1045,6 +1045,46 @@ class GaussianSequentialSolver { Matrix marginalCovariance(size_t j) const; }; +#include +virtual class IterativeOptimizationParameters { + string getKernel() const ; + string getVerbosity() const; + void setKernel(string s) ; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + size_t getMinIterations() const ; + size_t getMaxIterations() const ; + size_t getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(size_t value); + void setMaxIterations(size_t value); + void setReset(size_t value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print(string s) const; +}; + +class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters); + gtsam::VectorValues optimize() const; +}; + #include class KalmanFilter { KalmanFilter(size_t n); @@ -1288,6 +1328,7 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); bool isMultifrontal() const; bool isSequential() const; diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index cf1d31d85..a89d7a10c 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,9 +20,7 @@ namespace gtsam { */ class ConjugateGradientParameters : public IterativeOptimizationParameters { - public: - typedef IterativeOptimizationParameters Base; typedef boost::shared_ptr shared_ptr; @@ -49,7 +47,21 @@ public: inline double epsilon_rel() const { return epsilon_rel_; } inline double epsilon_abs() const { return epsilon_abs_; } - void print() const { + inline size_t getMinIterations() const { return minIterations_; } + inline size_t getMaxIterations() const { return maxIterations_; } + inline size_t getReset() const { return reset_; } + inline double getEpsilon() const { return epsilon_rel_; } + inline double getEpsilon_rel() const { return epsilon_rel_; } + inline double getEpsilon_abs() const { return epsilon_abs_; } + + inline void setMinIterations(size_t value) { minIterations_ = value; } + inline void setMaxIterations(size_t value) { maxIterations_ = value; } + inline void setReset(size_t value) { reset_ = value; } + inline void setEpsilon(double value) { epsilon_rel_ = value; } + inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } + inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } + + virtual void print(const std::string &s="") const { Base::print(); std::cout << "ConjugateGradientParameters: " << "minIter = " << minIterations_ @@ -61,18 +73,4 @@ public: } }; -//class ConjugateGradientSolver : public IterativeSolver { -// -//public: -// -// typedef ConjugateGradientParameters Parameters; -// -// Parameters parameters_; -// -// ConjugateGradientSolver(const ConjugateGradientParameters ¶meters) : parameters_(parameters) {} -// virtual VectorValues::shared_ptr optimize () = 0; -// virtual const IterativeOptimizationParameters& _params() const = 0; -//}; - - } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp new file mode 100644 index 000000000..2bba3e12b --- /dev/null +++ b/gtsam/linear/IterativeSolver.cpp @@ -0,0 +1,50 @@ +/** + * @file IterativeSolver.cpp + * @brief + * @date Sep 3, 2012 + * @author Yong-Dian Jian + */ + +#include +#include +#include + +namespace gtsam { + +std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); } +std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } + +IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "CG") return IterativeOptimizationParameters::CG; + /* default is cg */ + else return IterativeOptimizationParameters::CG; +} + +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; + /* default is default */ + else return IterativeOptimizationParameters::SILENT; +} + +std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) { + if ( k == CG ) return "CG"; + else return "UNKNOWN"; +} + +std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) return "SILENT"; + else if (verbosity == COMPLEXITY) return "COMPLEXITY"; + else if (verbosity == ERROR) return "ERROR"; + else return "UNKNOWN"; +} + + +} + + diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 8292b3831..2f76df2b7 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -13,6 +13,7 @@ #include #include +#include namespace gtsam { @@ -41,22 +42,32 @@ namespace gtsam { inline Kernel kernel() const { return kernel_; } inline Verbosity verbosity() const { return verbosity_; } + /* matlab interface */ + std::string getKernel() const ; + std::string getVerbosity() const; + void setKernel(const std::string &s) ; + void setVerbosity(const std::string &s) ; + void print() const { const std::string kernelStr[1] = {"cg"}; std::cout << "IterativeOptimizationParameters: " << "kernel = " << kernelStr[kernel_] << ", verbosity = " << verbosity_ << std::endl; } + + static Kernel kernelTranslator(const std::string &s); + static Verbosity verbosityTranslator(const std::string &s); + static std::string kernelTranslator(Kernel k); + static std::string verbosityTranslator(Verbosity v); }; class IterativeSolver { public: typedef boost::shared_ptr shared_ptr; - IterativeSolver(){} + IterativeSolver() {} virtual ~IterativeSolver() {} - /* update interface to the nonlinear optimizer */ - virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} + /* interface to the nonlinear optimizer */ virtual VectorValues optimize () = 0; }; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 04b1cb127..e4d99933b 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -25,9 +25,9 @@ class SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} + virtual void print(const std::string &s="") const { Base::print(s); } }; - /** * This class implements the SPCG solver presented in Dellaert et al in IROS'10. * diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index 9d85f26fe..fec460b08 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -15,6 +15,10 @@ namespace gtsam { +void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters ¶ms) { + iterativeParams = boost::make_shared(params); +} + void SuccessiveLinearizationParams::print(const std::string& str) const { NonlinearOptimizerParams::print(str); switch ( linearSolverType ) { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index e79a364c5..d422c09a9 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -74,6 +74,7 @@ public: std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setIterativeParams(const SubgraphSolverParameters ¶ms); void setOrdering(const Ordering& ordering) { this->ordering = ordering; } private: diff --git a/matlab/gtsam_examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m index a45ac40a0..67f22fe1d 100644 --- a/matlab/gtsam_examples/Pose2SLAMwSPCG.m +++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m @@ -51,7 +51,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -optimizer = DoglegOptimizer(graph, initialEstimate); -result = optimizer.optimizeSafely(); -result.print(sprintf('\nFinal result:\n')); \ No newline at end of file +%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver +params = gtsam.LevenbergMarquardtParams; +subgraphParams = gtsam.SubgraphSolverParameters; +params.setLinearSolverType('CONJUGATE_GRADIENT'); +params.setIterativeParams(subgraphParams); +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimize(); +result.print(sprintf('\nFinal result:\n')); + + + From 947588586c164b13e93f17eed831a32dc0102b4d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 22:11:09 +0000 Subject: [PATCH 32/53] Updated README to mention windows and MSVC --- README | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README b/README index 8e74f61e4..d7486c8b8 100644 --- a/README +++ b/README @@ -59,10 +59,12 @@ Tested compilers - GCC 4.2-4.7 - Clang 2.9-3.2 - OSX GCC 4.2 + - MSVC 2010, 2012 Tested systems: - Ubuntu 11.04, 11.10, 12.04 - MacOS 10.6, 10.7 + - Windows 7 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work From 9938b4785d72b24a55ef18b4aa0472a4d07e90ba Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 3 Sep 2012 22:48:08 +0000 Subject: [PATCH 33/53] Changed method summaries to lower case --- wrap/Class.cpp | 6 +-- wrap/tests/expected/Point2.m | 18 ++++---- wrap/tests/expected/Point3.m | 8 ++-- wrap/tests/expected/Test.m | 42 +++++++++---------- wrap/tests/expected_namespaces/+ns1/ClassA.m | 2 +- wrap/tests/expected_namespaces/+ns1/ClassB.m | 2 +- .../expected_namespaces/+ns2/+ns3/ClassB.m | 2 +- wrap/tests/expected_namespaces/+ns2/ClassA.m | 10 ++--- wrap/tests/expected_namespaces/+ns2/ClassC.m | 2 +- wrap/tests/expected_namespaces/ClassD.m | 2 +- 10 files changed, 47 insertions(+), 47 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index c5c306ce6..374324558 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -332,7 +332,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { @@ -352,7 +352,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const BOOST_FOREACH(ArgumentList argList, m.argLists) { string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << m.name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { @@ -373,7 +373,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const BOOST_FOREACH(ArgumentList argList, m.argLists) { string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << m.name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 913a869b5..d1c336c5f 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -1,15 +1,15 @@ %-------Constructors------- -%POINT2() -%POINT2(double x, double y) +%Point2() +%Point2(double x, double y) % %-------Methods------- -%ARGCHAR(char a) : returns void -%ARGUCHAR(unsigned char a) : returns void -%DIM() : returns int -%RETURNCHAR() : returns char -%VECTORCONFUSION() : returns VectorNotEigen -%X() : returns double -%Y() : returns double +%argChar(char a) : returns void +%argUChar(unsigned char a) : returns void +%dim() : returns int +%returnChar() : returns char +%vectorConfusion() : returns VectorNotEigen +%x() : returns double +%y() : returns double % %-------Static Methods------- % diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 965815dc6..f73b740ac 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -1,12 +1,12 @@ %-------Constructors------- -%POINT3(double x, double y, double z) +%Point3(double x, double y, double z) % %-------Methods------- -%NORM() : returns double +%norm() : returns double % %-------Static Methods------- -%STATICFUNCTIONRET(double z) : returns Point3 -%STATICFUNCTION() : returns double +%StaticFunctionRet(double z) : returns Point3 +%staticFunction() : returns double % %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html classdef Point3 < handle diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index d39025971..1e07974ee 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -1,27 +1,27 @@ %-------Constructors------- -%TEST() -%TEST(double a, Matrix b) +%Test() +%Test(double a, Matrix b) % %-------Methods------- -%ARG_EIGENCONSTREF(Matrix value) : returns void -%CREATE_MIXEDPTRS() : returns pair< Test, SharedTest > -%CREATE_PTRS() : returns pair< SharedTest, SharedTest > -%PRINT() : returns void -%RETURN_POINT2PTR(bool value) : returns Point2 -%RETURN_TEST(Test value) : returns Test -%RETURN_TESTPTR(Test value) : returns Test -%RETURN_BOOL(bool value) : returns bool -%RETURN_DOUBLE(double value) : returns double -%RETURN_FIELD(Test t) : returns bool -%RETURN_INT(int value) : returns int -%RETURN_MATRIX1(Matrix value) : returns Matrix -%RETURN_MATRIX2(Matrix value) : returns Matrix -%RETURN_PAIR(Vector v, Matrix A) : returns pair< Vector, Matrix > -%RETURN_PTRS(Test p1, Test p2) : returns pair< SharedTest, SharedTest > -%RETURN_SIZE_T(size_t value) : returns size_t -%RETURN_STRING(string value) : returns string -%RETURN_VECTOR1(Vector value) : returns Vector -%RETURN_VECTOR2(Vector value) : returns Vector +%arg_EigenConstRef(Matrix value) : returns void +%create_MixedPtrs() : returns pair< Test, SharedTest > +%create_ptrs() : returns pair< SharedTest, SharedTest > +%print() : returns void +%return_Point2Ptr(bool value) : returns Point2 +%return_Test(Test value) : returns Test +%return_TestPtr(Test value) : returns Test +%return_bool(bool value) : returns bool +%return_double(double value) : returns double +%return_field(Test t) : returns bool +%return_int(int value) : returns int +%return_matrix1(Matrix value) : returns Matrix +%return_matrix2(Matrix value) : returns Matrix +%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > +%return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > +%return_size_t(size_t value) : returns size_t +%return_string(string value) : returns string +%return_vector1(Vector value) : returns Vector +%return_vector2(Vector value) : returns Vector % %-------Static Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns1/ClassA.m b/wrap/tests/expected_namespaces/+ns1/ClassA.m index 2ce280041..ee87c12d7 100644 --- a/wrap/tests/expected_namespaces/+ns1/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns1/ClassA.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSA() +%ClassA() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns1/ClassB.m b/wrap/tests/expected_namespaces/+ns1/ClassB.m index 5c4d82882..7f393f03e 100644 --- a/wrap/tests/expected_namespaces/+ns1/ClassB.m +++ b/wrap/tests/expected_namespaces/+ns1/ClassB.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSB() +%ClassB() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m index ff5ed9e3c..4ad4a14f0 100644 --- a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m +++ b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSB() +%ClassB() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index d8f1dbc88..c56377c2a 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -1,13 +1,13 @@ %-------Constructors------- -%CLASSA() +%ClassA() % %-------Methods------- -%MEMBERFUNCTION() : returns double -%NSARG(ClassB arg) : returns int -%NSRETURN(double q) : returns ns2::ns3::ClassB +%memberFunction() : returns double +%nsArg(ClassB arg) : returns int +%nsReturn(double q) : returns ns2::ns3::ClassB % %-------Static Methods------- -%AFUNCTION() : returns double +%afunction() : returns double % %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html classdef ClassA < handle diff --git a/wrap/tests/expected_namespaces/+ns2/ClassC.m b/wrap/tests/expected_namespaces/+ns2/ClassC.m index 86cf9819f..050ef19f4 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassC.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassC.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSC() +%ClassC() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/ClassD.m b/wrap/tests/expected_namespaces/ClassD.m index 13bac35d6..41e9e63ec 100644 --- a/wrap/tests/expected_namespaces/ClassD.m +++ b/wrap/tests/expected_namespaces/ClassD.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSD() +%ClassD() % %-------Methods------- % From 403c6e39ad497bc44e627df9cfda99ddf28c949b Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 3 Sep 2012 23:24:37 +0000 Subject: [PATCH 34/53] Fixes to script to build self-contained matlab toolbox packages --- package_scripts/toolbox_package_unix.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 44d0dac87..b78649e15 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -2,7 +2,7 @@ # Detect platform os=`uname -s` -arch=`uname -p` +arch=`uname -m` if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then platform=lin64 elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then @@ -37,7 +37,7 @@ if [ ! $? ]; then fi # Compile -make -j4 install +make install # Create package tar czf gtsam-toolbox-2.1.0-$platform.tgz -C stage/borg toolbox From 2b4c0e1b140fba0932be461248f78a98af6c0a2c Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 4 Sep 2012 04:17:55 +0000 Subject: [PATCH 35/53] add printStats() and wrap saveGraph for ISAM2 --- gtsam.h | 2 ++ gtsam/nonlinear/ISAM2.h | 3 +++ 2 files changed, 5 insertions(+) diff --git a/gtsam.h b/gtsam.h index 5b21f726f..c5560668f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1484,6 +1484,8 @@ class ISAM2 { bool equals(const gtsam::ISAM2& other, double tol) const; void print(string s) const; + void printStats() const; + void saveGraph(string s) const; gtsam::ISAM2Result update(); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index deb2fc9d1..1cea78837 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -598,6 +598,9 @@ public: const ISAM2Params& params() const { return params_; } + /** prints out clique statistics */ + void printStats() const { getCliqueData().getStats().print(); } + //@} private: From 0f6ffcd4b202bf31b5a8ccd6da37ee3cf3844b5f Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 4 Sep 2012 04:19:47 +0000 Subject: [PATCH 36/53] add tooltips for VisualISAM_gui in matlab. Known bug: Figure won't update when run with "Save Figures" enabled. --- matlab/gtsam_examples/VisualISAM_gui.fig | Bin 10226 -> 10852 bytes matlab/gtsam_examples/VisualISAM_gui.m | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/gtsam_examples/VisualISAM_gui.fig b/matlab/gtsam_examples/VisualISAM_gui.fig index 3c8adb46c296faf06bd0e8ef5980bcbba3e68784..d4d65f56556bc11852652d2ea7f90ae7ba9a1b5b 100644 GIT binary patch delta 10437 zcmV;$C_2~jPvlgPA1qBlLrFF?EFeR2Wnpw>WFT*DIv`DNZXi=-a3CNvATl#LGdMak zI3O}GF*1=+BavVRf2k+{000000003=O%DJ703j#<0C=428BK2-HPqRpNk7u?)kB3; z6hIs}z^12gKo1{sF6udLuS*zah$m0pwoO(fO`45%2QE@@`qfG^JUfkAXz2W#>WKcegoiydOj{1ulq=;s6x*nG2t`nBgQcbfL+DJVaOBQz(+QQ zB`do!6FLqR#M9nr6e^tH!RY#MUd z=rQ5@1lv&RjrmK^1z8~dY=QmP`8Y_gv!v}RWYFe_e+5lF1+f2m_k;9$pBC-MA0+&B z&cssR*x=s!tPFjPPXBrM=3I<_QuwCA+Y0}x@GXV!D7>TaU4`!{d|%-Q3hyfXr@{{v ze)NpthlZWI#ysvD^VIY16`uP)ZC9uM*M87`(SFi?(|*)`)qd7~?-ic=KgLhX|4$EO z+}H&Bf5)?Fkd9}P&dU%Qz*6dk(b9_z#$=dF2Zk_ZsuJcz}h<6gf*X_ATPpi z2uZOtF~9DH_+63s=<&x67{5O?em|g57L=(9u-azZs_$h}j_9|5Mr6Nmb*ik7oLj=J<=hG~eTtnMNK_iQeOWw@<>X{ba05BAo`@*<$5VyDLF=I*Ydscn zo%u`GfvcdopX8)Bk*v71@`?IVk>y*oPh> z?aartD<}Ttp0M1|6CQ84OSu=ilsnO-f80Qqa-d7OlNoZI#H+`B4x0Ar82Epc1E9Tp zY0~hkqn$)BYjFQqT!f95C+ zP|*tzZ?INdm94e1F14~UF6LS;@4(^S({+;{-}_m>@Qii$+Vxx4u8&yD5@tS$aX4dj zd;b^1PNQz!&5!>wy!b|2`F6|x3^{G*98B7g6I5ScU*wlt`^XJt*nb1;1&@q6`hiim zZyV2d_ULcY+s6HX3FbH&w$F9@f8IX$N5Y$3*Hvde`{SeUUwg+2Z(CoUu2{PjTl?9Cxmcbmsq4ivGAiGwwu@!Q@YhT+?MV^ql5VpAl!kG!A?Mf9t95MKg|z z%)T>B)UHvqO31Dh4Nh#!ge*}Fp7}v4Bbr6LiwL{bW;g={%J7!pLoz|I>xsgUgE{P> z60fJmki3i7;nf6h9x_pmr{|jyq`@NP(BzdkYgE!2(J^IKiZ7e0`kCq&kg^xjyV!?mJklQ6Mo)-rgP zGI&!Nyon5+q8E`TawoM;Qw00Iv6^^&3ffiVnERodbHNdl?)HAof2Y`!FK#dO=wZYk zbG&~600960>{m~3(=ZfwyLDn5rh$;yfg5L}T@Y|;yLO`$8=BUw1B8UUG$L#b=B}r8-EtQ15AQAS>6i?egWMX6L;EtXAM5EgL0O970tMf6yB6I z?!w4FC@S4#VR_+Ur}6+DW{Lx$^t=r%%l?{5@TQQ2f47;}d525MyXoJj9dGf7 zcrSvNHqVkZPwl@7A%rpGo+Q6~clTV^vzXu&5oiAo@1%K|X#EQjuW0e?{a8{F-88;# zTObIgWDndbV_afMIBVY<^m~9N>-W0xb-M~4Ft^Ee^|`A%jm5h^VZYamuiK`>S)Bwv z#l4p7Q9!YNfB&U*&Ut6gJL()d>-$Yxyq&bpQumKM#eF}IzJIA`{=2!!_Q?(7KUWjp zVnhZgX)Byb8(+fCOHc5SxiwC?=ws2Z$jqL5jJu;Phn8Ir?01(14q%PD5a+g=7BYyX z$~+1dmKeZbEpY*ie_ACB5{HB`jcpTGc4F8X79L?J zVe32~v16-m8|HkBV=XBN^OD@AE){(N{esb08-%WYW6`PHhpN_Kb^eB}IKxhB0kniDrfuq;ZCbklwY%2g!#}db}s_#>k1Ks23uYR;N`E5K|CEX@-Xc+ru zo{qtujlpVTu<97h_#5hy0%6vvMv3oRt4Uteu+st8{vNWN3$B`U*1FW^gz1fY8oilT zx5v-PV&6O8x;@v$xh}qJy7=>r_a95YewHh@e|}@)e)@VDb@8M;2m84mJ|;i89?tb} zu7`8pW!A&T-D}$ZsviC^(8K$=9?tbJ%Js0eTo31ZIM>4$r-%Ol00960>{vl>8$}cz zJ5JPAN}D1CK?1Q7LP?}R(q0H$YR73CizKb$G%Xdv8t;znA>)~4cQ(O2^~jN9kG*r_ ze}u%L2a3eG;)H~n9{^It6(pG1o%QaHZFap$poKhXHT$0Ty?OIy-kbNi1Ol2A&l&L) z#51KvNi&K!5#`7M;JkQWRxZlK;#@9vU)lmiV$(>qPgnn9wT%M*ll{ zS>rO+{ud&>qNb`e~4N& z+G0-EHyZQzi!tq9RJ>l^QqqP>y4^I$9=y^ijQf2l@flH zr(c)-JUKsJp1iF1kHvy-QOsSCq-%_FUA_vCwjxUSeB2qcG z1-t!}16Qp!(!agxKo@N?3&nMJe`}f~@TEfA1ZlFw1WHyHfFJ!oz$(#R?&8l z-!LlLMGnVi+&d9_PxKmejUCWyW@xnPAjna~OIkw&>9SSN?N!XAH|vmEe+2Df7dJ7% zyywdhv3Hn_%@0wp{~hK#$c+Ur^x5%jj+^~>)y2I26zd|^B#3u-U%==yumiOK2)gAW zXAN=~msM<=h!{wV{hxeqN%Cym*}&ZzHHjBHGan6+-5nyU4v|%c$P~XVE-B=weX0^{ z+uCm8MirVJWa;N2PhGIYf1>mDr8dJ`1s6WQbcx|28h}V#|_c`V<$R;^I?We2RyG1I`^rV)N(5*M|YV{$Xmoe4A1HeK3fxtB{bUz|tYF<5nF% z=(ur*F~YIk7wcF_$5>h7cutF>qUaIW zbQn|c(?zy$i1+_Pe{cmf77#t$X3+Ae4?!CpAKd!$-%MHeo7%c1zZ46oyR=!^*{G~G zwl6PVy3~9Z+IFt~bqVC_d&^4CD2^A$a6ji`aRGq$Y<0?7IIQ~z%-M5oXa@0mkm4WG z;w5>v_4aakzH_7bDKhyY+85g*;p>9H>mDSvdS$cG_VE0ie?X(IXBXM@+Diy*o01(3hat^XWtUp zc<5$l*qu!pPC1fWkAM`ZXKwHp^iXba4+s8(BEb;}1oOPR@p!z|u75PHke{^L{nk65 zXWqQ`zPI+e5Dv46WZc~(lAtQtL%O}G4oe-g>X=hUKDs{>;hZ`yMQ5-=gzXmbh%Kq- zcX%F8e|LBa2|PKC2cF;6o}Ws2en!o#5Mh|_K9LTodz{PgbDs(|A;~=yV^=blI`KN2 zfop>kYAxb69NPZ=`zh;w&saC$WzC`f^45Cwv-OSI&h?em)%u4-h&Jy^3U989H;0xZ zfAl-fipE`?>d%kyewH-9AS90jjeNzN=<1Mre-C{@?9gA2Q|7m8_zSS#{9wf@wYKYD zP+Kn11GOy;6^Dxtm>&@5Ze@L|wimEc(U(+g1X8LUDig=46Z>Itsb|=ddKS(x;o@SJ z_?+pJ=*6%6uznck4@)^$9|Lt7HZB;z7N5#R@UD zet&lF`Lla}uVsmMowuM+Ma4*#yv3+{p8(%_MD0LQ%w38h)-Z2krlRjU4upy*YQ}e# zJX(k_oE_rJ2gG5GsK5?dS37wS-XA58e|b*hP0E{q+1!;LcQl?s;tT9}{*QJXJeeI# z2aeV8xyxjn!(egoi1BST+Y0+CPA_zy`z)+?p;e^ETul#pQV(ME6I@W1jW6NaB6X-~}8oL7=P-qnl6js2x{leb%kG z{tMKro2E`ynRKZ3xv6`mE|wi8gr=mX&W=(iht0>ddAw!J1MnvEg+q?;wrDF?rv0BY zcmPNHzY62OMx)J_Vdv9tJ>kt}e@{KhTHk*887y!8dUASpa*D0*z6D#0>f1_KH*dYz zKXK0L{&_vZpCDb0QPi=336BfvNk)AYSj=g76av&2)Mg}XbzX(O_-THwz5e4%gXQ-V z)SKTMsw3F*c;Gh5+)-WLuzhkx9(C1+vPach)NMN+aFKnp9=Az;*5h_cf1zA17xo=} z_S>yVzde$E8(&brt@o8l^Q#;F0-UwqZg}MIuB+nkfH-}&+-;lm-N}7-tQF(xyHDFX zv~2wZGW5J;wVGg8EKsSllYcVx-Dce1K-u!e{Jo1dn4Hw9Nu9C{e*qq>PQ@afy--c+ zRC1k~L7h5v(eJ%)7=8k*fA90CdCYSf@5}Cwcc%*0Fu#8uqxARRe*52X{%$-Do&3J7 zlYgy+y0~cF_z0|M{doE250`KLxt3*p3#fa2fHzhAVb%%2L3C5nM{V5{DdQ~T`vF&5 z^!)3w0{%DJC*B@&iQ{8cS1@D=K5S7JU)u@XYnb_1c+~f4e`6f)^)sk@9e}|H ztLpq800030|Lj-KZ`?!_9&ge#EwluRP^%&_3RF!{)FfP})I*#slvE-lV$+CIms#&@ zJjC%B&p7SU>ihC`%?{{a35j&O%V{{t@E`8Ch>#LldRH_j$osXJP0_Otz--+c4t zeKS`PLU+|Qp{_}Fe~qK0T}ZK&{(LCNdPs?+fA<}2up9&)a|pQjh)9Aq zNzNGc$Ma=$oM*s}OG4sE8j8%%0{VZR$8vsZ3R#vF+wG5Kc2gz%!u#%6wfTT??h@&d zR>-+bo(meNfk-tI8vBy@RHqN>M7{&$HC^Jj71QwjjS{}zG57*JpPL9VzqY*j>GDeR z^A8pl7h88oe-QNWSLD|^=GQ{=1-$oTXQy_!b}}Ay9?ax;Q7gC`qzTwzIu#%Da7P5h zN%-0?iO)6o0xUYW7VH^!z4aA!&f5ewcIoPUe)@!`Y*J1ZF9X8*^e9cSX~F-Lz}lh z`{QTzd(U<|(B85Se)~@S{$8q*_gZJl?8cATjU%eE3hWL~Sf_$fof=4;nmMOBbyyM~ zGx!30f0{a_rMj;|m40_ruZru{MU>U6e0=@n%as&E?-3!lh{xLLM#+q?i0?NHo&fjG zcky@!;m_>t68~qvZ@wPa{nd)~?;F&+Z$9n5c$0Havf${o^V}B?LqDJY0`0G*L@#gI zzsC4|`=@L7e!ORso9J`WFb-H*<-FZa2Y z7<>WlcfTme9{nUFfm>fW*?~6w-e{j#yie3)AC@ks@7ceDEFWMyfp_h-_?-JsU%FgW ze}5V(UvQi-8DC|3Gimq%?6}m~Y4Imu|E$FRkzxNx$01#cS4fwN&5%-oC6Dh2CD<6& z5}r=7ovCcU)8=woX?GI)7JG_2JXFPO4o87XY}y8Wr^IowkTgifm5*x>x?aZjE5&rC^Ly4hzhL1w-zP^pKhpV;f6n)Or)BU3xL=(QRKCiFBvMop`jO5T>%1QG zuyy{2`22mtPTaRSb-uR;4U`!c_0uhY|UzM>)M^nU;V0RR8&Sx;{hM-(3?A%sF`P*p)Cf21y= zfP(@F99l#U*~Ex~)DVQ-T&?kXoE@_4jAmzn@Co38I06#=4t#?iIF@@ZoO(t52yN$i zXYBQOqxC;Q4m;9h^YhMo`}Q|)-tSpPdQ_#L;SNGwycduzL^Z|&NqJ}4I_ClPu%s-! zg(XHcoHAD9q?)dpvue(%Iio*6f1+_o&2RJ-#)8J4j|KK55mDFA>*=nag$zHXlpk2{ zr`Av9tUsb6ThJKdeS~Fz8-g*J+>Z!X0ZAs1!J(uf3Cw%8;?M&U>OKyeiZOWp{hasy z(Ry#d%PB&m<<0flZ|fWN2iI4wU29y$oVWe1}NT} z3qo>_>qKf%lTuHZ*o`>$624yN#78W?07tD0D{k4}YCI&KTq2Lvwlq~9E^gB(#=+g{ z`euD6rsYdbfuGbwOzyLYM3s$t1qVR`d%KsG`fQh!0bGsO#YHq&9v`#f1B^o!?mc3$ zPUGBtFeB~9*roY7WvweIe@~m0tt#LjP+vaQyG(ce-AVbIx8j(!_7(7%vOfAMbrl$*dC?&jwY_db94cWsb; z`_|$c@GX@u4kV1z&X~UU%knd4`2pwtVU)K4HsmDSoSdj8FN}j@4DgD&W)48Lg z=?d)E?;(|@7Qird$l%*5+A4k4kzBMa(r*bP2h0)e8S=za8>1tqZ7vPHLgDA)q}H^{aF)-iXjf9 z(@Mf|By->hb>jNcIz5ki;{@Duz9#Gkp~I%r7u)$-E_Axk=|ZRbp3|`S0z8aP|0*!| zN0aG7r%|>}8;$$gI{p3X`|noY|Gk#L;dJ^q>g5}7TOX5zf9dcwu1@nD`&G++)x;rv z;%wkO64eAD4weqVp6^7G@Q!9XG)%(Xrq7%f6RLi7;{MV)p4IoH&)=l|9CXdwucMZK zz%k_*%KCUZUT){}{*ZOk=nv$Ht>dtO#JP4?W38&95oPxshQBi3s5^&x}S)sKI{FyDJ_x^LgW zSkrYi$?rdZKA0a{KY$-oXMaKIZUE_y2?iI86CtVk4&wPVXbJ)e8gb%LoU~epqffuw ze*cA{PcQoPqEGL8PRrs8@SuIVN~dN;Q7Q$dp&Y(fe=qv<>^|LS)7R?L6a6T;f56XO zFV4Bi-SL0#`X2xQ|Nrb*&u`N(6m~io%7GK&4EQB+<9S7COUjUlwa9&Q-q)A!J-^S* zB4$mqe@eZ8GfX^=P9lVEh-+M26XH6AqH#F34P}Ng729^(vDX{)Iru+L@xQG3gI#Vm z%{IhhcVjVIl$pHkm@UErZu&HEP#E7U1;5Y(-%#)miwGuT8)g>={1J9Za|N%#I^bC; z@lI*+!UeG=v{frv;>fuAjqj+23wDPHA~~b3e}bdtbv%W)qw)V?l#E#m{3nhK#=Cr8 z$k(bzcz}{o#je}H_PVt1h-ho%g(K}{8W2vXC+!+%#9K@JYkbRf5)-XZ>T=?DZ!7!0 zOKc1fw=u^kPIq_o!|O4PSFml7Fl=*&1qsgfSb{T^!VBEr9vhBFh<5{X7dIHCJX#MN ze_sT|DMQlsIPn~rcGT*4Hjq$nW3MR~gYREXdF~6^bAvsv5E{%cl~?YSEA@w0OA8B) zIqdr(ZYc%V*n?}J`IwJE{jAu}EDXk@%x6*I1tGrW$A#1)vZOXCTMv9}M|^Fk#Aj=K zfgQ9im8@d>LF1`o^I7MaNJ~@H;mjive{`^Ww^m-NuXRXq+NWaMFEc_}P_5MEu9%+P z=Q%5Ghgo9I%!J=JY=6KW^|m%>NAAhAJjkKqNI#XI)7pK8Kw7FksDXVB?W@On!gPYS zsNhX%ag1wu1^aCD@l!!rJE_dW^u5KOFfLm9e(B|ZNb@(vwHgk?+WGKO{GGR=f7Kh- z+i?82)lc3|Xncdc4E1701ta2j?|^utBeVh$2*J2itAa6&c;YENbZoq0^IS zZ=7K7*4^jui4Q^$wXmv`vz>$J#mM_sQn2Xp6+Edg^Kk4CQJ36G=FajfwL zcE38_z}#-%6`N-$70-42cRGFwfBhRe{{715Z&yB_v7*(%bi7ypsX)D$P%sH!f2!k8 zQvByMf3R;;6PvA$>zdT(WP`j2JA?Mqn{U$a=XbEbzS8ng5T9iJ^8WgN_t%ET7uZAf zS9wMlpuy(-wReA&d*&d{>&G>oz*h2%vc}QpG~7|2*SAqA)TLtnUajYOe|&d9z3_9r z$V4xSe@QQPQ{p2UUtssE7uemzTfsxe6-mQPY=Hb+-o?v8H%_C2J(s_E4a2ZL!t1r2 z3G4bb7&on@ukY`FeIK36)#vP$w=){wV3)(XF_prZ?D971_iVlTLmUrF;xRFg-nRwZ z@%Pn77P>tQZ$0)K8Wo;Ce_JW|qu23;u#UeR%kI5fMf9mPMmt`!N?oBsS?l_s@7Y9u zB@G23OPx>qJvpY}6oh3MMt(lX=zQ=C00960>{!i@+e8$fWVhQbWl_pPRU{-LzBXH} z*eoD{;1F*ERV%S<)od#+G)$aHc3Q7Jvd0U{U%;^v7vR)$#ewoSe;`h9>4^&xA15S) z0QHJso;_1frUOnKt6hmaWi)=V-_Or)-u&LP7Z5^=V$7pdj!3V?I7>z1I z%C4GYcSImRB=v?EgV=XD4Sa<70VC-(1M116_k-|S#IQ?}``kd;eQf^G5&u#>8a&r@ z8oS%QtHkANWS{uFe~&p{y-cGRd*?fwTiszy8!I6R!afPl`<#UP*n6{2dAE%{uZP`h zD{J@IuL&cV74qt;yf66aGVbC1dyfAC(_@D?ob(eaMS|Ihsm z@{9xa7jo?94g0znH$hG}oj<>N`&aR6CHl>i&ZR{DYURZn@Ty3UcNJ|k^ViArrzSKa zk}JvQxy-I+vD4!K*R#3F_E}Bpju?BGd$<<{0Z-3Igy9hLAVf;{IrWJrRWmP;8F$-r zK7~f}5%l#lw>Ed)+C0;}_+oQoqxS-4Y^?a1yt9tRi~C{ctE)!5K%a`Q zLV`It>-rNA){L+0e6@@+zS8|0CtsGN4zq_Nei?hTzk`Q1|7`JIMx${8-Ll@+=Pd&- zZ(q%g;LV|6Fi7l_y_%gI99v$+)D3(d2A+mfq+TSDe?NYi)QfkH7URtuVqEemS2+%H zM)U*2Fo^wrJMaX1o)zA9=!Yu((1jqPDLhnSF;JctpdyyJw}Y(d8RryCRu3 zcSZ03?P&<(#98rI8kAIbcLn{sV)3I4N6#Z$ob`2wIK#gA<@t@p&!7CQWzG+(&ZqUf z;yk-_f4jbP`;M&hr)8c0qcu*vYG^cGFfOM1rB3CBDN^@2`CwM}XLY~Ky1ye*YF77A zd3FCWbg1h-the=f%fKtE?yFhne;eLW!#~ht$}rH;RCWKSwC;a5;eJ=V$maH zl#kSLvre0JyzNoOv`)b66UwaP)7ITQKOQ#xe*hgGvcAm{y0`gZ!&x+NK$q+P64-x) z%H4m{`crg&6zOBW{RU*-dF|Hn>$g5`JD+_D(sFt$`{Gwm*6H`B|2;kmQ;*MaslS5s zDu!gG2Vr5rNXRLPL}F2|9}-B=h!L0Kv_&`+ef`hr{%#yEw!gnX`F*|m_NUHI+vkg1 ze~hQ;`^)49J&*JH{Ho9@l}ciFeZs!=y;EMed^;V>r)@Zdv+>6w7yP0r`MeHJ^N)R{lDs%??G99W_fJ#MUna}XczlKj}&~WKDLh6OydIY3ONZh!UzoGvDmvTTt;zv0YBqRhEe_EKa zXX4q3!OpIe6tN?XW}of%{N|hYz4yGs0Dv=eYIKhBm5p&Ohqi@X;$w$jf052*I!CzP z2){1d(Ih*g{CZ^a`!5Zqzjrp#FJHC^*=j={S_Jt)VrK#TJI+Irof4HaO^fvI4kdcy8Thj2Zw~c# z9&Dc-NE~ST7?b#ZfIPYpLQg`-F0oxC!Ur@%*J9w++t6)Lq}ltoGVppuf5D5%^U^^C z^BXJM@2srW-(6g|bjf@Tdfp-Y((Gzob~P}cf_ol4gX>L{j)tR%gBb}I0LZ?_1F&g} zP~64-PT)Z+#_Mi|e>WApm>e{x7L1wpHS>LBkvVjW9!rD9@9Ygb2%&Sewz5%gh4##} z8+NL&^LE1~bwQ=+xkGAmfAm?l3WKw=dE!1Tzd#qe`yCt#PG{jShHOXZeJtsnQ=ZeP zrH$&f8j~-8LvdJ75Kio^OZLVTKSq@EWb#D(^P7@%?`6LBnm5<~V&kHr@+3{atmx}> z{`LzU?y~X5`t7Cl+a=?RXuM=xkH(Wqo_H?-8SfHUT*$|ha+h8ie;^uF(K#J4KeY$> zsgV4npHP01d6?!ORPbVQFZoH7=3EJizi*JI((}{=Nb;0iU-HDs7?;`Vz<}I5E`vAu)GPO|SkjXJsXIvd|r zzP@ws;hlTH`1BJtf8I2Hez5uY!RDV8m1k-DJE!cA$(v!vM81xjx&=GPhk5fyTKyzw zFRGvKW#~;ReoUtMp%bi|NN*vRN(40|zdi!R@?SpBG=GN`f0@kox>%4t0M8iTe~kG) z{#d(q#$r^)|8fB66I4^0IxCg<%BgErp1+JP+;e^ghG_5~ZiZ_pp6_lFli zf8ViIpY=Je$C@#7<^K6A_rI#dcbAQg$E-jihk=M}t}T%+-##{qMIS600_qu7b| z*GATkiTCo<=P5;hO88+%e$Wc-kfBu5vFOy}S6>aCA)-3zS zUbi2`b^F^Rf96S1Z#+K@YY6Oxn$zMg;QP(WH$Iic>V*WiG@Gm!RpZCeXCz1XimC&5NWlsNIiT*x* vb*YPgS^NRY{=|O%E4&*6-TpIq7t%0sTL+9O(hf*j2LA>J@Tm#2HzvCQ-DquM delta 9807 zcmV-VCa~G$RPs-dA1zQrS4mDbG%O%Pa%Ew3Wn>_4ZaN@Tb!8w*b!;FqG$1fJIx;po zF*hJGFflTbQ6rIH1%Edt0000000001Nlgy`006Hg004NL>={jO95vM0q)9*0P@qT^ z5>){qaRE|+RD>!zX_9tVByGqh>4#Pd-Wl(#RquG@@sw;%T#(ue|DcE)oKR1QV-Fzp z08)@RM2JJ6RaHV1RpP)A>=}D^#yd&&BmDsNN#3{bji3GeY=6)Dm=OTL%@_K>pul@Y z%Yhj7D7r7v_KQ|^i;JTtMg{181K@yqenK=}_mLW+3O$F!gwLpt7{@38b`=wcAy04v zAK4g^sKwZ?z zFN>df5Ay&dV1Jwv%32sT=rF4K)}Ak0Tece8X-9@2ap`9x}(KeqAu8kr$qZ ziD6Un5hJXX5u3wYSsu(lVfh&vV2-I5F%thHm_-3zPL481DM`%|YqWp^T*d_RI+za- zn}8fPdW`u#!8VlR#{4Q!EXNxi+k4lu zys7YmCk$^IdafJmxM!?Wue(=xYX7ueo!Vd9LEA;!N!v}^QQKA9S=+r=cxwL`KQ8+} zK9GJxx_|WT{Mq{;?aw4V{$5Es(V)bzbZt(lOqpA9c>rsr8sr3)OK1t!e1d|KaJ(TT zh2q%sq8s8jMaUD+JG#TX{i%8T0j*TplFOy!azMrk?Z^$JGYa= zTkj0mK2AvLE&cj2ph2J80V~GG6h2~gJMKGg3xD|QlJ)($jqlEFyl(yYgMg`u_4(@c z8>`pzRxj(^&G

ey<@fX&Vw!hV~Ux?<{+O1lIAjlsh!kS=@b4(JzX2cD^t%dES?S zX8;Hc_yQ!@X-1#X{T%FUe0?6j5lzIL_$peg#!oqPu~(t0FBQ=_!;T7_86pSr=YJPL^E}B#FCm=~`C(8khg>epr+(>bGaxhmNI9k$w z?JBablgoc z_IuFOSNp*Ks~iCB#Yra(yGpu;IH}g|ybmir+vyw)8S_dyet#a{75SCEt?PRGjD6b8 zI$o#uPCAO7>UX!|`rRjcI`do4EBbGcQNPP5l!;RzyeosL2Wi$c72!Zj=}#5^&VMrp zHTh?#$BXI0+Z3e%DtH0n4c5ih!7jD3F1NBW3UDn&IH1qIej1*ArLFk7<$Q*mw)5Ug>X8#vpSXWgZgm^Ep$z@M z0$afYBVWF6AOA>rwd*|UrBDC-@Y|zrSm7_$ zmk0Tbw1_QTMnlhOzJKa7{0x}D zflpvP^<8Mvago_~l8Ib3@>U7ig@VC}bs3W>slhWpJY{&Zh<5>Dx7u`PAWs=y7JNv? z2zEV@2y!rmJyhiN)Ets`0Xw{!;7vm&((m+oGn_P-r5u{PB4>@vS%duE#(ai&!YePx zV`e9{6Sj{s*9rQucgEI(j(;-*Icm}pCXmS74c_T6sE^rcZCXC3Ngg8p9QCpN0jf9p zVLpd~gz>b_Ec7`pH|$ju^F~wTg{?_!?;>L2YT#wq?3VdWuH{P}$c_^tZAtoUT%5wS zf@hO3ab_-M@GfWY#xr8;gq;cd6AP^Vew07-AD>gK( zTL%aUdC6;P&9#H$0Nsh>PKYB1u3R~C1il9c_7NuW1+ZJUO{|Qx*~ZwECq>EoCBNtA z=lIXBgb=!`u32?mQGeHEG_V=t7YtS~_`LEMXHZhl4^nuuG2XyuTy!x-sFa~y97=u? z?Uqyca~5Cwb)c_75(gXH&VM!`P9YMfsGhDG-f{EK!gqj45R=ut5aSon?S#D3`a5Uv zfgLPnsaMgoTTkI#vf90=uHAb-KEdx_i$A}ufBbsqaB==or+@qq9cIb{q4c^9En4?U zcy|8t%9HRm^EmHtDS0>j|FrWh9+B_A;HC9*#_Ff`Uxg4tLc5dHm$%!eI-bQCuZTFi zIJ`;gGS&DO#=N4%v*+WC%4os(`rZUVFeQ87Rv6c);8S zYwLAa2aV*rKYwN1YsT02hQnEn1U|*RrtDEbk+}cUIOn{R=N+{Uo%H^uEqb5!oTc_3 zd7Sj@q*x5_COeQf+RHZ$j*;O=P3 zp=H+t``cxK16boOM7gbog$#13Fpq+T#TqbJQ!P(6wto%8>&P)y=Ys4h9McLV9_y;( z8d@a`VvB?_jcpTGb|Tms79L?JX6rm4k!7oI8)kisVofOs^WxlwJ}UYG`Ugg1EfD(j z8_7=9K2&E7R^xBjiZcva6@Vp-eQhtjIuP3{xt zG4w{?5r4S}L8N%YoJHtMvNNhzQ?=V~qONL92lXEA7kK;`+Qe1?lx+ndYFOeZR`GqR zYM>*IUiGu3@qgp#D(N$%kDgz1T!rQM<4Ogpzn*U4hvJAd7Cd#;OfU3^}3akcu7`6yobxV%9!}cVw4JFQzBSOp`?(&@^)Slyu(n(e=XyBT!)LFD z{{R30|Nrb*Pj4JG6!-o~)K+RqDT4NbA|aGS3R0*lQMhC_n})2&CQ&v`p^B7xXS}VYHw>>1C_jCaG%PLe`to-*>hmwA8u z{QT_S7C=Z-@|l#6ji^8zfD7_{LCem_r>wu5(0n2Bg!%>W4hZ$GC-f#q=$+N_kPa42 zzoYbv@~x=-u8zU2_KYwm4{_)$>pBJY->{#!EXSw3I4MyiJqBw6M zWyyCbrB?z1eLj%yVS1B>KgBnh5Ue}s*PN#rZ}E_LpNHP4c}^JfRQzW?^3f2#S@O$T ztrK0(VoI+F82$g~WzEZ2>tBfViiV!KA17o)Z)jeP4a6}d_&#zflrjMYW_0Jh(0}d$ z7|-sjnpfi*Y9s2@Xp8Y~U^Ef$7h~GJsChNkY(|%{=Mva!h#o;8VfVkZ&Zm0EuXk7< zI_~pLQF0s=S2-0|S)D)PDChl`R?76vG72eRu~ngd0`Pl;z%@?32ZSJK1~t zVR7ck4^{UJlFu*n1v?JE)(HKxhQ8wE{@h2ZGaQn)Ecogi$c6((#h(X9aNUMq7F_)G zH{{PNPk(s){qm*yweOxC2^WJ5d5h@Z#>HLiII2yb;^I?We2R-taer~%`hQXC@kP=* zPF(y?=bNI#aItAWx?{!rvJ48vQaSHE$;Yz51!pATKR3R<6XI)Ya=iUErFj<&;_E6T zq$#m<#GAO)#1AHJG_jG1sj}qloRK%h+a1WEEwP;!+j&@u?Mw}^lbpBuzEjGVCl#-3 zas)OxV;XLH$dv(!{(o-&*5tN5qGWd@0^*y}e+~b~c)yO2|Kt_T{oDxH>2CxerOLQGcoLwS7E0BT;C0E`~o| z>_v4yGe4j^FBQ}nS)H2$`Ee4I;IP+aezfO~a-T@%=c4glL0YO;HyVn+0tV`FFV1vq zFWG0$8*xN;>Bzp~FNd!`6+wF43m|`ATm4JXTb8+BhUu3L{po(T&RF}+8Tt3R74p;8 zuTR#0d9wcd0Dr#8f~#w?M=HGsV7$6NuX!~R=!qR!aE@+>S?cE%kY0Di8+CiEcA~hl z#&^=^8#wqXSg-3Z>G}3x%=7Kekn`2yIQDp?37T4CqF&eyY_~<>+`!7iE&jq-2b7Rdm z>o(rXFMm@;oQ;*@`WMB;jeD2p<}YL9*_~@+H`8P{gEmk)JuT3i9E$XY+US8F#{*oD z{DKhQu}I)6u`&PGW7B@*SXlS{wcloa4-|g^c4`;rv_fOG@{s6!mTZf#R58ah_o?Gz zcBfoi*{r*?Fzs++ExMeG2$e8n6|6s+o^4T^6@SmdIW#ykb2`X}cdhG`vM#{bFpZrp z({+OD?ff5#Y$w!vF6o_7p34YJE2Y&k;LlJ?9&Sb+j_lo*>`f@^7**m4c+&s*%aC+0 z{0;VQNcKi!{3F{t6W6Hu1t%+^O}$I+V8LKzIk=`pM@m( zE`Qt22~-w2qUK?ee4hv~oEdibeavWe9oIvDYi-ssj+)~RILcT5dN1v$=XQ4E^4q1l zX>i+QlASbp0J8h}A6~p}jCSVtuwPr^_|l*}tTm9Z$V0p=R$Hb|!!kf?rbGR{=aHn1hu#o0qJMK@JCU9JY-A@FV@KXMJpUk@&QGx30k54r zUfWds1vn|M*{+d>)>7B~ht6wRbP{;&T35W5MY6xM=$>n|kWMx^^HKdJmxV$`xEhEw^kmw$ZL zZQZn6$EV6V054I;Vt6ONYQe6YMDZU}>;MkpKM(rPP;cv{+x2u>_hzlu!q-Xr(9b`C z3~R6V4i5JY^4iy5fh=ghd3|Wki_>4lxBDloS=m1y2lzUYSrY}51&C#v7O^=cjz}!Z zsy2ZHbu6M&?2jZTVP1VXHRkPqcYi!xeSeEu>w8f!cilE!qgpbVn8v!DU(0-_sXvr# zf;m~%Lxq0Mw{qUb`B~1}A%QZPj6WZAI2Shtb8%1R;(VXxVtM|HTVF--7vQ9Gan;6; z?-(Kvw=wH*H1=dLAIHzfAy*vNe0<>X&?W6hprJPftx*FxuYGd}MD=f?`F~i=`>QBf zy{P{$B2~%J^Y_?T53szCU~G&xIuWn?TL`1H2^i zEl;C_OT&|#JmJUjJw1Y&_kRNz)-vBCeS}V0-JSFD=lS5f^sROJKL7v#|Nrb)%}>-o z6rY9VOGQBv1Bo#kl>mlB;3AO&g;j(^gt*9Q6EfQ!*h*=ec9suM9_8i{4=9O$f&L8~ z^^OM;{{Rkp_iJ7|Wrx{@cGm?Y_9c_)Z+G77n>X*hU&nTkE0@_AV1J{Ljb=WBG;o|? zqn^(obqpmE30H5czNFU{pqCKzV0}SYUl+2z0VQ3VfE6}YFx7F+HcjgN?+^>yn3^{7 zu|YM1=p3TOqM=gFG!_cj$m_&?|E#djWWf7x2=8y>IDJ1mYnw|pUP(=8I&reoV{?zk z#F**DzRcbVtKasIVP{sG2@X@6|2m_?rcq37U>XvfzR z6r}RE>xC}3#CYx>`e;xS|vXkrE>+`={@e(3IF-)z}_%AzOF@kE&TV% z-ypXXxn6?wlahXljqNueca@og-6sdTx0S*o$Tj8d-sbk+=Fib;@_rCStC!>vy(A(m zt3Yq3!ailieSfMZeX94A`qWN{f0`68z<<-HY=SA7P>o29ioZARSHbso#eF9rD*6z{82oB)@fckq5|VNafw5c@&-QPfn^pth+CcB<+7AYa=% z-M+t9Vcq)u&tl`5p;13qK_-}Y8SRCiHfYv#G_S5b$$wB=GnPv0Gju~Ig8rCg>6(f` zu&z+G*(x4 zFHw2j`F|hNK0Z%%7~d=Z?|UVd;srSB_nL!O$s-$Eh4gr37fSxV_&Y85JI&wwS@C3( z2bC{CliMl9gTwru`707n2KAo|%a@d!Fr|2f@k_I02e4Wo>T=Fp2mRNmwtN}3uZxmD zfQ33hwYDSAVRlcNKfg7nV*ZTzGv-fjFDJzdaDP<(v{)L`48_!`jzDAn4CYUN&u7h_ z@7;T5m*T<7_%nzfW+7ivZo(YK>%8&jCCMMaVja}ljy#9iJ$e4T8S`h%pD}-Odqj#C z;HdnWxAEGXT_h~ONi`IDTKxG300960>{-oE6G0T8@=-uUOH7PjOpPBD(*qI{2^?4` zL4Qo-BNjQ4G}G<29a(lavr|4id6bLsh$a~S1^x$)dgtQFD>42B>b&j@-A>o^(?FA* zWHSAA_x<|j?VI;|%_8KDh*)D{y~gJupHV&s`5aa5i=aNVoLWr-zRhR8Wa*69R;Gxp zoM~isZMx+;ST}rpOFq64@qNF*QNb% zo(A@7b?o~T`#7Hi9@O3+ogN&Wj%ps9(pJw-R?bd-&jPRJ1q@<_iL^>0Yw1zaM!Z82(_KAu+c3dXO&`{lg)5^XN-F@fLm`N|^9MU|1l zrkTb1mc)zP+cbG2j}_waAaL#^PBTiZLd8DK_D!P#(VXL$M8{yVfEh;O@t%)H;waCc zj3Z#Zh$+^YLEQ?^VQ+;e7Qo|=1Ap9K>}@S>;O{nOCeBjZX5MwzaJWOvrbuFo5zCN| zS*=${tp`E-zN73LaB6yUjoJl@x8sY%G~CVQx%C%w^P4XxCmv5?Z(kF2cq3K35wwX~ zv(toKyr;Q1%IBHz#RVa@?})_76PZGls;=rokbk-oFTig8!GxA5u4Z2wI)6(VJN&Wa zcpi>#5VwTQ=jplSOrb;)V=mq?7D=H*;k0MuDHu!ESSI=Huu9JH@$&DrS`V;gTl?E~ zNuI%a&q0oE(a&LYIrQ@<$mL3(=E3Il^QsbWz-gW@agi`GUfrgjBdFRRz*XmSw5*p?(U!tP(H4&41|UGm4#JYw{WDCkob2sXZ61pEqZYZ_FJ3nDtW2nJDMgX_NI!>VSi4^;6Z$ zb;7FjP}M(GA5$iA9G{}9dfQ38Z1+6Y^CKuP2(P&DRR^}zR^KU}|9>RdzeAlpH|_dU zPe*&J?0b=&&)4(=jn=nsUt!4q1=HH6k1%FMee?2r@5#U8qtVMTA^Rhe5bX?seO|9rY z7$#qK-wkX_Jn7%?NK<*F-?rj+QhP5Zm=<$O1;A%TTdUh9r;c5$V!LdS;sb|m zxHW7Nh3~e)H>!J*K7#Skj4YptP*phUTu$b7xytAH$_{vDVKW}OjZtw=f!9X^pV_FB5zy@`%dBlXVk8={R_Nd zkv2qPO@Heu&M|^HnKvatrdG44iGlK_s7lCR{$1-N&Le1VoB^9v%&^G`bK5-I18ttM z03J0?@P2imx46Q1H!-(xi8&7Up1Y(b2E-kPB(^!ViA)2%o*}(n1kL-rHgCYW>#KR@ zR2be&&QpuHtBaZYcQV=4do$_j85~}p^?8knPk*bZH$TJoOUAp4qg>CF5*LJcqvj{l z8kH&5uKK!u1dR`C@dDhbzLqdIs|#YY^rWtlzWz#IPoTd;U%yO!_%!ul-0+@ur>{Zv zCky$KgoJ6wYrp!cu6tN~;^LExYgG6C@48pg;sv;;x+k@}i^3Pxz2Leh_sn6On~!R7 z0)MQmGwL^w-iPrX^tt(ABHhx9*Mq&)xyR_>&&_}GT-@wZA8L_4^hF<1e@GviA>*T3 zya0Eq57>Hu8}2<~iABRqBz$x}-sS6uPMk!Cdp>^t3@m26gX^akL&mkMU^k4Voi}%P z-gxI@b)G@%Hm=1RaHf?vNhNVccjLC-bAPhBj{VTnUgj6DFVI2p>AexcfAIP`-O9IS zAMd5IqMlTkQ@6`G#}aDMZ@oRtd#SMhbpr}dT%cGi7U_1R+xyaOMjTcqY{3?(xP`4- zWy&MnM*Y+6Q|NGYyLEkaIcP$2c?IWih98g0d64_xwJfkN9T_YRC8=+TaowdnK#5$!_U*Rsg>JLvIi8Q^ z#cJ#H+c=HTvKLUuvskZlpStbDzRoutqTtnhf;pMDV}m)y9BN~L_!4uBWcXi8KYl%? z`4x0~9sAf8K?@QbHU4RSEq`c!g?#_>(=%f~pMF`@pLyf7@DIpG-8e00*cUbXvKZI9 zVpt!KKP!(vdsY3ZE1y}>{9o06{8d7|{(^BW%JVXni4Hh3892oroFd}nE(g2qLSk3S z@C*E&74qdP?6ZTpgB!l*MaL2ago(g&k*Trna_W-!c2XNg=YI#9zd)~DSY7tL zwvRjIdFqg0b#ZRxtGP?7*JfwV%;IQYS^hJ6a1FGI^4VF^u3`@6$N1b-6u%(EH<>)J zEgB!)ggv_P%~;*$`oAit|2og9agXuGHri*V-YiOltLZ zMWoc)>zXk+y#rucSbxDYhMt;=-kY}`W7l&xJ6;&qz^r)0qTmNfq`C8}a70P?~SLauq ze`^k9Okr@H--pS^yXDWUb-bmm19aGAd5OW6g0yv{;f!fGpnuEwp9TBx(QwbvZr78= zAB7(K0`gj|9X!}?Kl0nJAVurqso^2cjSUB|6`g#9aLS zZu@hQi{U}(*MHSK&g$1qp&5qJwc8r7|GYWsKZnwPp4y}Sb1P$gUu*sXz2E*5CIcdR zPI<2SZ8=B%W_rK*E=u~%{kShj^1S-=>We2=UzDuqzI@J#=1oh#&m{So|qrLCrM}7NcG|A&N3%Q2hbND6F&@!`#FGba{15yGUhrUI;>{2fFM9E7 zUU!$>4y)TQRFb}QGX16Vetq-i{bpXl(j+SDT7R~LwY);m6@<_#8&NiT*yuvGbvro1 z##Ltq8*p%PHnVEzMFXosN>+%OSxT(kOU!5PPfgyO!l=SK8NwT>;*FpTYRpe`{DcnA zuOq~-n=$t}IL1c7$qPbsw@mCc1-3E!+qQ0Onq@56`PvVO4|DkfY!tggMVRGut_(tBInm|vDSR|q3S#V_RiVcXRCPl zIgB@`9xPeXHEQUJk7vfm6Aj5*FY@Z-DJqKK|GrLr4mh~X#=0g`w>H62EV{J($So7J zs++Q{U)$2ukhP+>K23KO;`2|G%Ze;vaJj>Dgq-6%wMNfFDq#allo)1nd0Y4iHh(Je z_j8bc7w0dqF?|c{L;UUD!SUV!5??$AyDfe?T0S{i{ubwT6|}!WZhwH6Ej33BK`#gr zR*8XYy)XDU2N>(3gw=Zq%bOZ$C{h_SiD zp}E61ar@U9apeRa%pdVu&g~MHZ@_6*FM?e#r}+wdUwSwmz*31wn>l^wUw`zkm$`TV z`*qN2p5M)3{OA6TitRqO-n@NmQkRa26)+_R}Dy8&KB^hy8# diff --git a/matlab/gtsam_examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m index bc08e7abf..4ccde5e53 100644 --- a/matlab/gtsam_examples/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -82,7 +82,7 @@ options.showImages = get(handles.showImagesCB,'Value'); options.hardConstraint = get(handles.hardConstraintCB,'Value'); options.pointPriors = get(handles.pointPriorsCB,'Value'); options.batchInitialization = get(handles.batchInitCB,'Value'); -options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String')); +%options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String')); options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value'); % Display Options From 9ac2ae575528b48e87bb509c1aed27184a07dd18 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 4 Sep 2012 04:58:38 +0000 Subject: [PATCH 37/53] make print more comprehensible --- gtsam/linear/IterativeSolver.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 2f76df2b7..4323d8e55 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -49,10 +49,9 @@ namespace gtsam { void setVerbosity(const std::string &s) ; void print() const { - const std::string kernelStr[1] = {"cg"}; std::cout << "IterativeOptimizationParameters: " - << "kernel = " << kernelStr[kernel_] - << ", verbosity = " << verbosity_ << std::endl; + << "kernel = " << kernelTranslator(kernel_) + << ", verbosity = " << verbosityTranslator(verbosity_) << std::endl; } static Kernel kernelTranslator(const std::string &s); From 0d5cb9a7e042dd30b8ba8c0e84c2cda7643ff254 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 4 Sep 2012 05:13:35 +0000 Subject: [PATCH 38/53] fix figure update bug in VisualISAMPlot when run with "Save Figures" enabled. --- matlab/+gtsam/VisualISAMPlot.m | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m index 5ef64cf62..cd7ab188e 100644 --- a/matlab/+gtsam/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -39,11 +39,12 @@ drawnow %% do various optional things if options.saveFigures - fig2 = figure('visible','off'); - newax = copyobj(h,fig2); - colormap(fig2,'hot'); + figToSave = figure('visible','off'); + newax = copyobj(h,figToSave); + colormap(figToSave,'hot'); set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]); - print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M)); + print(figToSave,'-dpng',sprintf('VisualiSAM%03d.png',M)); + axes(h); end if options.saveDotFiles From 8e4f6051220883f0b00209691b8c5a942d81848d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Sep 2012 15:05:47 +0000 Subject: [PATCH 39/53] Made HessianFactor::updateATA public to allow low-rank updates --- gtsam/linear/HessianFactor.cpp | 12 ++++-------- gtsam/linear/HessianFactor.h | 28 ++++++++++++++-------------- 2 files changed, 18 insertions(+), 22 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6db6330b0..a44f5c682 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph& factors, if (debug) cout << "Combining " << factors.size() << " factors" << endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (hessian) - updateATA(*hessian, scatter); - else { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if (jacobianFactor) + if(factor) { + if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) + updateATA(*hessian, scatter); + else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) updateATA(*jacobianFactor, scatter); else throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); @@ -360,7 +358,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { @@ -437,7 +434,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 74216f0f0..2e9b67c2a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -120,20 +120,6 @@ namespace gtsam { InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] BlockInfo info_; ///< The block view of the full information matrix. - /** Update the factor by adding the information from the JacobianFactor - * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); - public: typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this @@ -315,6 +301,20 @@ namespace gtsam { /** split partially eliminated factor */ boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + /** assert invariants */ void assertInvariants() const; From abd07e553e5d36aeb15744b8eb67d8038300cca0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Sep 2012 15:05:55 +0000 Subject: [PATCH 40/53] Made new global functions in unit tests static to avoid duplicate symbols --- tests/testSubgraphPreconditioner.cpp | 2 +- tests/testSubgraphSolver.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index b0a008478..c41b95881 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -54,7 +54,7 @@ TEST( SubgraphPreconditioner, planarOrdering ) { /* ************************************************************************* */ /** unnormalized error */ -double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const JacobianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) total_error += factor->error(x); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 04e0554e5..91b44a815 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -38,7 +38,7 @@ using namespace example; /* ************************************************************************* */ /** unnormalized error */ -double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const JacobianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) total_error += factor->error(x); From 73f8c0830b40330873c94a728a4bb73123f633ad Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Sep 2012 15:05:57 +0000 Subject: [PATCH 41/53] Created new dense matrix functions in GaussianFactorGraph returning pair for easier access, and renamed functions to augmentedJacobian, augmentedHessian, jacobian, hessian --- gtsam.h | 6 ++- gtsam/linear/GaussianFactorGraph.cpp | 24 +++++++++--- gtsam/linear/GaussianFactorGraph.h | 38 ++++++++++++++++--- .../linear/tests/testGaussianFactorGraph.cpp | 21 ++++++++-- gtsam/nonlinear/Marginals.cpp | 2 +- tests/testDoglegOptimizer.cpp | 6 +-- 6 files changed, 78 insertions(+), 19 deletions(-) diff --git a/gtsam.h b/gtsam.h index c5560668f..9c6f76527 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1022,8 +1022,10 @@ class GaussianFactorGraph { // Conversion to matrices Matrix sparseJacobian_() const; - Matrix denseJacobian() const; - Matrix denseHessian() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + Matrix augmentedHessian() const; + pair hessian() const; }; class GaussianISAM { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 02f37915c..144f2c0d9 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -166,7 +166,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix GaussianFactorGraph::denseJacobian() const { + Matrix GaussianFactorGraph::augmentedJacobian() const { // Convert to Jacobians FactorGraph jfg; jfg.reserve(this->size()); @@ -182,6 +182,14 @@ namespace gtsam { return combined.matrix_augmented(); } + /* ************************************************************************* */ + std::pair GaussianFactorGraph::jacobian() const { + Matrix augmented = augmentedJacobian(); + return make_pair( + augmented.leftCols(augmented.cols()-1), + augmented.col(augmented.cols()-1)); + } + /* ************************************************************************* */ // Helper functions for Combine static boost::tuple, size_t, size_t> countDims(const FactorGraph& factors, const VariableSlots& variableSlots) { @@ -317,9 +325,7 @@ break; } /* ************************************************************************* */ - static - FastMap findScatterAndDims - (const FactorGraph& factors) { + static FastMap findScatterAndDims(const FactorGraph& factors) { const bool debug = ISDEBUG("findScatterAndDims"); @@ -349,7 +355,7 @@ break; } /* ************************************************************************* */ - Matrix GaussianFactorGraph::denseHessian() const { + Matrix GaussianFactorGraph::augmentedHessian() const { Scatter scatter = findScatterAndDims(*this); @@ -367,6 +373,14 @@ break; return result; } + /* ************************************************************************* */ + std::pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); + return make_pair( + augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), + augmented.col(augmented.rows()-1).head(augmented.rows()-1)); + } + /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 51748b79b..e7563b0c5 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -188,15 +188,43 @@ namespace gtsam { Matrix sparseJacobian_() const; /** - * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b - * with standard deviations are baked into A and b + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - Matrix denseJacobian() const; + Matrix augmentedJacobian() const; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + std::pair jacobian() const; /** - * Return a dense \f$ n \times n \f$ Hessian matrix, augmented with \f$ A^T b \f$ + * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian + * matrix, augmented with the information vector \f$ \eta \f$. The + * augmented Hessian is + \f[ \left[ \begin{array}{ccc} + \Lambda & \eta \\ + \eta^T & c + \end{array} \right] \f] + and the negative log-likelihood is + \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - Matrix denseHessian() const; + Matrix augmentedHessian() const; + + /** + * Return the dense Hessian \f$ \Lambda \f$ and information vector + * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood + * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also + * GaussianFactorGraph::augmentedHessian. + */ + std::pair hessian() const; private: /** Serialization function */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 4c0aa3871..cb835a76a 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -410,7 +410,7 @@ TEST(GaussianFactor, eliminateFrontals) factors.push_back(factor4); // extract the dense matrix for the graph - Matrix actualDense = factors.denseJacobian(); + Matrix actualDense = factors.augmentedJacobian(); EXPECT(assert_equal(2.0 * Ab, actualDense)); // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians @@ -619,7 +619,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { } /* ************************************************************************* */ -TEST(GaussianFactorGraph, denseHessian) { +TEST(GaussianFactorGraph, matrices) { // Create factor graph: // x1 x2 x3 x4 x5 b // 1 2 3 0 0 4 @@ -639,9 +639,24 @@ TEST(GaussianFactorGraph, denseHessian) { 9,10, 0,11,12,13, 0, 0, 0,14,15,16; + Matrix expectedJacobian = jacobian; Matrix expectedHessian = jacobian.transpose() * jacobian; - Matrix actualHessian = gfg.denseHessian(); + Matrix expectedA = jacobian.leftCols(jacobian.cols()-1); + Vector expectedb = jacobian.col(jacobian.cols()-1); + Matrix expectedL = expectedA.transpose() * expectedA; + Vector expectedeta = expectedA.transpose() * expectedb; + + Matrix actualJacobian = gfg.augmentedJacobian(); + Matrix actualHessian = gfg.augmentedHessian(); + Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); + Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + + EXPECT(assert_equal(expectedJacobian, actualJacobian)); EXPECT(assert_equal(expectedHessian, actualHessian)); + EXPECT(assert_equal(expectedA, actualA)); + EXPECT(assert_equal(expectedb, actualb)); + EXPECT(assert_equal(expectedL, actualL)); + EXPECT(assert_equal(expectedeta, actualeta)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 52c507374..c3078ea76 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -152,7 +152,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab } // Get information matrix - Matrix augmentedInfo = jointFG.denseHessian(); + Matrix augmentedInfo = jointFG.augmentedHessian(); Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); return JointMarginal(info, dims, variableConversion); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 781685a23..6b03b572b 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -96,7 +96,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { gradientValues.vector() = gradient; // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(gbn).denseHessian(); + Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(gbn); denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); @@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { GaussianFactorGraph expected(gbn); GaussianFactorGraph actual(bt); - EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian())); + EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian())); } /* ************************************************************************* */ @@ -276,7 +276,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { gradientValues.vector() = gradient; // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(bt).denseHessian(); + Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(bt); denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); From f1bc66c0174b7200426df183426e39bd500f552f Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 4 Sep 2012 15:33:36 +0000 Subject: [PATCH 42/53] add back an accidentally deleted function --- gtsam/linear/IterativeSolver.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 4323d8e55..7946874bb 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -68,6 +68,8 @@ namespace gtsam { /* interface to the nonlinear optimizer */ virtual VectorValues optimize () = 0; + /* update interface to the nonlinear optimizer */ + virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} }; } From a553d2f84540b7a943a8896b145e2481bf4830ce Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 4 Sep 2012 19:10:03 +0000 Subject: [PATCH 43/53] fix point prior bug in VisualISAM matlab example. --- matlab/+gtsam/VisualISAMInitialize.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m index 1e408d3d9..b29288fdf 100644 --- a/matlab/+gtsam/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -50,7 +50,7 @@ for i=1:2 initialEstimates.insert(jj, truth.points{j}); end if options.pointPriors % add point priors - newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point)); + newFactors.add(PriorFactorPoint3(jj, truth.points{j}, noiseModels.point)); end end end From c55f4be0d10cfcd91aef01cbc544cb62cb5c8ec7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 4 Sep 2012 22:20:40 +0000 Subject: [PATCH 44/53] Comments added --- tests/testSubgraphSolver.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 91b44a815..84f54a6fa 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -55,9 +55,11 @@ TEST( SubgraphSolver, constructor1 ) size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + // The first constructor just takes a factor graph (and parameters) + // and it will split the graph into A1 and A2, where A1 is a spanning tree SubgraphSolverParameters parameters; SubgraphSolver solver(Ab, parameters); - VectorValues optimized = solver.optimize(); + VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -74,6 +76,8 @@ TEST( SubgraphSolver, constructor2 ) JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // The second constructor takes two factor graphs, + // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) SubgraphSolverParameters parameters; SubgraphSolver solver(Ab1_, Ab2_, parameters); VectorValues optimized = solver.optimize(); @@ -93,8 +97,12 @@ TEST( SubgraphSolver, constructor3 ) JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT + GaussianBayesNet::shared_ptr Rc1 = // + EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The third constructor allows the caller to pass an already solved preconditioner Rc1_ + // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before SubgraphSolverParameters parameters; SubgraphSolver solver(Rc1, Ab2_, parameters); VectorValues optimized = solver.optimize(); From b0508cc1a8996a5dbfe5e1e6d71e7316d5b94f83 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Sep 2012 22:42:09 +0000 Subject: [PATCH 45/53] Removed JacobianFactorGraph - moved its linear algebra interface to GaussianFactorGraph and redirected all uses of it to GaussianFactorGraph --- gtsam/linear/GaussianBayesNet.cpp | 6 +- gtsam/linear/GaussianBayesTree.cpp | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 147 ++++++++++++++++++ gtsam/linear/GaussianFactorGraph.h | 66 +++++--- gtsam/linear/JacobianFactorGraph.cpp | 135 ---------------- gtsam/linear/JacobianFactorGraph.h | 75 --------- gtsam/linear/SubgraphPreconditioner.cpp | 2 +- gtsam/linear/SubgraphPreconditioner.h | 3 +- gtsam/linear/SubgraphSolver.cpp | 46 +++--- gtsam/linear/SubgraphSolver.h | 16 +- gtsam/linear/iterative.cpp | 10 +- gtsam/linear/iterative.h | 4 +- .../linear/tests/testJacobianFactorGraph.cpp | 2 +- gtsam/nonlinear/ISAM2.cpp | 2 +- .../NonlinearConjugateGradientOptimizer.cpp | 2 +- tests/smallExample.cpp | 26 ++-- tests/smallExample.h | 15 +- tests/testGaussianFactor.cpp | 6 +- tests/testGaussianFactorGraphB.cpp | 2 +- tests/testGaussianISAM2.cpp | 2 +- tests/testIterative.cpp | 2 +- tests/testSubgraphPreconditioner.cpp | 28 ++-- tests/testSubgraphSolver.cpp | 16 +- 23 files changed, 292 insertions(+), 323 deletions(-) delete mode 100644 gtsam/linear/JacobianFactorGraph.cpp delete mode 100644 gtsam/linear/JacobianFactorGraph.h diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 0ddc4a7b3..5182bb914 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) { /* ************************************************************************* */ VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(FactorGraph(bayesNet), x0); + return gradient(GaussianFactorGraph(bayesNet), x0); } /* ************************************************************************* */ void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(FactorGraph(bayesNet), g); + gradientAtZero(GaussianFactorGraph(bayesNet), g); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index f9ea3fdc8..04cb4a25a 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -18,7 +18,7 @@ */ #include -#include +#include namespace gtsam { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 144f2c0d9..14d558e62 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -521,4 +521,151 @@ break; } // \EliminatePreferCholesky + /* ************************************************************************* */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back((*Ai)*x); + } + return e; + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { + multiplyInPlace(fg,x,e.begin()); + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { + Errors::iterator ei = e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + *ei = (*Ai)*x; + ei++; + } + } + + /* ************************************************************************* */ + // x += alpha*A'*e + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + Ai->transposeMultiplyAdd(alpha,*(ei++),x); + } + } + + /* ************************************************************************* */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::Zero(x0); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back(Ai->error_vector(x0)); + } + transposeMultiplyAdd(fg, 1.0, e, g); + return g; + } + + /* ************************************************************************* */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e.push_back(-Ai->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + + /* ************************************************************************* */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + Index i = 0 ; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + r[i] = Ai->getb(); + i++; + } + VectorValues Ax = VectorValues::SameStructure(r); + multiply(fg,x,Ax); + axpy(-1.0,Ax,r); + } + + /* ************************************************************************* */ + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + r.vector() = Vector::Zero(r.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + SubVector &y = r[i]; + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + y += Ai->getA(j) * x[*j]; + } + ++i; + } + } + + /* ************************************************************************* */ + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { + x.vector() = Vector::Zero(x.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + x[*j] += Ai->getA(j).transpose() * r[i]; + } + ++i; + } + } + + /* ************************************************************************* */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) + Ai = Ai_J; + else + Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + e->push_back(Ai->error_vector(x)); + } + return e; + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e7563b0c5..6c06e34c0 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -31,24 +31,6 @@ namespace gtsam { - /** return A*x-b - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - Errors gaussianErrors(const FactorGraph& fg, const VectorValues& x) { - return *gaussianErrors_(fg, x); - } - - /** shared pointer version - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const typename boost::shared_ptr& factor, fg) { - e->push_back(factor->error_vector(x)); - } - return e; - } - /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor @@ -321,5 +303,53 @@ namespace gtsam { */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals = 1); + + /** return A*x */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); + + /** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around zero. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g); + + /* matrix-vector operations */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x); + + /** shared pointer version + * \todo Make this a member function - affects SubgraphPreconditioner */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x); + + /** return A*x-b + * \todo Make this a member function - affects SubgraphPreconditioner */ + inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { + return *gaussianErrors_(fg, x); } } // namespace gtsam diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp deleted file mode 100644 index 70c57e8df..000000000 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - * @file JacobianFactorGraph.h - * @date Jun 6, 2012 - * @author Yong Dian Jian - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) { - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - e.push_back((*Ai)*x); - } - return e; -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { - Errors::iterator ei = e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - *ei = (*Ai)*x; - ei++; - } -} - - -/* ************************************************************************* */ -// x += alpha*A'*e -void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - Ai->transposeMultiplyAdd(alpha,*(ei++),x); - } -} - -/* ************************************************************************* */ -VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) { - // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x0); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(factor->error_vector(x0)); - } - transposeMultiplyAdd(fg, 1.0, e, g); - return g; -} - -/* ************************************************************************* */ -void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) { - // Zero-out the gradient - g.setZero(); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(-factor->getb()); - } - transposeMultiplyAdd(fg, 1.0, e, g); -} - -/* ************************************************************************* */ -void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - Index i = 0 ; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - r[i] = factor->getb(); - i++; - } - VectorValues Ax = VectorValues::SameStructure(r); - multiply(fg,x,Ax); - axpy(-1.0,Ax,r); -} - -/* ************************************************************************* */ -void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.vector() = Vector::Zero(r.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - SubVector &y = r[i]; - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - y += factor->getA(j) * x[*j]; - } - ++i; - } -} - -/* ************************************************************************* */ -void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.vector() = Vector::Zero(x.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - x[*j] += factor->getA(j).transpose() * r[i]; - } - ++i; - } -} - -/* ************************************************************************* */ -JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) { - JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); - jfg->reserve(gfg.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast(factor)); - if(castedFactor) jfg->push_back(castedFactor); - else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested."); - } - return jfg; -} - -/* ************************************************************************* */ -JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg) { - JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); - jfg->reserve(gfg.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr & factor, gfg) { - if( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor) ) { - jfg->push_back(jf); - } - else { - jfg->push_back(boost::make_shared(*factor)); - } - } - return jfg; -} - -} // namespace - - diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h deleted file mode 100644 index 1164652c3..000000000 --- a/gtsam/linear/JacobianFactorGraph.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 JacobianFactorGraph.cpp - * @date Jun 6, 2012 - * @brief Linear Algebra Operations for a JacobianFactorGraph - * @author Yong Dian Jian - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - typedef FactorGraph JacobianFactorGraph; - - /** return A*x */ - Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x); - - /** In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e); - - /** In-place version e <- A*x that takes an iterator. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around zero. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g); - - /* matrix-vector operations */ - void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x); - - /** dynamic_cast the gaussian factors down to jacobian factors, may throw exception if it contains non-Jacobian Factor */ - JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg); - - /** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */ - JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg); -} diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index b33adbc38..72aa25554 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index b109368af..db7c023ce 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -35,7 +36,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedBayesNet; - typedef boost::shared_ptr > sharedFG; + typedef boost::shared_ptr sharedFG; typedef boost::shared_ptr sharedValues; typedef boost::shared_ptr sharedErrors; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 14a0cc174..c34f7e22e 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -31,12 +31,11 @@ namespace gtsam { SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : parameters_(parameters) { - JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg); - initialize(*jfg); + initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) : parameters_(parameters) { initialize(*jfg); @@ -47,13 +46,12 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFac : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1)->eliminate(&EliminateQR); - JacobianFactorGraph::shared_ptr Ab2Jacobian = dynamicCastFactors(Ab2); - initialize(Rc1, Ab2Jacobian); + initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, - const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); @@ -64,13 +62,12 @@ SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) : parameters_(parameters) { - JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2); - initialize(Rc1, Ab2Jacobians); + initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) { initialize(Rc1, Ab2); } @@ -80,10 +77,10 @@ VectorValues SubgraphSolver::optimize() { return pc_->x(ybar); } -void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { - JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); + GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); boost::tie(Ab1, Ab2) = splitGraph(jfg) ; if (parameters_.verbosity()) @@ -91,28 +88,33 @@ void SubgraphSolver::initialize(const JacobianFactorGraph &jfg) GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); - JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2); - pc_ = boost::make_shared(Ab2Jacobians, Rc1, xbar); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2) +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) { VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); pc_ = boost::make_shared(Ab2, Rc1, xbar); } -boost::tuple -SubgraphSolver::splitGraph(const JacobianFactorGraph &jfg) { +boost::tuple +SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); const size_t n = index.size(), m = jfg.size(); DisjointSet D(n) ; - JacobianFactorGraph::shared_ptr At(new JacobianFactorGraph()); - JacobianFactorGraph::shared_ptr Ac( new JacobianFactorGraph()); + GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); size_t t = 0; - BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { + + JacobianFactor::shared_ptr jf; + if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(gf)) + jf = Ai_J; + else + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) if ( jf->keys().size() > 2 ) { throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index e4d99933b..441fdaeef 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,7 @@ #include #include -#include +#include #include #include @@ -62,26 +62,26 @@ protected: public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); - SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters ¶meters); /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); /* The same as above, but the A1 is solved before */ SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; protected: - void initialize(const JacobianFactorGraph &jfg); - void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2); + void initialize(const GaussianFactorGraph &jfg); + void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2); - boost::tuple - splitGraph(const JacobianFactorGraph &gfg) ; + boost::tuple + splitGraph(const GaussianFactorGraph &gfg) ; public: diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index d0a7fd551..07d4de865 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -61,15 +61,15 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues steepestDescent(const FactorGraph& fg, + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors>( + return conjugateGradients( fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const FactorGraph& fg, + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors>( + return conjugateGradients( fg, x, parameters); } diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 384dfe814..0eb04bf49 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -130,7 +130,7 @@ namespace gtsam { * Method of steepest gradients, Gaussian Factor Graph version */ VectorValues steepestDescent( - const FactorGraph& fg, + const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); @@ -138,7 +138,7 @@ namespace gtsam { * Method of conjugate gradients (CG), Gaussian Factor Graph version */ VectorValues conjugateGradientDescent( - const FactorGraph& fg, + const GaussianFactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters); diff --git a/gtsam/linear/tests/testJacobianFactorGraph.cpp b/gtsam/linear/tests/testJacobianFactorGraph.cpp index e756d83e7..07dda4652 100644 --- a/gtsam/linear/tests/testJacobianFactorGraph.cpp +++ b/gtsam/linear/tests/testJacobianFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testJacobianFactorGraph.cpp - * @brief Unit tests for JacobianFactorGraph + * @brief Unit tests for GaussianFactorGraph * @author Yong Dian Jian **/ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b09133c4d..081910e8c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -27,7 +27,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index f0b3852ef..da14d8982 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include #include diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index f81ab5528..6df106acf 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,9 +138,9 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { + FactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - JacobianFactorGraph fg; + FactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); @@ -273,7 +273,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSimpleConstraintGraph() { + GaussianFactorGraph createSimpleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -293,7 +293,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -310,7 +310,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSingleConstraintGraph() { + GaussianFactorGraph createSingleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -335,7 +335,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -351,7 +351,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createMultiConstraintGraph() { + GaussianFactorGraph createMultiConstraintGraph() { // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); @@ -396,7 +396,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(lf1); fg.push_back(lc1); fg.push_back(lc2); @@ -421,7 +421,7 @@ namespace example { } /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { + boost::tuple planarGraph(size_t N) { // create empty graph NonlinearFactorGraph nlfg; @@ -460,7 +460,7 @@ namespace example { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); - JacobianFactorGraph jfg; + GaussianFactorGraph jfg; BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg) jfg.push_back(boost::dynamic_pointer_cast(factor)); @@ -477,9 +477,9 @@ namespace example { } /* ************************************************************************* */ - pair splitOffPlanarTree(size_t N, - const JacobianFactorGraph& original) { - JacobianFactorGraph T, C; + pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; // Add the x11 constraint to the tree T.push_back(boost::dynamic_pointer_cast(original[0])); diff --git a/tests/smallExample.h b/tests/smallExample.h index 54c3a14c5..9c43563a3 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -23,7 +23,6 @@ #include #include -#include #include namespace gtsam { @@ -66,7 +65,7 @@ namespace gtsam { * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering); + FactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y @@ -100,21 +99,21 @@ namespace gtsam { * Creates a simple constrained graph with one linear factor and * one binary equality constraint that sets x = y */ - JacobianFactorGraph createSimpleConstraintGraph(); + GaussianFactorGraph createSimpleConstraintGraph(); VectorValues createSimpleConstraintValues(); /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ - JacobianFactorGraph createSingleConstraintGraph(); + GaussianFactorGraph createSingleConstraintGraph(); VectorValues createSingleConstraintValues(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ - JacobianFactorGraph createMultiConstraintGraph(); + GaussianFactorGraph createMultiConstraintGraph(); VectorValues createMultiConstraintValues(); /* ******************************************************* */ @@ -130,7 +129,7 @@ namespace gtsam { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ - boost::tuple planarGraph(size_t N); + boost::tuple planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree @@ -147,8 +146,8 @@ namespace gtsam { * | * -x11-x21-x31 */ - std::pair splitOffPlanarTree( - size_t N, const JacobianFactorGraph& original); + std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); } // example } // gtsam diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 3fe92c50d..a41c8d9a7 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -72,7 +72,7 @@ TEST( GaussianFactor, getDim ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // get a factor Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); GaussianFactor::shared_ptr factor = fg[0]; // get the size of a variable @@ -89,7 +89,7 @@ TEST( GaussianFactor, error ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); // get the first factor from the factor graph GaussianFactor::shared_ptr lf = fg[0]; @@ -210,7 +210,7 @@ TEST( GaussianFactor, size ) // create a linear factor graph const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); Ordering ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); + FactorGraph fg = example::createGaussianFactorGraph(ordering); // get some factors from the graph boost::shared_ptr factor1 = fg[0]; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index c6ec69d4e..ca8b9b1ea 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 67fa287d3..c4b90d882 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 88eb222bd..09047005e 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -40,7 +40,7 @@ TEST( Iterative, steepestDescent ) // Create factor graph Ordering ord; ord += L(1), X(1), X(2); - JacobianFactorGraph fg = createGaussianFactorGraph(ord); + FactorGraph fg = createGaussianFactorGraph(ord); // eliminate and solve VectorValues expected = *GaussianSequentialSolver(fg).optimize(); diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index c41b95881..39c5e2b70 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -54,9 +54,9 @@ TEST( SubgraphPreconditioner, planarOrdering ) { /* ************************************************************************* */ /** unnormalized error */ -static double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -65,7 +65,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) { TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction - JacobianFactorGraph A; + GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); @@ -82,12 +82,12 @@ TEST( SubgraphPreconditioner, planarGraph ) TEST( SubgraphPreconditioner, splitOffPlanarTree ) { // Build a planar graph - JacobianFactorGraph A; + GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); // Get the spanning tree and constraints, and check their sizes - JacobianFactorGraph T, C; + GaussianFactorGraph T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); LONGS_EQUAL(9,T.size()); LONGS_EQUAL(4,C.size()); @@ -103,16 +103,16 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree ) TEST( SubgraphPreconditioner, system ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 @@ -179,16 +179,16 @@ TEST( SubgraphPreconditioner, system ) TEST( SubgraphPreconditioner, conjugateGradients ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior Ordering ordering = planarOrdering(N); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 84f54a6fa..700f35261 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -38,9 +38,9 @@ using namespace example; /* ************************************************************************* */ /** unnormalized error */ -static double error(const JacobianFactorGraph& fg, const VectorValues& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -50,7 +50,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) { TEST( SubgraphSolver, constructor1 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b @@ -67,13 +67,13 @@ TEST( SubgraphSolver, constructor1 ) TEST( SubgraphSolver, constructor2 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The second constructor takes two factor graphs, @@ -88,13 +88,13 @@ TEST( SubgraphSolver, constructor2 ) TEST( SubgraphSolver, constructor3 ) { // Build a planar graph - JacobianFactorGraph Ab; + GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT From c4c5dec9a31a7dc75eb770f864e99590dc111d03 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Sep 2012 03:06:22 +0000 Subject: [PATCH 46/53] Fixed testIterative unit tests --- .cproject | 354 ++++++++++++++++------------- gtsam/linear/SubgraphSolver.cpp | 2 +- tests/smallExample.cpp | 4 +- tests/smallExample.h | 2 +- tests/testGaussianFactor.cpp | 15 +- tests/testGaussianFactorGraphB.cpp | 4 +- tests/testGaussianISAM.cpp | 2 +- tests/testIterative.cpp | 208 +++++++---------- 8 files changed, 293 insertions(+), 298 deletions(-) diff --git a/.cproject b/.cproject index 95c0f2a96..df6fdc5f5 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -985,6 +991,7 @@ make + testGraph.run true false @@ -992,6 +999,7 @@ make + testJunctionTree.run true false @@ -999,6 +1007,7 @@ make + testSymbolicBayesNetB.run true false @@ -1028,6 +1037,30 @@ true true + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + make -j5 @@ -1134,6 +1167,7 @@ make + testErrors.run true false @@ -1179,10 +1213,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1267,10 +1301,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1605,7 +1639,6 @@ make - testSimulated2DOriented.run true false @@ -1645,7 +1678,6 @@ make - testSimulated2D.run true false @@ -1653,7 +1685,6 @@ make - testSimulated3D.run true false @@ -1845,7 +1876,6 @@ make - tests/testGaussianISAM2 true false @@ -1867,102 +1897,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 -j1 @@ -2164,6 +2098,7 @@ cpack + -G DEB true false @@ -2171,6 +2106,7 @@ cpack + -G RPM true false @@ -2178,6 +2114,7 @@ cpack + -G TGZ true false @@ -2185,6 +2122,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2318,34 +2256,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2389,6 +2391,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index c34f7e22e..9794ba51e 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -101,7 +101,7 @@ boost::tuple SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); - const size_t n = index.size(), m = jfg.size(); + const size_t n = index.size(); DisjointSet D(n) ; GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 6df106acf..afa878a01 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,9 +138,9 @@ namespace example { } /* ************************************************************************* */ - FactorGraph createGaussianFactorGraph(const Ordering& ordering) { + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - FactorGraph fg; + GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); diff --git a/tests/smallExample.h b/tests/smallExample.h index 9c43563a3..f87847f24 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -65,7 +65,7 @@ namespace gtsam { * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ - FactorGraph createGaussianFactorGraph(const Ordering& ordering); + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index a41c8d9a7..f5419b7ff 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -57,10 +57,11 @@ TEST( GaussianFactor, linearFactor ) JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); // create a small linear factor graph - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph - JacobianFactor::shared_ptr lf = fg[1]; + JacobianFactor::shared_ptr lf = + boost::dynamic_pointer_cast(fg[1]); // check if the two factors are the same EXPECT(assert_equal(expected,*lf)); @@ -72,7 +73,7 @@ TEST( GaussianFactor, getDim ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // get a factor Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); GaussianFactor::shared_ptr factor = fg[0]; // get the size of a variable @@ -89,7 +90,7 @@ TEST( GaussianFactor, error ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the first factor from the factor graph GaussianFactor::shared_ptr lf = fg[0]; @@ -109,7 +110,7 @@ TEST( GaussianFactor, matrix ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version @@ -158,7 +159,7 @@ TEST( GaussianFactor, matrix_aug ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; @@ -210,7 +211,7 @@ TEST( GaussianFactor, size ) // create a linear factor graph const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get some factors from the graph boost::shared_ptr factor1 = fg[0]; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index ca8b9b1ea..306c64fb7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -369,7 +369,7 @@ TEST( GaussianFactorGraph, multiplication ) // create an ordering Ordering ord; ord += X(2),L(1),X(1); - FactorGraph A = createGaussianFactorGraph(ord); + GaussianFactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; @@ -523,7 +523,7 @@ TEST(GaussianFactorGraph, hasConstraints) EXPECT(hasConstraints(fgc2)); Ordering ordering; ordering += X(1), X(2), L(1); - FactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index ea91eda5a..74417b57a 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -42,7 +42,7 @@ using symbol_shorthand::L; // Some numbers that should be consistent among all smoother tests static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = - 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; + 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; static const double tol = 1e-4; diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 09047005e..bfa3a8e79 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -16,13 +16,14 @@ **/ #include +#include +#include +#include #include #include #include -//#include -//#include -#include -//#include +#include +#include #include @@ -32,23 +33,23 @@ using namespace example; using symbol_shorthand::X; // to create pose keys using symbol_shorthand::L; // to create landmark keys -static bool verbose = false; +static ConjugateGradientParameters parameters; +// add following below to add printing: +// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; /* ************************************************************************* */ TEST( Iterative, steepestDescent ) { // Create factor graph - Ordering ord; - ord += L(1), X(1), X(2); - FactorGraph fg = createGaussianFactorGraph(ord); + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // eliminate and solve VectorValues expected = *GaussianSequentialSolver(fg).optimize(); // Do gradient descent VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? - ConjugateGradientParameters parameters; -// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; VectorValues actual = steepestDescent(fg, zero, parameters); CHECK(assert_equal(expected,actual,1e-2)); } @@ -56,134 +57,93 @@ TEST( Iterative, steepestDescent ) /* ************************************************************************* */ TEST( Iterative, conjugateGradientDescent ) { -// // Expected solution -// Ordering ord; -// ord += L(1), X(1), X(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// VectorValues expected = fg.optimize(ord); // destructive -// -// // create graph and get matrices -// GaussianFactorGraph fg2 = createGaussianFactorGraph(); -// Matrix A; -// Vector b; -// Vector x0 = gtsam::zero(6); -// boost::tie(A, b) = fg2.matrix(ord); -// Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); -// -// // Do conjugate gradient descent, System version -// System Ab(A, b); -// Vector actualX = conjugateGradientDescent(Ab, x0, verbose); -// CHECK(assert_equal(expectedX,actualX,1e-9)); -// -// // Do conjugate gradient descent, Matrix version -// Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose); -// CHECK(assert_equal(expectedX,actualX2,1e-9)); -// -// // Do conjugate gradient descent on factor graph -// VectorValues zero = createZeroDelta(); -// VectorValues actual = conjugateGradientDescent(fg2, zero, verbose); -// CHECK(assert_equal(expected,actual,1e-2)); -// -// // Test method -// VectorValues actual2 = fg2.conjugateGradientDescent(zero, verbose); -// CHECK(assert_equal(expected,actual2,1e-2)); + // Create factor graph + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + + // eliminate and solve + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + + // get matrices + Matrix A; + Vector b; + Vector x0 = gtsam::zero(6); + boost::tie(A, b) = fg.jacobian(); + Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); + + // Do conjugate gradient descent, System version + System Ab(A, b); + Vector actualX = conjugateGradientDescent(Ab, x0, parameters); + CHECK(assert_equal(expectedX,actualX,1e-9)); + + // Do conjugate gradient descent, Matrix version + Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters); + CHECK(assert_equal(expectedX,actualX2,1e-9)); + + // Do conjugate gradient descent on factor graph + VectorValues zero = VectorValues::Zero(expected); + VectorValues actual = conjugateGradientDescent(fg, zero, parameters); + CHECK(assert_equal(expected,actual,1e-2)); } /* ************************************************************************* */ -/*TEST( Iterative, conjugateGradientDescent_hard_constraint ) +TEST( Iterative, conjugateGradientDescent_hard_constraint ) { - typedef Pose2Values::Key Key; + Values config; + Pose2 pose1 = Pose2(0.,0.,0.); + config.insert(X(1), pose1); + config.insert(X(2), Pose2(1.5,0.,0.)); - Pose2Values config; - config.insert(1, Pose2(0.,0.,0.)); - config.insert(2, Pose2(1.5,0.,0.)); + NonlinearFactorGraph graph; + graph.add(NonlinearEquality(X(1), pose1)); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); - Pose2Graph graph; - Matrix cov = eye(3); - graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); - graph.addHardConstraint(1, config[1]); + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); - VectorValues zeros; - zeros.insert(X(1),zero(3)); - zeros.insert(X(2),zero(3)); + VectorValues zeros = VectorValues::Zero(2, 3); - GaussianFactorGraph fg = graph.linearize(config); - VectorValues actual = conjugateGradientDescent(fg, zeros, true, 1e-3, 1e-5, 10); + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); - VectorValues expected; - expected.insert(X(1), zero(3)); - expected.insert(X(2), Vector_(-0.5,0.,0.)); - CHECK(assert_equal(expected, actual)); -}*/ + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); +} /* ************************************************************************* */ TEST( Iterative, conjugateGradientDescent_soft_constraint ) { -// Pose2Values config; -// config.insert(1, Pose2(0.,0.,0.)); -// config.insert(2, Pose2(1.5,0.,0.)); -// -// Pose2Graph graph; -// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); -// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); -// -// VectorValues zeros; -// zeros.insert(X(1),zero(3)); -// zeros.insert(X(2),zero(3)); -// -// boost::shared_ptr fg = graph.linearize(config); -// VectorValues actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100); -// -// VectorValues expected; -// expected.insert(X(1), zero(3)); -// expected.insert(X(2), Vector_(3,-0.5,0.,0.)); -// CHECK(assert_equal(expected, actual)); -} + Values config; + config.insert(X(1), Pose2(0.,0.,0.)); + config.insert(X(2), Pose2(1.5,0.,0.)); -/* ************************************************************************* */ -TEST( Iterative, subgraphPCG ) -{ -// typedef Pose2Values::Key Key; -// -// Pose2Values theta_bar; -// theta_bar.insert(1, Pose2(0.,0.,0.)); -// theta_bar.insert(2, Pose2(1.5,0.,0.)); -// -// Pose2Graph graph; -// graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); -// graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); -// -// // generate spanning tree and create ordering -// PredecessorMap tree = graph.findMinimumSpanningTree(); -// list keys = predecessorMap2Keys(tree); -// list symbols; -// symbols.resize(keys.size()); -// std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); -// Ordering ordering(symbols); -// -// Key root = keys.back(); -// Pose2Graph T, C; -// graph.split(tree, T, C); -// -// // build the subgraph PCG system -// boost::shared_ptr Ab1_ = T.linearize(theta_bar); -// SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar); -// SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar); -// SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering); -// SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); -// -// VectorValues zeros = VectorValues::zero(*xbar); -// -// // Solve the subgraph PCG -// VectorValues ybar = conjugateGradients (system, zeros, verbose, 1e-5, 1e-5, 100); -// VectorValues actual = system.x(ybar); -// -// VectorValues expected; -// expected.insert(X(1), zero(3)); -// expected.insert(X(2), Vector_(3, -0.5, 0., 0.)); -// CHECK(assert_equal(expected, actual)); + NonlinearFactorGraph graph; + graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); + + VectorValues zeros = VectorValues::Zero(2, 3); + + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); + + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ From 7266293a6129717a8ceb698676d554bc975a36f1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Sep 2012 03:52:01 +0000 Subject: [PATCH 47/53] Cleaned up some remaining JacobinaFactorGraph remnants --- tests/smallExample.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index afa878a01..71c8269e9 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -459,12 +459,7 @@ namespace example { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); - - GaussianFactorGraph jfg; - BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg) - jfg.push_back(boost::dynamic_pointer_cast(factor)); - - return boost::make_tuple(jfg, xtrue); + return boost::make_tuple(*gfg, xtrue); } /* ************************************************************************* */ @@ -482,21 +477,21 @@ namespace example { GaussianFactorGraph T, C; // Add the x11 constraint to the tree - T.push_back(boost::dynamic_pointer_cast(original[0])); + T.push_back(original[0]); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(boost::dynamic_pointer_cast(original[i])); + T.push_back(original[i]); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(boost::dynamic_pointer_cast(original[i])); + T.push_back(original[i]); else - C.push_back(boost::dynamic_pointer_cast(original[i])); + C.push_back(original[i]); return make_pair(T, C); } From 6c2f213091f9e7fef90eea567c2a506281c0ffb8 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Wed, 5 Sep 2012 15:03:35 +0000 Subject: [PATCH 48/53] 1. use DSFVector for spanning tree in SubgraphSolver 2. move DSFVector from unstable to stable 3. create Dummy.cpp to prevent base_unstable degenerate --- {gtsam_unstable => gtsam}/base/DSFVector.cpp | 2 +- {gtsam_unstable => gtsam}/base/DSFVector.h | 0 .../base/tests/testDSFVector.cpp | 2 +- gtsam/linear/SubgraphSolver.cpp | 56 ++----------------- gtsam/linear/SubgraphSolver.h | 25 --------- gtsam_unstable/base/Dummy.cpp | 43 ++++++++++++++ gtsam_unstable/base/Dummy.h | 22 ++------ 7 files changed, 55 insertions(+), 95 deletions(-) rename {gtsam_unstable => gtsam}/base/DSFVector.cpp (98%) rename {gtsam_unstable => gtsam}/base/DSFVector.h (100%) rename {gtsam_unstable => gtsam}/base/tests/testDSFVector.cpp (99%) create mode 100644 gtsam_unstable/base/Dummy.cpp diff --git a/gtsam_unstable/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp similarity index 98% rename from gtsam_unstable/base/DSFVector.cpp rename to gtsam/base/DSFVector.cpp index cdea89f34..6d79dcb53 100644 --- a/gtsam_unstable/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include using namespace std; diff --git a/gtsam_unstable/base/DSFVector.h b/gtsam/base/DSFVector.h similarity index 100% rename from gtsam_unstable/base/DSFVector.h rename to gtsam/base/DSFVector.h diff --git a/gtsam_unstable/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp similarity index 99% rename from gtsam_unstable/base/tests/testDSFVector.cpp rename to gtsam/base/tests/testDSFVector.cpp index 8997559f5..c0b72f1a0 100644 --- a/gtsam_unstable/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -24,7 +24,7 @@ using namespace boost::assign; #include -#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 9794ba51e..47d33831b 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -102,7 +103,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); const size_t n = index.size(); - DisjointSet D(n) ; + DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); @@ -126,10 +127,10 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { if ( jf->keys().size() == 1 ) augment = true; else { const Index u = jf->keys()[0], v = jf->keys()[1], - u_root = D.find(u), v_root = D.find(v); + u_root = D.findSet(u), v_root = D.findSet(v); if ( u_root != v_root ) { t++; augment = true ; - D.makeUnion(u_root, v_root); + D.makeUnionInPlace(u_root, v_root); } } if ( augment ) At->push_back(jf); @@ -139,53 +140,4 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { return boost::tie(At, Ac); } - -SubgraphSolver::DisjointSet::DisjointSet(const size_t n):n_(n),rank_(n,1),parent_(n) { - for ( Index i = 0 ; i < n ; ++i ) parent_[i] = i ; -} - -Index SubgraphSolver::DisjointSet::makeUnion(const Index &u, const Index &v) { - - Index u_root = find(u), v_root = find(v) ; - Index u_rank = rank(u), v_rank = rank(v) ; - - if ( u_root != v_root ) { - if ( v_rank > u_rank ) { - parent_[u_root] = v_root ; - rank_[v_root] += rank_[u_root] ; - return v_root ; - } - else { - parent_[v_root] = u_root ; - rank_[u_root] += rank_[v_root] ; - return u_root ; - } - } - return u_root ; -} - -Index SubgraphSolver::DisjointSet::find(const Index &u) { - vector path ; - Index x = u; - Index x_root = parent_[x] ; - - // find the root, and keep the vertices along the path - while ( x != x_root ) { - path.push_back(x) ; - x = x_root ; - x_root = parent_[x] ; - } - - // path compression - BOOST_FOREACH(const Index &i, path) { - rank_[i] = 1 ; - parent_[i] = x_root ; - } - - return x_root ; -} - - - - } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 441fdaeef..7c60c0588 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -82,31 +82,6 @@ protected: boost::tuple splitGraph(const GaussianFactorGraph &gfg) ; - -public: - - // a simplfied implementation of disjoint set data structure. - class DisjointSet { - protected: - size_t n_ ; - std::vector rank_ ; - std::vector parent_ ; - - public: - // initialize a disjoint set, point every vertex to itself - DisjointSet(const size_t n) ; - inline size_t size() const { return n_ ; } - - // union the root of u and and the root of v, return the root of u and v - Index makeUnion(const Index &u, const Index &v) ; - - // return the root of u - Index find(const Index &u) ; - - // return the rank of x, which is defined as the cardinality of the set containing x - inline size_t rank(const Index &x) {return rank_[find(x)] ;} - }; - }; } // namespace gtsam diff --git a/gtsam_unstable/base/Dummy.cpp b/gtsam_unstable/base/Dummy.cpp new file mode 100644 index 000000000..90d71d27e --- /dev/null +++ b/gtsam_unstable/base/Dummy.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 Dummy.h + * @brief Dummy class for testing MATLAB memory allocation + * @author Andrew Melim + * @author Frank Dellaert + * @date June 14, 2012 + */ + +#include +#include + +namespace gtsam { + +static size_t gDummyCount = 0; + +Dummy::Dummy():id(++gDummyCount) { + std::cout << "Dummy constructor " << id << std::endl; +} + +Dummy::~Dummy() { + std::cout << "Dummy destructor " << id << std::endl; +} + +void Dummy::print(const std::string& s) const { + std::cout << s << "Dummy " << id << std::endl; +} + +unsigned char Dummy::dummyTwoVar(unsigned char a) const { + return a; +} + +} diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index 995186cd6..0ab344f74 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -17,26 +17,16 @@ * @date June 14, 2012 */ -namespace gtsam { +#include - static size_t gDummyCount; +namespace gtsam { struct Dummy { size_t id; - Dummy():id(++gDummyCount) { - std::cout << "Dummy constructor " << id << std::endl; - } - ~Dummy() { - std::cout << "Dummy destructor " << id << std::endl; - } - void print(const std::string& s="") const { - std::cout << s << "Dummy " << id << std::endl; - } - - unsigned char dummyTwoVar(unsigned char a) const { - return a; - } - + Dummy(); + ~Dummy(); + void print(const std::string& s="") const ; + unsigned char dummyTwoVar(unsigned char a) const ; }; } // namespace gtsam From 4443752a18165495c1f705d6955db491416b2700 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Wed, 5 Sep 2012 17:04:48 +0000 Subject: [PATCH 49/53] clean the gfg to jfg conversion code --- gtsam/linear/GaussianFactorGraph.cpp | 73 ++++++++-------------------- gtsam/linear/GaussianFactorGraph.h | 4 +- gtsam/linear/JacobianFactor.cpp | 9 ++++ gtsam/linear/JacobianFactor.h | 4 +- gtsam/linear/SubgraphSolver.cpp | 6 +-- 5 files changed, 36 insertions(+), 60 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 14d558e62..ad31bd9e5 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -521,15 +521,14 @@ break; } // \EliminatePreferCholesky + + + /* ************************************************************************* */ Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { Errors e; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); e.push_back((*Ai)*x); } return e; @@ -544,12 +543,8 @@ break; void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { Errors::iterator ei = e; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - *ei = (*Ai)*x; + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + *ei = (*Ai)*x; ei++; } } @@ -560,12 +555,8 @@ break; // For each factor add the gradient contribution Errors::const_iterator ei = e.begin(); BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - Ai->transposeMultiplyAdd(alpha,*(ei++),x); + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha,*(ei++),x); } } @@ -575,12 +566,8 @@ break; VectorValues g = VectorValues::Zero(x0); Errors e; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - e.push_back(Ai->error_vector(x0)); + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(Ai->error_vector(x0)); } transposeMultiplyAdd(fg, 1.0, e, g); return g; @@ -592,12 +579,8 @@ break; g.setZero(); Errors e; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - e.push_back(-Ai->getb()); + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(-Ai->getb()); } transposeMultiplyAdd(fg, 1.0, e, g); } @@ -606,12 +589,8 @@ break; void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { Index i = 0 ; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - r[i] = Ai->getb(); + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + r[i] = Ai->getb(); i++; } VectorValues Ax = VectorValues::SameStructure(r); @@ -624,12 +603,8 @@ break; r.vector() = Vector::Zero(r.dim()); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - SubVector &y = r[i]; + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + SubVector &y = r[i]; for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { y += Ai->getA(j) * x[*j]; } @@ -642,12 +617,8 @@ break; x.vector() = Vector::Zero(x.dim()); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { x[*j] += Ai->getA(j).transpose() * r[i]; } ++i; @@ -658,12 +629,8 @@ break; boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { boost::shared_ptr e(new Errors); BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - JacobianFactor::shared_ptr Ai; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(Ai_G)) - Ai = Ai_J; - else - Ai = boost::make_shared(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - e->push_back(Ai->error_vector(x)); + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e->push_back(Ai->error_vector(x)); } return e; } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6c06e34c0..5e6e792be 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -303,7 +303,9 @@ namespace gtsam { */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals = 1); - + + /****** Linear Algebra Opeations ******/ + /** return A*x */ Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 759be4948..c0aefab39 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -489,4 +489,13 @@ namespace gtsam { return description_.c_str(); } + /* ************************************************************************* */ + JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) { + result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + return result; + } + } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index def36b086..1c73f621a 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -322,8 +322,10 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(model_); ar & BOOST_SERIALIZATION_NVP(matrix_); } - }; // JacobianFactor + /** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */ + JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf); + } // gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 47d33831b..4cb95c404 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -111,11 +111,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - JacobianFactor::shared_ptr jf; - if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast(gf)) - jf = Ai_J; - else - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf); if ( jf->keys().size() > 2 ) { throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); From 574a9711ccf83ba71660470fab5f7eed30d0751f Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Thu, 6 Sep 2012 01:21:15 +0000 Subject: [PATCH 50/53] add a conversion function from GaussianFactorGraph to JacobianFactorGraph --- gtsam/linear/GaussianFactorGraph.cpp | 9 +++++++++ gtsam/linear/GaussianFactorGraph.h | 3 +++ gtsam/linear/SubgraphPreconditioner.cpp | 4 +--- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index ad31bd9e5..eda9732b2 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -635,4 +635,13 @@ break; return e; } + /* ************************************************************************* */ + GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + result->push_back(convertToJacobianFactorPtr(gf)); + } + return result; + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5e6e792be..b6486b3a6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -354,4 +354,7 @@ namespace gtsam { inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { return *gaussianErrors_(fg, x); } + /** Convert all factors to JacobianFactor */ + GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg); + } // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 72aa25554..04eed29d2 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -17,8 +17,6 @@ #include #include -#include - #include using namespace std; @@ -28,7 +26,7 @@ namespace gtsam { /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { } /* ************************************************************************* */ From 1322b7e49d982c9bfd364507380b86bce014e26e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 14:33:40 +0000 Subject: [PATCH 51/53] Fixed size checking bug --- gtsam/inference/FactorGraph-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 97a7fa1c6..04490dbc6 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -100,7 +100,7 @@ namespace gtsam { ++ firstIndex; // Check that number of variables is in bounds - if(firstIndex + nFrontals >= variableIndex.size()) + if(firstIndex + nFrontals > variableIndex.size()) throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); // Get set of involved factors From ce3c774bfa9ae72da9c906a835d0a3e53fae0213 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 14:33:42 +0000 Subject: [PATCH 52/53] Added non-const HessianFactor matrix block accessors and inlined all matrix block accessors for speed --- gtsam/linear/HessianFactor.cpp | 10 --------- gtsam/linear/HessianFactor.h | 37 ++++++++++++++++++++++++++++++++-- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a44f5c682..e152738fb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -307,16 +307,6 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { } } -/* ************************************************************************* */ -double HessianFactor::constantTerm() const { - return info_(this->size(), this->size())(0,0); -} - -/* ************************************************************************* */ -HessianFactor::constColumn HessianFactor::linearTerm() const { - return info_.rangeColumn(0, this->size(), this->size(), 0); -} - /* ************************************************************************* */ Matrix HessianFactor::computeInformation() const { return info_.full().selfadjointView(); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 2e9b67c2a..9e4ed2074 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -242,16 +242,39 @@ namespace gtsam { */ constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } + /** Return the upper-triangular part of the full *augmented* information matrix, * as described above. See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ constBlock info() const { return info_.full(); } + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + Block info() { return info_.full(); } + /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ */ - double constantTerm() const; + double constantTerm() const { return info_(this->size(), this->size())(0,0); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double& constantTerm() { return info_(this->size(), this->size())(0,0); } /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can @@ -259,9 +282,19 @@ namespace gtsam { * @return The linear term \f$ g \f$ */ constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); } + /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const; + constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); } /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an From 6d1b86c2e04ffd185171cc8d156deb650f8285f3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 14:33:44 +0000 Subject: [PATCH 53/53] Moved JacobianFactor type check/conversion functions into SubgraphSolver instead of GaussianFactorGraph and JacobianFactor --- gtsam/linear/GaussianFactorGraph.cpp | 17 ++++++++--------- gtsam/linear/GaussianFactorGraph.h | 3 --- gtsam/linear/JacobianFactor.cpp | 9 --------- gtsam/linear/JacobianFactor.h | 3 --- gtsam/linear/SubgraphPreconditioner.cpp | 13 +++++++++++++ gtsam/linear/SubgraphPreconditioner.h | 1 - gtsam/linear/SubgraphSolver.cpp | 12 +++++------- 7 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index eda9732b2..222f025a0 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -523,6 +523,14 @@ break; + /* ************************************************************************* */ + static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) { + result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + return result; + } /* ************************************************************************* */ Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { @@ -635,13 +643,4 @@ break; return e; } - /* ************************************************************************* */ - GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { - result->push_back(convertToJacobianFactorPtr(gf)); - } - return result; - } - } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index b6486b3a6..5e6e792be 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -354,7 +354,4 @@ namespace gtsam { inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { return *gaussianErrors_(fg, x); } - /** Convert all factors to JacobianFactor */ - GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg); - } // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c0aefab39..759be4948 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -489,13 +489,4 @@ namespace gtsam { return description_.c_str(); } - /* ************************************************************************* */ - JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { - JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); - if( !result ) { - result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - return result; - } - } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 1c73f621a..f2bd9c75d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -324,8 +324,5 @@ namespace gtsam { } }; // JacobianFactor - /** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */ - JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf); - } // gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 04eed29d2..8b5a3708f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -22,6 +22,19 @@ using namespace std; namespace gtsam { + + /* ************************************************************************* */ + static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; + } /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index db7c023ce..f266928d7 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -33,7 +33,6 @@ namespace gtsam { class SubgraphPreconditioner { public: - typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedBayesNet; typedef boost::shared_ptr sharedFG; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 4cb95c404..9986fb76c 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -111,26 +111,24 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf); - - if ( jf->keys().size() > 2 ) { + if ( gf->keys().size() > 2 ) { throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } bool augment = false ; /* check whether this factor should be augmented to the "tree" graph */ - if ( jf->keys().size() == 1 ) augment = true; + if ( gf->keys().size() == 1 ) augment = true; else { - const Index u = jf->keys()[0], v = jf->keys()[1], + const Index u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), v_root = D.findSet(v); if ( u_root != v_root ) { t++; augment = true ; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(jf); - else Ac->push_back(jf); + if ( augment ) At->push_back(gf); + else Ac->push_back(gf); } return boost::tie(At, Ac);