diff --git a/.cproject b/.cproject index 32cb2d277..0a809847f 100644 --- a/.cproject +++ b/.cproject @@ -2412,6 +2412,14 @@ true true + + make + -j5 + check.linear_unstable + true + true + true + make -j5 diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 960b39f59..b872b0ff1 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -4,7 +4,7 @@ set (gtsam_unstable_subdirs base geometry discrete -# linear + linear dynamics nonlinear slam @@ -38,7 +38,7 @@ set(gtsam_unstable_srcs ${geometry_srcs} ${discrete_srcs} ${dynamics_srcs} - #${linear_srcs} + ${linear_srcs} ${nonlinear_srcs} ${slam_srcs} ${navigation_srcs} diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt index 51bfa8f86..34c8fbc96 100644 --- a/gtsam_unstable/linear/CMakeLists.txt +++ b/gtsam_unstable/linear/CMakeLists.txt @@ -3,10 +3,15 @@ file(GLOB base_headers "*.h") install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear) # Components to link tests in this subfolder against -set(linear_local_libs - linear - linear_unstable - base) +set(linear_local_libs + linear_unstable + nonlinear + linear + inference + geometry + base + ccolamd +) set (linear_full_libs ${gtsam-default} diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp new file mode 100644 index 000000000..e889b3cbb --- /dev/null +++ b/gtsam_unstable/linear/bayesTreeOperations.cpp @@ -0,0 +1,152 @@ +/** + * @file bayesTreeOperations.cpp + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +std::set keysToIndices(const KeySet& keys, const Ordering& ordering) { + std::set result; + BOOST_FOREACH(const Key& key, keys) + result.insert(ordering[key]); + return result; +} + +/* ************************************************************************* */ +GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) { + GaussianFactorGraph result; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { + GaussianFactorGraph split = splitFactor(factor); + result.push_back(split); + } + return result; +} + +/* ************************************************************************* */ +GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) { + GaussianFactorGraph result; + if (!factor) return result; + + // Needs to be a jacobian factor - just pass along hessians + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); + if (!jf) { + result.push_back(factor); + return result; + } + + // Loop over variables and strip off factors using split conditionals + // Assumes upper triangular structure + JacobianFactor::const_iterator rowIt, colIt; + const size_t totalRows = jf->rows(); + size_t rowsRemaining = totalRows; + for (rowIt = jf->begin(); rowIt != jf->end(); ++rowIt) { + // get dim of current variable + size_t varDim = jf->getDim(rowIt); + size_t startRow = totalRows - rowsRemaining; + size_t nrRows = std::min(rowsRemaining, varDim); + + // Extract submatrices + std::vector > matrices; + for (colIt = rowIt; colIt != jf->end(); ++colIt) { + Index idx = *colIt; + const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows); + matrices.push_back(make_pair(idx, subA)); + } + + Vector subB = jf->getb().segment(startRow, nrRows); + Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows); + SharedDiagonal model; + if (jf->get_model()->isConstrained()) + model = noiseModel::Constrained::MixedSigmas(sigmas); + else + model = noiseModel::Diagonal::Sigmas(sigmas); + + // extract matrices from each + // assemble into new JacobianFactor + result.add(matrices, subB, model); + + rowsRemaining -= nrRows; + } + return result; +} + +/* ************************************************************************* */ +GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) { + GaussianFactorGraph result; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); + if (factor && (!jf || jf->size() > 1)) + result.push_back(factor); + } + return result; +} + +/* ************************************************************************* */ +void findCliques(const GaussianBayesTree::sharedClique& current_clique, + std::set& result) { + // Add the current clique + result.insert(current_clique->conditional()); + + // Add the parent if not root + if (!current_clique->isRoot()) + findCliques(current_clique->parent(), result); +} + +/* ************************************************************************* */ +std::set findAffectedCliqueConditionals( + const GaussianBayesTree& bayesTree, const std::set& savedIndices) { + std::set affected_cliques; + // FIXME: track previously found keys more efficiently + BOOST_FOREACH(const Index& index, savedIndices) { + GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index]; + + // add path back to root to affected set + findCliques(clique, affected_cliques); + } + return affected_cliques; +} + +/* ************************************************************************* */ +std::deque +findPathCliques(const GaussianBayesTree::sharedClique& initial) { + std::deque result, parents; + if (initial->isRoot()) + return result; + result.push_back(initial->parent()); + parents = findPathCliques(initial->parent()); + result.insert(result.end(), parents.begin(), parents.end()); + return result; +} + +/* ************************************************************************* */ +GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals) { + GaussianFactorGraph result; + if (root && root->conditional()) { + GaussianConditional::shared_ptr conditional = root->conditional(); + if (!splitConditionals) + result.push_back(conditional->toFactor()); + else + result.push_back(splitFactor(conditional->toFactor())); + } + BOOST_FOREACH(const GaussianBayesTree::sharedClique& child, root->children()) + result.push_back(liquefy(child, splitConditionals)); + return result; +} + +/* ************************************************************************* */ +GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals) { + return liquefy(bayesTree.root(), splitConditionals); +} + +/* ************************************************************************* */ +} // \namespace gtsam + diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h new file mode 100644 index 000000000..0cf0cdf84 --- /dev/null +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -0,0 +1,79 @@ +/** + * @file bayesTreeOperations.h + * + * @brief Types and utility functions for operating on linear systems + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +// Managing orderings + +/** Converts sets of keys to indices by way of orderings */ +std::set keysToIndices(const KeySet& keys, const Ordering& ordering); + +// Linear Graph Operations + +/** + * Given a graph, splits each factor into factors where the dimension is + * that of the first variable. + */ +GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph); + +/** + * Splits a factor into factors where the dimension is + * that of the first variable. + */ +GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor); + +/** Removes prior jacobian factors from the graph */ +GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph); + +// Bayes Tree / Conditional operations + +/** + * Given a Bayes Tree, return conditionals corresponding to cliques that have or + * are connected to a set of wanted variables. + * + * @return the set of conditionals extracted from cliques. + */ +std::set findAffectedCliqueConditionals( + const GaussianBayesTree& bayesTree, const std::set& savedIndices); + +/** + * Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals + * Adds any new cliques from path to root to the result set. + * + * Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations, + * which ensures unique cliques, but the order of the cliques is meaningless + */ +void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique, + std::set& result); + +/** + * Given a clique, returns a sequence of clique parents to the root, not including the + * given clique. + */ +std::deque +findPathCliques(const GaussianBayesTree::sharedClique& initial); + +/** + * Liquefies a GaussianBayesTree into a GaussianFactorGraph recursively, given either a + * root clique or a full bayes tree. + * + * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors + */ +GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals = false); +GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals = false); + +} // \namespace gtsam + + diff --git a/gtsam_unstable/linear/conditioning.cpp b/gtsam_unstable/linear/conditioning.cpp new file mode 100644 index 000000000..c02f4d48c --- /dev/null +++ b/gtsam_unstable/linear/conditioning.cpp @@ -0,0 +1,205 @@ +/** + * @file summarization.cpp + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional, + const std::set& saved_indices, const VectorValues& solution) { + const bool verbose = false; + + if (!initConditional) + return initConditional; + + if (verbose) { + cout << "backsubSummarize() Starting" << endl; + initConditional->print("Full initial conditional"); + } + + // Check for presence of variables to remove + std::set frontalsToRemove, parentsToRemove; + BOOST_FOREACH(const Index& frontal, initConditional->frontals()) + if (!saved_indices.count(frontal)) + frontalsToRemove.insert(frontal); + + BOOST_FOREACH(const Index& parent, initConditional->parents()) + if (!saved_indices.count(parent)) + parentsToRemove.insert(parent); + + // If all variables in this conditional are to be saved, just return initial conditional + if (frontalsToRemove.empty() && parentsToRemove.empty()) + return initConditional; + + // If none of the frontal variables are to be saved, return empty pointer + if (frontalsToRemove.size() == initConditional->nrFrontals()) + return GaussianConditional::shared_ptr(); + + // Collect dimensions of the new conditional + if (verbose) cout << " Collecting dimensions" << endl; + size_t newTotalRows = 0, newTotalCols = 1; // Need spacing for RHS + size_t newNrFrontals = 0; + size_t oldOffset = 0; + vector newDims, oldDims; + vector oldColOffsets; + vector newIndices; + vector newIdxToOldIdx; // Access to arrays, maps from new var to old var + const vector& oldIndices = initConditional->keys(); + const size_t oldNrFrontals = initConditional->nrFrontals(); + GaussianConditional::const_iterator varIt = initConditional->beginFrontals(); + size_t oldIdx = 0; + for (; varIt != initConditional->endFrontals(); ++varIt) { + size_t varDim = initConditional->dim(varIt); + oldDims += varDim; + if (!frontalsToRemove.count(*varIt)) { + newTotalCols += varDim; + newTotalRows += varDim; + newDims += varDim; + newIndices += *varIt; + ++newNrFrontals; + newIdxToOldIdx += oldIdx; + oldColOffsets += oldOffset; + } + oldOffset += varDim; + ++oldIdx; + } + varIt = initConditional->beginParents(); + for (; varIt != initConditional->endParents(); ++varIt) { + size_t varDim = initConditional->dim(varIt); + oldDims += varDim; + if (!parentsToRemove.count(*varIt)) { + newTotalCols += varDim; + newDims += varDim; + newIndices += *varIt; + } + } + newDims += 1; // For the RHS + + // Initialize new conditional + Matrix full_matrix = Matrix::Zero(newTotalRows, newTotalCols); + Vector sigmas = zero(newTotalRows); + if (verbose) cout << " Initializing new conditional\nfull_matrix:\n" << full_matrix << endl; + + // Fill in full matrix - iterate over rows for each sub-conditional + const size_t oldRNrCols = initConditional->get_R().cols(); + size_t newColOffset = 0; + for (size_t newfrontalIdx=0; newfrontalIdx rblock = + initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset); + if (verbose) cout << " rblock\n" << rblock << endl; + + + // set the R matrix for this var + full_matrix.block(newColOffset, newColOffset, dim, dim) = rblock.leftCols(dim); + if (verbose) cout << " full_matrix: set R\n" << full_matrix << endl; + + // set RHS + full_matrix.block(newColOffset, newTotalCols-1, dim, 1) = initConditional->get_d().segment(oldColOffset, dim); + if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl; + + // set sigmas + sigmas.segment(newColOffset, dim) = initConditional->get_sigmas().segment(oldColOffset, dim); + + // add parents in R block, while updating rhs + // Loop through old variable list + size_t newParentStartCol = newColOffset + dim; + size_t oldParentStartCol = dim; // Copying from Rblock - offset already accounted for + for (size_t oldIdx = newIdxToOldIdx[newfrontalIdx]+1; oldIdxbeginParents(); + for (; oldParent != initConditional->endParents(); ++oldParent) { + Index parentKey = *oldParent; + size_t parentDim = initConditional->dim(oldParent); + if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl; + if (parentsToRemove.count(parentKey)) { + // Solve out the variable + const Vector& parentSol = solution.at(parentKey); + assert((size_t)parentSol.size() == parentDim); + full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -= + initConditional->get_S(oldParent).middleRows(oldColOffset, dim) * parentSol; + if (verbose) cout << " full_matrix: update rhs from parent in S\n" << full_matrix << endl; + } else { + // Copy the matrix block + full_matrix.block(newColOffset, newParentStartCol, dim, parentDim) = + initConditional->get_S(oldParent).middleRows(oldColOffset, dim); + if (verbose) cout << " full_matrix: add parent from S\n" << full_matrix << endl; + } + } + + // Increment the rows + newColOffset += dim; + oldColOffset += dim; + } + + // Construct a new conditional + if (verbose) cout << "backsubSummarize() Complete!" << endl; + GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end()); + return GaussianConditional::shared_ptr(new + GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas)); +} + +/* ************************************************************************* */ +GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree, + const std::set& saved_indices) { + const bool verbose = false; + + VectorValues solution = optimize(bayesTree); + + // FIXME: set of conditionals does not manage possibility of solving out whole separators + std::set affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices); + + // Summarize conditionals separately + GaussianFactorGraph summarized_graph; + BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) { + if (verbose) conditional->print("Initial conditional"); + GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution); + if (reducedConditional) { + if (verbose) reducedConditional->print("Final conditional"); + summarized_graph.push_back(reducedConditional->toFactor()); + } else if (verbose) { + cout << "Conditional removed after summarization!" << endl; + } + } + return summarized_graph; +} + +/* ************************************************************************* */ + +} // \namespace gtsam + diff --git a/gtsam_unstable/linear/conditioning.h b/gtsam_unstable/linear/conditioning.h new file mode 100644 index 000000000..2c6c7ddb6 --- /dev/null +++ b/gtsam_unstable/linear/conditioning.h @@ -0,0 +1,43 @@ +/** + * @file summarization.h + * + * @brief Types and utility functions for summarization + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Backsubstitution-based conditioning - reduces conditionals to + * densities on a sub-set of variables. + * + * Corner cases: + * - If no frontal vars are saved, returns a null pointer + * + * @param initConditional the conditional from which to remove a variable + * @param saved_indices is the set of indices that should appear in the result + * @param solution is a full solution for the system + */ +gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional, + const std::set& saved_indices, const gtsam::VectorValues& solution); + +/** + * Backsubstitution-based conditioning for a complete Bayes Tree - reduces + * conditionals by solving out variables to eliminate. Traverses the tree to + * add the correct dummy factors whenever a separator is eliminated. + */ +gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree, + const std::set& saved_indices); + + +} // \namespace gtsam + + diff --git a/gtsam_unstable/linear/corruptInitialization.h b/gtsam_unstable/linear/corruptInitialization.h new file mode 100644 index 000000000..de657f054 --- /dev/null +++ b/gtsam_unstable/linear/corruptInitialization.h @@ -0,0 +1,56 @@ +/** + * @file corruptInitialization.h + * + * @brief Utilities for using noisemodels to corrupt given initialization value + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** given a noisemodel and a measurement, add noise to the measurement */ +template +T corruptWithNoise(const T& init, + const noiseModel::Base::shared_ptr& model, Sampler& sampler) { + Vector n = zero(model->dim()); + noiseModel::Diagonal::shared_ptr + diag_model = boost::dynamic_pointer_cast(model); + if (diag_model) + n = sampler.sampleNewModel(diag_model); + return init.retract(n); +} + +// specialization for doubles - just adds, rather than retract +template<> +inline double corruptWithNoise(const double& init, + const noiseModel::Base::shared_ptr& model, Sampler& sampler) { + double n = 0.0; + noiseModel::Diagonal::shared_ptr + diag_model = boost::dynamic_pointer_cast(model); + if (diag_model) + n = sampler.sampleNewModel(diag_model)(0); + return init + n; +} + +// specialization for doubles - just adds, rather than retract +template<> +inline Vector corruptWithNoise(const Vector& init, + const noiseModel::Base::shared_ptr& model, Sampler& sampler) { + Vector n = zero(init.size()); + noiseModel::Diagonal::shared_ptr + diag_model = boost::dynamic_pointer_cast(model); + if (diag_model) + n = sampler.sampleNewModel(diag_model); + return init + n; +} + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp new file mode 100644 index 000000000..b1c833f2c --- /dev/null +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -0,0 +1,125 @@ +/** + * @file testLinearTools.cpp + * + * @brief + * + * @date Aug 27, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +using namespace gtsam; + +SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2)); +SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4)); +SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6)); + +/* ************************************************************************* */ +TEST( testLinearTools, splitFactor ) { + + // Build upper-triangular system + JacobianFactor initFactor( + 0,Matrix_(4, 2, + 1.0, 2.0, + 0.0, 3.0, + 0.0, 0.0, + 0.0, 0.0), + 1,Matrix_(4, 2, + 1.0, 2.0, + 9.0, 3.0, + 6.0, 8.0, + 0.0, 7.0), + Vector_(4, 0.1, 0.2, 0.3, 0.4), + model4); + + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; + + expSplit.add( + 0,Matrix_(2, 2, + 1.0, 2.0, + 0.0, 3.0), + 1,Matrix_(2, 2, + 1.0, 2.0, + 9.0, 3.0), + Vector_(2, 0.1, 0.2), + model2); + expSplit.add( + 1,Matrix_(2, 2, + 6.0, 8.0, + 0.0, 7.0), + Vector_(2, 0.3, 0.4), + model2); + + EXPECT(assert_equal(expSplit, actSplit)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( testLinearTools, splitFactor2 ) { + + // Build upper-triangular system + JacobianFactor initFactor( + 0,Matrix_(6, 2, + 1.0, 2.0, + 0.0, 3.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0), + 1,Matrix_(6, 2, + 1.0, 2.0, + 9.0, 3.0, + 6.0, 8.0, + 0.0, 7.0, + 0.0, 0.0, + 0.0, 0.0), + 2,Matrix_(6, 2, + 1.1, 2.2, + 9.1, 3.2, + 6.1, 8.2, + 0.1, 7.2, + 0.1, 3.2, + 0.0, 1.2), + Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6), + model6); + + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; + + expSplit.add( + 0,Matrix_(2, 2, + 1.0, 2.0, + 0.0, 3.0), + 1,Matrix_(2, 2, + 1.0, 2.0, + 9.0, 3.0), + 2,Matrix_(2, 2, + 1.1, 2.2, + 9.1, 3.2), + Vector_(2, 0.1, 0.2), + model2); + expSplit.add( + 1,Matrix_(2, 2, + 6.0, 8.0, + 0.0, 7.0), + 2,Matrix_(2, 2, + 6.1, 8.2, + 0.1, 7.2), + Vector_(2, 0.3, 0.4), + model2); + expSplit.add( + 2,Matrix_(2, 2, + 0.1, 3.2, + 0.0, 1.2), + Vector_(2, 0.5, 0.6), + model2); + + EXPECT(assert_equal(expSplit, actSplit)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testConditioning.cpp b/gtsam_unstable/linear/tests/testConditioning.cpp new file mode 100644 index 000000000..69646fbdc --- /dev/null +++ b/gtsam_unstable/linear/tests/testConditioning.cpp @@ -0,0 +1,250 @@ +/** + * @file testConditioning.cpp + * + * @brief Experiments using backsubstitution for conditioning (not summarization, it turns out) + * + * @date Sep 3, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +const double tol = 1e-5; + +// Simple example +Matrix R = Matrix_(3,3, + 1.0,-2.0,-3.0, + 0.0, 3.0,-5.0, + 0.0, 0.0, 6.0); +Vector d = Vector_(3, + 0.1, 0.2, 0.3); +Vector x = Vector_(3, + 0.55, + 0.15, + 0.05); + +/* ************************************************************************* */ +TEST( testConditioning, directed_elimination_example ) { + + // create a 3-variable system from which to eliminate variables + // Scalar variables, pre-factorized into R,d system + // Use multifrontal representation + // Variables 0, 1, 2 - want to summarize out 1 + Vector expx = R.triangularView().solve(d); + EXPECT(assert_equal(x, expx, tol)); + EXPECT(assert_equal(Vector(R*x), d, tol)); + + // backsub-summarized version + Matrix Rprime = Matrix_(2,2, + 1.0,-3.0, + 0.0, 6.0); + Vector dprime = Vector_(2, + d(0) - R(0,1)*x(1), + d(2)); + Vector xprime = Vector_(2, + x(0), // Same solution, just smaller + x(2)); + EXPECT(assert_equal(Vector(Rprime*xprime), dprime, tol)); +} + +/* ************************************************************************* */ +TEST( testConditioning, directed_elimination_singlefrontal ) { + // Gaussian conditional with a single frontal variable, parent is to be removed + // Top row from above example + + Index root_key = 0, removed_key = 1, remaining_parent = 2; + Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0); + Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0); + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); + + VectorValues solution; + solution.insert(0, x.segment(0,1)); + solution.insert(1, x.segment(1,1)); + solution.insert(2, x.segment(2,1)); + + std::set saved_indices; + saved_indices += root_key, remaining_parent; + + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr expSummarized(new + GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas)); + + CHECK(actSummarized); + EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); + + // Simple test of base case: if target index isn't present, return clone + GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution); + CHECK(actSummarizedSimple); + EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol)); + + // case where frontal variable is to be eliminated - return null + GaussianConditional::shared_ptr removeFrontalInit(new + GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas)); + GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution); + EXPECT(!actRemoveFrontal); +} + +/* ************************************************************************* */ +TEST( testConditioning, directed_elimination_multifrontal ) { + // Use top two rows from the previous example + Index root_key = 0, removed_key = 1, remaining_parent = 2; + Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1), + Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0); + Vector d1 = d.segment(0,2), sigmas1 = Vector_(1, 1.0), sigmas2 = Vector_(2, 1.0, 1.0); + + + std::list > terms; + terms += make_pair(root_key, Matrix(R11.col(0))); + terms += make_pair(removed_key, Matrix(R11.col(1))); + terms += make_pair(remaining_parent, S); + GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2)); + + VectorValues solution; + solution.insert(0, x.segment(0,1)); + solution.insert(1, x.segment(1,1)); + solution.insert(2, x.segment(2,1)); + + std::set saved_indices; + saved_indices += root_key, remaining_parent; + + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + GaussianConditional::shared_ptr expSummarized(new + GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); + + CHECK(actSummarized); + EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); +} + +/* ************************************************************************* */ +TEST( testConditioning, directed_elimination_multifrontal_multidim ) { + // use larger example, three frontal variables, dim = 2 each, two parents (one removed) + // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4; + // Remove 1, 3 + Matrix Rinit = Matrix_(6, 11, + 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2, + 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3, + 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4, + 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5, + 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); + + vector init_dims; init_dims += 2, 2, 2, 2, 2, 1; + GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); + Vector sigmas = ones(6); + vector init_keys; init_keys += 0, 1, 2, 3, 4; + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); + + // Construct a solution vector + VectorValues solution; + solution.insert(0, zero(2)); + solution.insert(1, zero(2)); + solution.insert(2, zero(2)); + solution.insert(3, Vector_(2, 1.0, 2.0)); + solution.insert(4, Vector_(2, 3.0, 4.0)); + + initConditional->solveInPlace(solution); + + std::set saved_indices; + saved_indices += 0, 2, 4; + + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + CHECK(actSummarized); + + Matrix Rexp = Matrix_(4, 7, + 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1, + 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2, + 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5, + 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6); + + // Update rhs + Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3); + Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); + + vector exp_dims; exp_dims += 2, 2, 2, 1; + GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); + Vector exp_sigmas = ones(4); + vector exp_keys; exp_keys += 0, 2, 4; + GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); + + EXPECT(assert_equal(expSummarized, *actSummarized, tol)); +} + +/* ************************************************************************* */ +TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { + // Example from LinearAugmentedSystem + // 4 variables, last two in ordering kept - should be able to do this with no computation. + + vector init_dims; init_dims += 3, 3, 2, 2, 1; + + //Full initial conditional: density on [3] [4] [5] [6] + Matrix Rinit = Matrix_(10, 11, + 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0, + 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0, + 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0, + 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0, + 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0); + Vector dinit = Vector_(10, + -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134); + Rinit.rightCols(1) = dinit; + Vector sigmas = ones(10); + + GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); + vector init_keys; init_keys += 3, 4, 5, 6; + GaussianConditional::shared_ptr initConditional(new + GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); + + // Calculate a solution + VectorValues solution; + solution.insert(0, zero(3)); + solution.insert(1, zero(3)); + solution.insert(2, zero(3)); + solution.insert(3, zero(3)); + solution.insert(4, zero(3)); + solution.insert(5, zero(2)); + solution.insert(6, zero(2)); + + initConditional->solveInPlace(solution); + + // Perform summarization + std::set saved_indices; + saved_indices += 5, 6; + + GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); + CHECK(actSummarized); + + // Create expected value on [5], [6] + Matrix Rexp = Matrix_(4, 5, + 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011, + 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465, + 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795, + 0.0, 0.0, 0.0, 2.24068986, -0.0222134); + Vector expsigmas = ones(4); + + vector exp_dims; exp_dims += 2, 2, 1; + GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); + vector exp_keys; exp_keys += 5, 6; + GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); + + EXPECT(assert_equal(expConditional, *actSummarized, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index 767b0162b..e40ffdc09 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -7,6 +7,7 @@ set(nonlinear_local_libs nonlinear_unstable nonlinear linear + linear_unstable inference geometry base diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/summarization.cpp new file mode 100644 index 000000000..1c14ea974 --- /dev/null +++ b/gtsam_unstable/nonlinear/summarization.cpp @@ -0,0 +1,60 @@ +/** + * @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 +partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, + const KeySet& overlap_keys) { + // compute a new ordering with non-saved keys first + Ordering ordering; + KeySet eliminatedKeys; + BOOST_FOREACH(const Key& key, values.keys()) { + if (!overlap_keys.count(key)) { + eliminatedKeys.insert(key); + ordering += key; + } + } + + BOOST_FOREACH(const Key& key, overlap_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 new file mode 100644 index 000000000..5d232299e --- /dev/null +++ b/gtsam_unstable/nonlinear/summarization.h @@ -0,0 +1,48 @@ +/** + * @file summarization.h + * + * @brief Types and utility functions for summarization + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include + +#include +#include + +namespace gtsam { + +// Sequential graph summarization +typedef FactorGraph JacobianGraph; +typedef boost::shared_ptr shared_jacobianGraph; + +/** + * Summarization function that eliminates a set of variables (does not convert to Jacobians) + * NOTE: uses sequential solver - requires fully constrained system + */ +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); + +/** Summarization that also converts keys to indices */ +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const Ordering& ordering, + const KeySet& saved_keys, bool useQR = false); + +/** + * 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 + */ +std::pair +partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, + const KeySet& overlap_keys); + + +} // \namespace gtsam + +