From 0f3732d7f5acceb8d77379a5814544f9e11e84bc Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 16 Aug 2013 16:32:20 +0000 Subject: [PATCH] Moved summarization back to MastSLAM - functionality largely replaced with newer elimination functionality --- gtsam/nonlinear/summarization.cpp | 79 ---------- gtsam/nonlinear/summarization.h | 56 -------- tests/testSummarization.cpp | 230 ------------------------------ 3 files changed, 365 deletions(-) delete mode 100644 gtsam/nonlinear/summarization.cpp delete mode 100644 gtsam/nonlinear/summarization.h delete mode 100644 tests/testSummarization.cpp diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp deleted file mode 100644 index e38226c9b..000000000 --- a/gtsam/nonlinear/summarization.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -GaussianFactorGraph summarize(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode) { - const size_t nrEliminatedKeys = values.size() - saved_keys.size(); - GaussianFactorGraph full_graph = *graph.linearize(values); - - // If we aren't eliminating anything, linearize and return - if (!nrEliminatedKeys || saved_keys.empty()) - return full_graph; - - std::vector saved_keys_vec(saved_keys.begin(), saved_keys.end()); - -// // Compute a constrained ordering with variables grouped to end -// std::map ordering_constraints; -// -// // group all saved variables together -// BOOST_FOREACH(const gtsam::Key& key, saved_keys) -// ordering_constraints.insert(make_pair(key, 1)); -// -// Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); - - // Linearize the system - GaussianFactorGraph summarized_system; - - // PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system - // PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system - // SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) - // SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) - - switch (mode) { - case PARTIAL_QR: { -// summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); - break; - } - case PARTIAL_CHOLESKY: { -// summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); - break; - } - case SEQUENTIAL_QR: { - summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateQR)); -// GaussianSequentialSolver solver(full_graph, true); -// summarized_system.push_back(*solver.jointFactorGraph(indices)); - break; - } - case SEQUENTIAL_CHOLESKY: { - summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateCholesky)); -// GaussianSequentialSolver solver(full_graph, false); -// summarized_system.push_back(*solver.jointFactorGraph(indices)); - break; - } - } - return summarized_system; -} - -/* ************************************************************************* */ -NonlinearFactorGraph summarizeAsNonlinearContainer( - const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, SummarizationMode mode) { - GaussianFactorGraph summarized_graph = summarize(graph, values, saved_keys, mode); - return LinearContainerFactor::convertLinearGraph(summarized_graph); -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h deleted file mode 100644 index cfed16330..000000000 --- a/gtsam/nonlinear/summarization.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file summarization.h - * - * @brief Types and utility functions for summarization - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -// Flag to control how summarization should be performed -typedef enum { - PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system - PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system - SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) - SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) -} SummarizationMode; - -/** - * 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. - * - * Requirement: set of keys in the graph should match the set of keys in the - * values structure. - * - * @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 mode controls what elimination technique and requirements to use - * @return a pair of the remaining graph and the ordering used for linearization - */ -GaussianFactorGraph GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph, - const Values& values, const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); - -/** - * 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, SummarizationMode mode = PARTIAL_QR); - -} // \namespace gtsam diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp deleted file mode 100644 index 9db7586ef..000000000 --- a/tests/testSummarization.cpp +++ /dev/null @@ -1,230 +0,0 @@ -/** - * @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 - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -const double tol=1e-5; - -typedef gtsam::PriorFactor PosePrior; -typedef gtsam::BetweenFactor PoseBetween; -typedef gtsam::BearingRangeFactor PosePointBearingRange; - -gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); -gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); - -/* ************************************************************************* */ -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); - - SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2); - SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4); - - 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 += PosePrior(xA0, Pose2(), model3); - graph += PoseBetween(xA0, xA1, pose0.between(pose1), model3); - graph += PoseBetween(xA1, xA2, pose1.between(pose2), model3); - graph += PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2); - graph += PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2); - graph += PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2); - - KeySet saved_keys; - saved_keys += lA3, lA5; - - { - // Summarize to a linear system - GaussianFactorGraph actLinGraph; - SummarizationMode mode = PARTIAL_QR; - actLinGraph = summarize(graph, values, saved_keys, mode); - -// Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; -// EXPECT(assert_equal(expSumOrdering, actOrdering)); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph += JacobianFactor( - lA3, - Matrix_(4,2, - 0.595867, 0.605092, - 0.0, -0.406109, - 0.0, 0.0, - 0.0, 0.0), - lA5, - Matrix_(4,2, - -0.125971, -0.160052, - 0.13586, 0.301096, - 0.268667, 0.31703, - 0.0, -0.131698), - zero(4), diagmodel4); - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system using cholesky - compare to previous version - GaussianFactorGraph actLinGraph; - SummarizationMode mode = PARTIAL_CHOLESKY; - actLinGraph = summarize(graph, values, saved_keys, mode); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph += HessianFactor(JacobianFactor( - lA3, - Matrix_(4,2, - 0.595867, 0.605092, - 0.0, -0.406109, - 0.0, 0.0, - 0.0, 0.0), - lA5, - Matrix_(4,2, - -0.125971, -0.160052, - 0.13586, 0.301096, - 0.268667, 0.31703, - 0.0, -0.131698), - zero(4), diagmodel4)); - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system with joint factor graph version - SummarizationMode mode = SEQUENTIAL_QR; - GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph += JacobianFactor( - lA3, - Matrix_(2,2, - 0.595867, 0.605092, - 0.0, 0.406109), - lA5, - Matrix_(2,2, - -0.125971, -0.160052, - -0.13586, -0.301096), - zero(2), diagmodel2); - - expLinGraph += JacobianFactor( - lA5, - Matrix_(2,2, - 0.268667, 0.31703, - 0.0, 0.131698), - zero(2), diagmodel2); - - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } - - { - // Summarize to a linear system with joint factor graph version - SummarizationMode mode = SEQUENTIAL_CHOLESKY; - GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); - - // Does not split out subfactors where possible - GaussianFactorGraph expLinGraph; - expLinGraph += JacobianFactor( - lA3, - Matrix_(2,2, - 0.595867, 0.605092, - 0.0, 0.406109), - lA5, - Matrix_(2,2, - -0.125971, -0.160052, - -0.13586, -0.301096), - zero(2), diagmodel2); - - expLinGraph += JacobianFactor( - lA5, - Matrix_(2,2, - 0.268667, 0.31703, - 0.0, 0.131698), - zero(2), diagmodel2); - - EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); - - // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); - - EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); - } -} - -/* ************************************************************************* */ -TEST( testSummarization, no_summarize_case ) { - // Checks a corner case in which no variables are being eliminated - gtsam::Key key = 7; - gtsam::KeySet saved_keys; saved_keys.insert(key); - NonlinearFactorGraph graph; - graph += PosePrior(key, Pose2(1.0, 2.0, 0.3), model3); - graph += PosePrior(key, Pose2(2.0, 3.0, 0.4), model3); - Values values; - values.insert(key, Pose2(0.0, 0.0, 0.1)); - - SummarizationMode mode = SEQUENTIAL_CHOLESKY; - GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); - GaussianFactorGraph expLinGraph = *graph.linearize(values); - EXPECT(assert_equal(expLinGraph, actLinGraph)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */