diff --git a/.cproject b/.cproject index 9b476b986..4be5dfb1e 100644 --- a/.cproject +++ b/.cproject @@ -315,14 +315,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -349,6 +341,7 @@ make + tests/testBayesTree.run true false @@ -356,6 +349,7 @@ make + testBinaryBayesNet.run true false @@ -403,6 +397,7 @@ make + testSymbolicBayesNet.run true false @@ -410,6 +405,7 @@ make + tests/testSymbolicFactor.run true false @@ -417,6 +413,7 @@ make + testSymbolicFactorGraph.run true false @@ -432,11 +429,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -525,22 +531,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -557,6 +547,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -581,42 +587,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 - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -661,26 +651,42 @@ 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 + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -1071,6 +1077,7 @@ make + testGraph.run true false @@ -1078,6 +1085,7 @@ make + testJunctionTree.run true false @@ -1085,6 +1093,7 @@ make + testSymbolicBayesNetB.run true false @@ -1252,6 +1261,7 @@ make + testErrors.run true false @@ -1297,14 +1307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1385,6 +1387,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1691,7 +1701,6 @@ make - testSimulated2DOriented.run true false @@ -1731,7 +1740,6 @@ make - testSimulated2D.run true false @@ -1739,7 +1747,6 @@ make - testSimulated3D.run true false @@ -1849,6 +1856,14 @@ true true + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + make -j2 @@ -1963,7 +1978,6 @@ make - tests/testGaussianISAM2 true false @@ -1985,102 +1999,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 -j3 @@ -2282,6 +2200,7 @@ cpack + -G DEB true false @@ -2289,6 +2208,7 @@ cpack + -G RPM true false @@ -2296,6 +2216,7 @@ cpack + -G TGZ true false @@ -2303,6 +2224,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2468,34 +2390,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 @@ -2539,6 +2525,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_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index ab2a669fc..7a9acc787 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -4,7 +4,6 @@ set (gtsam_unstable_subdirs base geometry #discrete - linear dynamics nonlinear slam @@ -53,8 +52,7 @@ set(gtsam_unstable_srcs ${base_srcs} ${geometry_srcs} #${discrete_srcs} - ${dynamics_srcs} - ${linear_srcs} + ${dynamics_srcs} ${nonlinear_srcs} ${slam_srcs} ) diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt deleted file mode 100644 index e73e95743..000000000 --- a/gtsam_unstable/linear/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -# Install headers -file(GLOB linear_headers "*.h") -install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear) - -# Components to link tests in this subfolder against -set(linear_local_libs - linear_unstable - nonlinear - linear - inference - geometry - base - ccolamd -) - -set (linear_full_libs - ${gtsam-default} - ${gtsam_unstable-default}) - -# Exclude tests that don't work -set (base_excluded_tests "") - -# Add all tests -gtsam_add_subdir_tests(linear_unstable "${linear_local_libs}" "${linear_full_libs}" "${linear_excluded_tests}") -add_dependencies(check.unstable check.linear_unstable) diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp deleted file mode 100644 index 9dd8ed607..000000000 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file bayesTreeOperations.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include - -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -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() && rowsRemaining > 0; ++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) { - Key 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); - SharedDiagonal model; - if(jf->get_model()) { - Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows); - 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 Key& index, savedIndices) { - GaussianBayesTree::sharedClique clique = bayesTree[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; -} - -/* ************************************************************************* */ -} // \namespace gtsam - diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h deleted file mode 100644 index 1658c8973..000000000 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ /dev/null @@ -1,99 +0,0 @@ -/** - * @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 { - -// Linear Graph Operations - -/** - * Given a graph, splits each factor into factors where the dimension is - * that of the first variable. - */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph); - -/** - * Splits a factor into factors where the dimension is - * that of the first variable. - */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor); - -/** Removes prior jacobian factors from the graph */ -GTSAM_UNSTABLE_EXPORT 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. - */ -GTSAM_UNSTABLE_EXPORT 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 - */ -GTSAM_UNSTABLE_EXPORT 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. - */ -GTSAM_UNSTABLE_EXPORT std::deque -findPathCliques(const GaussianBayesTree::sharedClique& initial); - -/** - * Liquefies a BayesTree into a GaussianFactorGraph recursively, given a - * root clique - * - * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors - */ -template -GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) { - GaussianFactorGraph result; - if (root && root->conditional()) { - GaussianConditional::shared_ptr conditional = root->conditional(); - if (!splitConditionals) - result.push_back(conditional); - else - result.push_back(splitFactor(conditional)); - } - BOOST_FOREACH(typename BAYESTREE::sharedClique child, root->children) - result.push_back(liquefy(child, splitConditionals)); - return result; -} - -/** - * Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree. - * - * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors - */ -template -GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { - GaussianFactorGraph result; - BOOST_FOREACH(const typename BAYESTREE::sharedClique& root, bayesTree.roots()) - result.push_back(liquefy(root, splitConditionals)); - return result; -} - -} // \namespace gtsam - - diff --git a/gtsam_unstable/linear/conditioning.cpp b/gtsam_unstable/linear/conditioning.cpp deleted file mode 100644 index 63871776c..000000000 --- a/gtsam_unstable/linear/conditioning.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/** - * @file summarization.cpp - * - * @date Jun 22, 2012 - * @author Alex Cunningham - */ - -#include -#include - -#include - -using namespace std; -using namespace boost::assign; - -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 Key& frontal, initConditional->frontals()) - if (!saved_indices.count(frontal)) - frontalsToRemove.insert(frontal); - - BOOST_FOREACH(const Key& 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->getDim(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->getDim(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_model()->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) { - Key parentKey = *oldParent; - size_t parentDim = initConditional->getDim(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()); - VerticalBlockMatrix matrices(newDims, full_matrix); - return GaussianConditional::shared_ptr(new - GaussianConditional(newIndices, newNrFrontals, matrices, noiseModel::Diagonal::Sigmas(sigmas))); -} - -/* ************************************************************************* */ -GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree, - const std::set& saved_indices) { - const bool verbose = false; - - VectorValues solution = bayesTree.optimize(); - - // 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); - } 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 deleted file mode 100644 index 22eeaaeb2..000000000 --- a/gtsam_unstable/linear/conditioning.h +++ /dev/null @@ -1,45 +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 { - -/** - * 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_UNSTABLE_EXPORT GaussianConditional::shared_ptr conditionDensity( - const GaussianConditional::shared_ptr& initConditional, - const std::set& saved_indices, const 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_UNSTABLE_EXPORT GaussianFactorGraph conditionDensity( - const GaussianBayesTree& bayesTree, - const std::set& saved_indices); - - -} // \namespace gtsam - - diff --git a/gtsam_unstable/linear/corruptInitialization.h b/gtsam_unstable/linear/corruptInitialization.h deleted file mode 100644 index de657f054..000000000 --- a/gtsam_unstable/linear/corruptInitialization.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @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 deleted file mode 100644 index b0d36c515..000000000 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ /dev/null @@ -1,373 +0,0 @@ -/** - * @file testLinearTools.cpp - * - * @brief - * - * @date Aug 27, 2012 - * @author Alex Cunningham - */ - -#include - -#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)); - -using namespace std; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -static const double tol = 1e-4; - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor1 ) { - - // 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( testBayesTreeOperations, 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)); -} - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor3 ) { - - // 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), - 2,Matrix_(4, 2, - 1.1, 2.2, - 9.1, 3.2, - 6.1, 8.2, - 0.1, 7.2), - 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), - 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); - - EXPECT(assert_equal(expSplit, actSplit)); -} - -/* ************************************************************************* */ -// 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; - -/* ************************************************************************* */ -TEST( testBayesTreeOperations, liquefy ) { - using namespace example; - - // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7); - - // Create the Bayes tree - GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); -// bayesTree.print("Full tree"); - - SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); - SharedDiagonal unit4 = noiseModel::Diagonal::Sigmas(Vector_(ones(4))); - SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector_(ones(2))); - - // Liquefy the tree back into a graph - { - GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals - GaussianFactorGraph expGraph; - - Matrix A12 = Matrix_(6, 2, - 1.73205081, 0.0, - 0.0, 1.73205081, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A15 = Matrix_(6, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - 1.47196014, 0.0, - 0.0, 1.47196014, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A16 = Matrix_(6, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - -0.226455407, 0.0, - 0.0, -0.226455407, - 1.49357599, 0.0, - 0.0, 1.49357599); - expGraph.add(2, A12, 5, A15, 6, A16, zeros(6,1), unit6); - - Matrix A21 = Matrix_(4, 2, - 1.73205081, 0.0, - 0.0, 1.73205081, - 0.0, 0.0, - 0.0, 0.0); - - Matrix A24 = Matrix_(4, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A26 = Matrix_(4, 2, - -0.577350269, 0.0, - 0.0, -0.577350269, - -0.226455407, 0.0, - 0.0, -0.226455407); - - expGraph.add(1, A21, 4, A24, 6, A26, zeros(4,1), unit4); - - Matrix A30 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - - Matrix A34 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); - - Matrix A43 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - Matrix A45 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); - - EXPECT(assert_equal(expGraph, actGraph, tol)); - } - - // Liquefy the tree back into a graph, splitting factors - { - GaussianFactorGraph actGraph = liquefy(bayesTree, true); - GaussianFactorGraph expGraph; - - // Conditional 1 - { - Matrix A12 = Matrix_(2, 2, - 1.73205081, 0.0, - 0.0, 1.73205081); - - Matrix A15 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - Matrix A16 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - expGraph.add(2, A12, 5, A15, 6, A16, zeros(2,1), unit2); - } - - { - Matrix A15 = Matrix_(2, 2, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A16 = Matrix_(2, 2, - -0.226455407, 0.0, - 0.0, -0.226455407); - expGraph.add(5, A15, 6, A16, zeros(2,1), unit2); - } - - { - Matrix A16 = Matrix_(2, 2, - 1.49357599, 0.0, - 0.0, 1.49357599); - expGraph.add(6, A16, zeros(2,1), unit2); - } - - // Conditional 2 - { - Matrix A21 = Matrix_(2, 2, - 1.73205081, 0.0, - 0.0, 1.73205081); - - Matrix A24 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - Matrix A26 = Matrix_(2, 2, - -0.577350269, 0.0, - 0.0, -0.577350269); - - expGraph.add(1, A21, 4, A24, 6, A26, zeros(2,1), unit2); - } - - { - Matrix A24 = Matrix_(2, 2, - 1.47196014, 0.0, - 0.0, 1.47196014); - - Matrix A26 = Matrix_(2, 2, - -0.226455407, 0.0, - 0.0, -0.226455407); - - expGraph.add(4, A24, 6, A26, zeros(2,1), unit2); - } - - // Conditional 3 - Matrix A30 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - - Matrix A34 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); - - // Conditional 4 - Matrix A43 = Matrix_(2, 2, - 1.41421356, 0.0, - 0.0, 1.41421356); - Matrix A45 = Matrix_(2, 2, - -0.707106781, 0.0, - 0.0, -0.707106781); - - expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); - - EXPECT(assert_equal(expGraph, actGraph, tol)); - } -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testConditioning.cpp b/gtsam_unstable/linear/tests/testConditioning.cpp deleted file mode 100644 index da8f56117..000000000 --- a/gtsam_unstable/linear/tests/testConditioning.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/** - * @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 -#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); - SharedDiagonal sigmas = noiseModel::Unit::Create(1); - 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); -// SharedDiagonal sigmas1 = noiseModel::Unit::Create(1), sigmas2 = noiseModel::Unit::Create(2); -// -// -// 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; -// VerticalBlockMatrix init_matrices(init_dims, Rinit); -// SharedDiagonal sigmas = noiseModel::Unit::Create(6); -// vector init_keys; init_keys += 0, 1, 2, 3, 4; -// GaussianConditional::shared_ptr initConditional(new -// GaussianConditional(init_keys, 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)); -// -// solution = initConditional->solve(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; -// VerticalBlockMatrix exp_matrices(exp_dims, Rexp); -// SharedDiagonal exp_sigmas = noiseModel::Unit::Create(4); -// vector exp_keys; exp_keys += 0, 2, 4; -// GaussianConditional expSummarized(exp_keys, 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; -// SharedDiagonal sigmas = noiseModel::Unit::Create(10); -// -// VerticalBlockMatrix init_matrices(init_dims, Rinit); -// vector init_keys; init_keys += 3, 4, 5, 6; -// GaussianConditional::shared_ptr initConditional(new -// GaussianConditional(init_keys, 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)); -// -// solution = initConditional->solve(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); -// SharedDiagonal expsigmas = noiseModel::Unit::Create(4); -// -// vector exp_dims; exp_dims += 2, 2, 1; -// VerticalBlockMatrix exp_matrices(exp_dims, Rexp); -// vector exp_keys; exp_keys += 5, 6; -// GaussianConditional expConditional(exp_keys, 2, exp_matrices, expsigmas); -// -// EXPECT(assert_equal(expConditional, *actSummarized, tol)); -//} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */