diff --git a/.cproject b/.cproject index 07c34bf38..42416eeb8 100644 --- a/.cproject +++ b/.cproject @@ -307,6 +307,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -333,7 +341,6 @@ make - tests/testBayesTree.run true false @@ -341,7 +348,6 @@ make - testBinaryBayesNet.run true false @@ -389,7 +395,6 @@ make - testSymbolicBayesNet.run true false @@ -397,7 +402,6 @@ make - tests/testSymbolicFactor.run true false @@ -405,7 +409,6 @@ make - testSymbolicFactorGraph.run true false @@ -421,20 +424,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -523,22 +517,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -555,6 +533,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -579,26 +573,34 @@ 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 + + + make + -j5 + testLinearContainerFactor.run true true true @@ -683,34 +685,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 - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -1061,7 +1055,6 @@ make - testGraph.run true false @@ -1069,7 +1062,6 @@ make - testJunctionTree.run true false @@ -1077,7 +1069,6 @@ make - testSymbolicBayesNetB.run true false @@ -1131,6 +1122,14 @@ true true + + make + -j5 + testSummarization.run + true + true + true + make -j5 @@ -1237,7 +1236,6 @@ make - testErrors.run true false @@ -1283,6 +1281,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1363,14 +1369,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1677,6 +1675,7 @@ make + testSimulated2DOriented.run true false @@ -1716,6 +1715,7 @@ make + testSimulated2D.run true false @@ -1723,6 +1723,7 @@ make + testSimulated3D.run true false @@ -1922,6 +1923,7 @@ make + tests/testGaussianISAM2 true false @@ -1943,6 +1945,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 -j3 @@ -2144,7 +2242,6 @@ cpack - -G DEB true false @@ -2152,7 +2249,6 @@ cpack - -G RPM true false @@ -2160,7 +2256,6 @@ cpack - -G TGZ true false @@ -2168,7 +2263,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2310,98 +2404,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 @@ -2445,38 +2475,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/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp new file mode 100644 index 000000000..c416315f4 --- /dev/null +++ b/gtsam/nonlinear/summarization.cpp @@ -0,0 +1,104 @@ +/** + * @file summarization.cpp + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#include +#include +#include +//#include + +using namespace std; + +namespace gtsam { + +///* ************************************************************************* */ +//GaussianFactorGraph::shared_ptr summarizeGraphSequential( +// const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { +// GaussianSequentialSolver solver(full_graph, useQR); +// return solver.jointFactorGraph(indices); +//} +// +///* ************************************************************************* */ +//GaussianFactorGraph::shared_ptr summarizeGraphSequential( +// const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { +// std::vector indices; +// BOOST_FOREACH(const Key& k, saved_keys) +// indices.push_back(ordering[k]); +// return summarizeGraphSequential(full_graph, indices, useQR); +//} + +/* ************************************************************************* */ +std::pair +summarize(const NonlinearFactorGraph& graph, const Values& values, + const KeySet& saved_keys, bool useQR) { + // compute a new ordering with non-saved keys first + Ordering ordering; + KeySet eliminatedKeys; + BOOST_FOREACH(const Key& key, values.keys()) { + if (!saved_keys.count(key)) { + eliminatedKeys.insert(key); + ordering += key; + } + } + + BOOST_FOREACH(const Key& key, saved_keys) + ordering += key; + + // Linearize the system + GaussianFactorGraph full_graph = *graph.linearize(values, ordering); + GaussianFactorGraph summarized_system; + if (useQR) + summarized_system.push_back(EliminateQR(full_graph, eliminatedKeys.size()).second); + else + summarized_system.push_back(EliminateCholesky(full_graph, eliminatedKeys.size()).second); + + return make_pair(summarized_system, ordering); +} + +///* ************************************************************************* */ +//std::pair +//partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, +// const KeySet& saved_keys) { +// // compute a new ordering with non-saved keys first +// Ordering ordering; +// KeySet eliminatedKeys; +// BOOST_FOREACH(const Key& key, values.keys()) { +// if (!saved_keys.count(key)) { +// eliminatedKeys.insert(key); +// ordering += key; +// } +// } +// +// BOOST_FOREACH(const Key& key, saved_keys) +// ordering += key; +// +// // reorder the system +// GaussianFactorGraph linearSystem = *graph.linearize(values, ordering); +// +// // Eliminate frontals +// GaussianFactorGraph summarized_system; +// summarized_system.push_back(EliminateCholesky(linearSystem, eliminatedKeys.size()).second); +// return make_pair(summarized_system, ordering); +//} +// +//std::pair GTSAM_EXPORT +//summarize(const NonlinearFactorGraph& graph, const Values& values, +// const KeySet& saved_keys, bool useQR = true); + + +/* ************************************************************************* */ +NonlinearFactorGraph summarizeAsNonlinearContainer( + const NonlinearFactorGraph& graph, const Values& values, + const KeySet& saved_keys, bool useQR) { + GaussianFactorGraph summarized_graph; + Ordering ordering; + boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, useQR); + return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering); +} + +/* ************************************************************************* */ +} // \namespace gtsam + diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h new file mode 100644 index 000000000..d92651235 --- /dev/null +++ b/gtsam/nonlinear/summarization.h @@ -0,0 +1,76 @@ +/** + * @file summarization.h + * + * @brief Types and utility functions for summarization + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + +/** + * Summarization function to remove a subset of variables from a system with the + * sequential solver. This does not require that the system be fully constrained. + * + * @param graph A full nonlinear graph + * @param values The chosen linearization point + * @param saved_keys is the set of keys for variables that should remain + * @param useQR uses QR as the elimination algorithm if true, Cholesky otherwise + * @return a pair of the remaining graph and the ordering used for linearization + */ +std::pair GTSAM_EXPORT +summarize(const NonlinearFactorGraph& graph, const Values& values, + const KeySet& saved_keys, bool useQR = true); + +///** +// * Summarization function to remove a subset of variables from a system using +// * a partial cholesky approach. This does not require that the system be fully constrained. +// * Performs linearization to apply an ordering. +// * +// * @param graph A full nonlinear graph +// * @param values The chosen linearization point +// * @param saved_keys is the set of keys for variables that should remain +// * * @return a pair of the remaining graph and the ordering used for linearization +// */ +//std::pair GTSAM_EXPORT +//partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, +// const KeySet& saved_keys); + +/** + * Performs the same summarization technique used in summarize(), but returns the + * result as a NonlinearFactorGraph comprised of LinearContainerFactors. + * + * @param graph A full nonlinear graph + * @param values The chosen linearization point + * @param saved_keys is the set of keys for variables that should remain + * @param useQR uses QR as the elimination algorithm if true, Cholesky otherwise + * @return a NonlinearFactorGraph with linear factors + */ +NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( + const NonlinearFactorGraph& graph, const Values& values, + const KeySet& saved_keys, bool useQR = true); + +/** + * Summarization function that eliminates a set of variables (does not convert to Jacobians) + * NOTE: uses sequential solver - requires fully constrained system + */ +//GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( +// const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); +// +///** Summarization that also converts keys to indices */ +//GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( +// const GaussianFactorGraph& full_graph, const Ordering& ordering, +// const KeySet& saved_keys, bool useQR = false); + +} // \namespace gtsam + + diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/summarization.cpp deleted file mode 100644 index 680719c82..000000000 --- a/gtsam_unstable/nonlinear/summarization.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { - GaussianSequentialSolver solver(full_graph, useQR); - return solver.jointFactorGraph(indices); -} - -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { - std::vector indices; - BOOST_FOREACH(const Key& k, saved_keys) - indices.push_back(ordering[k]); - return summarizeGraphSequential(full_graph, indices, useQR); -} - -/* ************************************************************************* */ -std::pair -sequentialSummarization(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys) { - // compute a new ordering with non-saved keys first - Ordering ordering; - KeySet eliminatedKeys; - BOOST_FOREACH(const Key& key, values.keys()) { - if (!saved_keys.count(key)) { - eliminatedKeys.insert(key); - ordering += key; - } - } - - BOOST_FOREACH(const Key& key, saved_keys) - ordering += key; - - // Linearize the system - GaussianFactorGraph full_graph = *graph.linearize(values, ordering); - GaussianFactorGraph summarized_system; - summarized_system.push_back(EliminateQR(full_graph, eliminatedKeys.size()).second); - return make_pair(summarized_system, ordering); -} - -/* ************************************************************************* */ -std::pair -partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys) { - // compute a new ordering with non-saved keys first - Ordering ordering; - KeySet eliminatedKeys; - BOOST_FOREACH(const Key& key, values.keys()) { - if (!saved_keys.count(key)) { - eliminatedKeys.insert(key); - ordering += key; - } - } - - BOOST_FOREACH(const Key& key, saved_keys) - ordering += key; - - // reorder the system - GaussianFactorGraph linearSystem = *graph.linearize(values, ordering); - - // Eliminate frontals - GaussianFactorGraph summarized_system; - summarized_system.push_back(EliminateCholesky(linearSystem, eliminatedKeys.size()).second); - return make_pair(summarized_system, ordering); -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam_unstable/nonlinear/summarization.h b/gtsam_unstable/nonlinear/summarization.h deleted file mode 100644 index 7226c810b..000000000 --- a/gtsam_unstable/nonlinear/summarization.h +++ /dev/null @@ -1,61 +0,0 @@ -/** - * @file summarization.h - * - * @brief Types and utility functions for summarization - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -#include -#include - -namespace gtsam { - -/** - * Summarization function to remove a subset of variables from a system with the - * sequential solver. This does not require that the system be fully constrained. - * - * @param graph A full nonlinear graph - * @param values The chosen linearization point - * @param saved_keys is the set of keys for variables that should remain - * @return a pair of the remaining graph and the ordering used for linearization - */ -std::pair GTSAM_UNSTABLE_EXPORT -sequentialSummarization(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys); - -/** - * Summarization function to remove a subset of variables from a system using - * a partial cholesky approach. This does not require that the system be fully constrained. - * Performs linearization to apply an ordering. - * - * @param graph A full nonlinear graph - * @param values The chosen linearization point - * @param saved_keys is the set of keys for variables that should remain - * * @return a pair of the remaining graph and the ordering used for linearization - */ -std::pair GTSAM_UNSTABLE_EXPORT -partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys); - -/** - * Summarization function that eliminates a set of variables (does not convert to Jacobians) - * NOTE: uses sequential solver - requires fully constrained system - */ -GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); - -/** Summarization that also converts keys to indices */ -GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraph& full_graph, const Ordering& ordering, - const KeySet& saved_keys, bool useQR = false); - -} // \namespace gtsam - - diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp new file mode 100644 index 000000000..5ea83a843 --- /dev/null +++ b/tests/testSummarization.cpp @@ -0,0 +1,117 @@ +/** + * @file testSummarization.cpp + * + * @brief Test ported from MastSLAM for a simple batch summarization technique + * + * @date May 7, 2013 + * @author Alex Cunningham + */ + +#include +#include + +#include + +#include + +#include + +#include +#include +#include + +#include +#include +#include + +//#include +// +//#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; +//using namespace ddfsam; +//using namespace planarDDF; + +const double tol=1e-5; + +typedef gtsam::PriorFactor PosePrior; +typedef gtsam::BetweenFactor PoseBetween; +typedef gtsam::BearingRangeFactor PosePointBearingRange; + +/* ************************************************************************* */ +TEST( testSummarization, example_from_ddf1 ) { + + Key xA0 = LabeledSymbol('x', 'A', 0), + xA1 = LabeledSymbol('x', 'A', 1), + xA2 = LabeledSymbol('x', 'A', 2); + Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5); + + gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); + gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); + + SharedDiagonal model = noiseModel::Unit::Create(2); + + Pose2 pose0; + Pose2 pose1(1.0, 0.0, 0.0); + Pose2 pose2(2.0, 0.0, 0.0); + Point2 landmark3(3.0, 3.0); + Point2 landmark5(5.0, 5.0); + + Values values; + values.insert(xA0, pose0); + values.insert(xA1, pose1); + values.insert(xA2, pose2); + values.insert(lA3, landmark3); + values.insert(lA5, landmark5); + + // build from nonlinear graph/values + NonlinearFactorGraph graph; + graph.add(PosePrior(xA0, Pose2(), model3)); + graph.add(PoseBetween(xA0, xA1, pose0.between(pose1), model3)); + graph.add(PoseBetween(xA1, xA2, pose1.between(pose2), model3)); + graph.add(PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2)); + graph.add(PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2)); + graph.add(PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2)); + + KeySet saved_keys; + saved_keys += lA3, lA5; + + // Summarize to a linear system + GaussianFactorGraph actLinGraph; Ordering actOrdering; + bool useQR = true; + boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, useQR); + + Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; + EXPECT(assert_equal(expSumOrdering, actOrdering)); + + GaussianFactorGraph expLinGraph; + expLinGraph.add( + expSumOrdering[lA3], + Matrix_(2,2, + 0.595867, 0.605092, + 0.0, 0.406109), + expSumOrdering[lA5], + Matrix_(2,2, + -0.125971, -0.160052, + -0.13586, -0.301096), + zero(2), model); + expLinGraph.add( + expSumOrdering[lA5], + Matrix_(2,2, + 0.268667, 0.31703, + 0.0, 0.131698), + zero(2), model); + EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); + + // Summarize directly from a nonlinear graph to another nonlinear graph + NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, useQR); + NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); + + EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */