diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 12855e0c0..07b617354 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -43,6 +43,7 @@ template class BayesTree; class FactorGraph { public: typedef FACTOR FactorType; + typedef typename FACTOR::KeyType KeyType; typedef boost::shared_ptr > shared_ptr; typedef typename boost::shared_ptr sharedFactor; typedef typename std::vector::iterator iterator; @@ -56,6 +57,11 @@ template class BayesTree; /** typedef for an eliminate subroutine */ typedef boost::function&, size_t)> Eliminate; + /** Typedef for the result of factorization */ + typedef std::pair< + boost::shared_ptr, + FactorGraph > FactorizationResult; + protected: /** concept check */ diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 04e8cfbeb..b2629566b 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,7 +89,7 @@ namespace gtsam { const std::vector& js, Eliminate function) const { // Compute a COLAMD permutation with the marginal variable constrained to the end. - Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js)); + Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); Permutation::shared_ptr permutationInverse(permutation->inverse()); // Permute the factors - NOTE that this permutes the original factors, not diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 58d5f93d1..9830782f3 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -31,12 +31,12 @@ namespace gtsam { // Checks for uniqueness of keys Base::assertInvariants(); #ifndef NDEBUG - // Check that separator keys are sorted - FastSet uniquesorted(beginFrontals(), endFrontals()); - assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals())); - // Check that separator keys are less than parent keys - //BOOST_FOREACH(Index j, frontals()) { - // assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); } + // Check that frontal keys are sorted + //FastSet uniquesorted(beginFrontals(), endFrontals()); + //assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals())); + //// Check that separator keys are less than parent keys + ////BOOST_FOREACH(Index j, frontals()) { + //// assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); } #endif } @@ -60,13 +60,13 @@ namespace gtsam { /* ************************************************************************* */ void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { // The permutation may not move the separators into the frontals - #ifndef NDEBUG - BOOST_FOREACH(const KeyType frontal, this->frontals()) { - BOOST_FOREACH(const KeyType separator, this->parents()) { - assert(inversePermutation[frontal] < inversePermutation[separator]); - } - } - #endif +// #ifndef NDEBUG +// BOOST_FOREACH(const KeyType frontal, this->frontals()) { +// BOOST_FOREACH(const KeyType separator, this->parents()) { +// assert(inversePermutation[frontal] < inversePermutation[separator]); +// } +// } +// #endif BOOST_FOREACH(Index& key, keys()) key = inversePermutation[key]; assertInvariants(); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 5cbd8e187..dc3aa669a 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -26,8 +26,6 @@ namespace gtsam { -class Inference; - /** * A permutation reorders variables, for example to reduce fill-in during * elimination. To save computation, the permutation can be applied to @@ -162,8 +160,6 @@ protected: void check(Index variable) const { assert(variable < rangeIndices_.size()); } /// @} - - friend class Inference; }; diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index fc4fde620..878f6a85a 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -26,8 +26,6 @@ namespace gtsam { -class Inference; - /** * The VariableIndex class computes and stores the block column structure of a * factor graph. The factor graph stores a collection of factors, each of diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h new file mode 100644 index 000000000..cf7e13f01 --- /dev/null +++ b/gtsam/inference/inference-inl.h @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file inference-inl.h + * @brief + * @author Richard Roberts + * @date Mar 3, 2012 + */ + +#pragma once + +#include + +// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h +#include + +#include + +namespace gtsam { + +namespace inference { + +/* ************************************************************************* */ +template +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { + + std::vector cmember(variableIndex.size(), 0); + + // If at least some variables are not constrained to be last, constrain the + // ones that should be constrained. + if(constrainLast.size() < variableIndex.size()) { + BOOST_FOREACH(Index var, constrainLast) { + assert(var < variableIndex.size()); + cmember[var] = 1; + } + } + + return PermutationCOLAMD_(variableIndex, cmember); +} + +/* ************************************************************************* */ +inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) { + std::vector cmember(variableIndex.size(), 0); + return PermutationCOLAMD_(variableIndex, cmember); +} + +/* ************************************************************************* */ +template +typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex_) { + + const VariableIndex& variableIndex = variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph); + + // First find the involved factors + Graph involvedFactors; + Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors + + // First get the indices of the involved factors, but uniquely in a set + FastSet involvedFactorIndices; + BOOST_FOREACH(Index variable, variables) { + involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); } + + // Add the factors themselves to involvedFactors and update largest index + involvedFactors.reserve(involvedFactorIndices.size()); + BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) { + const typename Graph::sharedFactor factor = factorGraph[factorIndex]; + involvedFactors.push_back(factor); // Add involved factor + highestInvolvedVariable = std::max( // Updated largest index + highestInvolvedVariable, + *std::max_element(factor->begin(), factor->end())); + } + + // Now permute the variables to be eliminated to the front of the ordering + Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); + Permutation toFrontInverse = *toFront.inverse(); + involvedFactors.permuteWithInverse(toFrontInverse); + + // Eliminate into conditional and remaining factor + typename Graph::EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); + boost::shared_ptr conditional = eliminated.first; + typename Graph::sharedFactor remainingFactor = eliminated.second; + + // Undo the permutation + conditional->permuteWithInverse(toFront); + remainingFactor->permuteWithInverse(toFront); + + // Build the remaining graph, without the removed factors + Graph remainingGraph; + remainingGraph.reserve(factorGraph.size() - involvedFactors.size() + 1); + FastSet::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin(); + for(size_t i = 0; i < factorGraph.size(); ++i) { + if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i) + ++ involvedFactorIndexIt; + else + remainingGraph.push_back(factorGraph[i]); + } + + // Add remaining factor + remainingGraph.push_back(remainingFactor); + + return typename Graph::FactorizationResult(conditional, remainingGraph); + +} + + +} + +} + + diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp index d30ea79c4..e1c250fcf 100644 --- a/gtsam/inference/inference.cpp +++ b/gtsam/inference/inference.cpp @@ -29,7 +29,9 @@ using namespace std; namespace gtsam { -Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { +namespace inference { + +Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */ @@ -79,10 +81,10 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia // Convert elimination ordering in p to an ordering Permutation::shared_ptr permutation(new Permutation(nVars)); for (Index j = 0; j < nVars; j++) { -// if(p[j] == -1) -// permutation->operator[](j) = j; -// else - permutation->operator[](j) = p[j]; + // if(p[j] == -1) + // permutation->operator[](j) = j; + // else + permutation->operator[](j) = p[j]; if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl; } if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl; @@ -91,3 +93,5 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia } } + +} diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index d853ff91b..d87720b5a 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -22,58 +22,59 @@ #include #include +#include #include namespace gtsam { - class Inference { - private: - /* Static members only, private constructor */ - Inference() {} +namespace inference { - public: +/** + * Compute a permutation (variable ordering) using colamd + */ +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex); - /** - * Compute a permutation (variable ordering) using colamd - */ - static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex); +/** + * Compute a permutation (variable ordering) using constrained colamd + */ +template +Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); - /** - * Compute a permutation (variable ordering) using constrained colamd - */ - template - static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); +/** + * Compute a CCOLAMD permutation using the constraint groups in cmember. + */ +Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember); - /** - * Compute a CCOLAMD permutation using the constraint groups in cmember. - */ - static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember); +/** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + */ +template +typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none); - }; +/** Eliminate a single variable, by calling + * eliminate(const Graph&, const std::vector&, const typename Graph::Eliminate&, boost::optional) + */ +template +typename Graph::FactorizationResult eliminateOne(const Graph& factorGraph, typename Graph::KeyType variable, + const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none) { + std::vector variables(1, variable); + return eliminate(factorGraph, variables, eliminateFcn, variableIndex); +} - /* ************************************************************************* */ - template - Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { - - std::vector cmember(variableIndex.size(), 0); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - if(constrainLast.size() < variableIndex.size()) { - BOOST_FOREACH(Index var, constrainLast) { - assert(var < variableIndex.size()); - cmember[var] = 1; - } - } - - return PermutationCOLAMD_(variableIndex, cmember); - } - - /* ************************************************************************* */ - inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) { - std::vector cmember(variableIndex.size(), 0); - return PermutationCOLAMD_(variableIndex, cmember); - } +} } // namespace gtsam + +#include diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp index 697ee9ec2..41c33a456 100644 --- a/gtsam/inference/tests/testInference.cpp +++ b/gtsam/inference/tests/testInference.cpp @@ -25,7 +25,7 @@ using namespace gtsam; /* ************************************************************************* */ -TEST(Inference, UnobservedVariables) { +TEST(inference, UnobservedVariables) { SymbolicFactorGraph sfg; // Create a factor graph that skips some variables @@ -35,7 +35,7 @@ TEST(Inference, UnobservedVariables) { VariableIndex variableIndex(sfg); - Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex)); + Permutation::shared_ptr colamd(inference::PermutationCOLAMD(variableIndex)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index c7224884e..d3da8907e 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -303,7 +303,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, } } } - Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); + Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); toc(3,"ccolamd"); tic(4,"ccolamd permutations"); Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 32a27f6fb..1b8130098 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -238,7 +238,7 @@ boost::shared_ptr > ISAM2::recalculate( if(theta_.size() > constrainedKeysSet.size()) { BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } } - Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember)); + Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); toc(1,"CCOLAMD"); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 647afa2d5..2400ea996 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -78,7 +78,7 @@ namespace gtsam { "orderingCOLAMD: some variables in the graph are not constrained!"); // Compute a fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD( + Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( variableIndex)); // Permute the Ordering with the COLAMD ordering diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index 671d09cc7..fecba4d3c 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -192,96 +192,116 @@ TEST( GaussianFactorGraph, equals ) { // EXPECT(assert_equal(expected,*actual)); //} -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x1 ) -//{ -// Ordering ordering; ordering += kx(1),kl(1),kx(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; -// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x2 ) -//{ -// Ordering ordering; ordering += kx(2),kl(1),kx(1); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// double sig = 0.0894427; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_l1 ) -//{ -// Ordering ordering; ordering += kl(1),kx(1),kx(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); -// -// // create expected Conditional Gaussian -// double sig = sqrt(2)/10.; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; -// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x1 ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x1_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false); -// -// // create expected Conditional Gaussian -// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; -// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_x2_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false); -// -// // create expected Conditional Gaussian -// double sig = 0.0894427; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} -// -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateOne_l1_fast ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false); -// -// // create expected Conditional Gaussian -// double sig = sqrt(2)/10.; -// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; -// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma); -// -// EXPECT(assert_equal(expected,*actual,tol)); -//} + GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR); + + // create expected Conditional Gaussian + Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*result.first,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x2 ) +{ + Ordering ordering; ordering += kx(2),kl(1),kx(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; + + // create expected Conditional Gaussian + double sig = 0.0894427; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_l1 ) +{ + Ordering ordering; ordering += kl(1),kx(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; + + // create expected Conditional Gaussian + double sig = sqrt(2)/10.; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x1_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[kx(1)], EliminateQR); + GaussianConditional::shared_ptr conditional = result.first; + GaussianFactorGraph remaining = result.second; + + // create expected Conditional Gaussian + Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); + GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); + + // Create expected remaining new factor + JacobianFactor expectedFactor(1, Matrix_(4,2, + 4.714045207910318, 0., + 0., 4.714045207910318, + 0., 0., + 0., 0.), + 2, Matrix_(4,2, + -2.357022603955159, 0., + 0., -2.357022603955159, + 7.071067811865475, 0., + 0., 7.071067811865475), + Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), sharedUnit(4)); + + EXPECT(assert_equal(expected,*conditional,tol)); + EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_x2_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kx(2)], EliminateQR).first; + + // create expected Conditional Gaussian + double sig = 0.0894427; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kx(1)],S13,ordering[kl(1)],S12,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminateOne_l1_fast ) +{ + Ordering ordering; ordering += kx(1),kl(1),kx(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first; + + // create expected Conditional Gaussian + double sig = sqrt(2)/10.; + Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); + GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); + + EXPECT(assert_equal(expected,*actual,tol)); +} /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateAll ) @@ -439,7 +459,7 @@ TEST( GaussianFactorGraph, getOrdering) { Ordering original; original += kl(1),kx(1),kx(2); FactorGraph symbolic(createGaussianFactorGraph(original)); - Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic))); + Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering expected; expected += kl(1),kx(2),kx(1); EXPECT(assert_equal(expected,actual)); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index b02d68583..1df8cdff3 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -85,7 +85,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) // Ordering ord; ord += kx(4),kx(3),kx(2),kx(1); // GaussianFactorGraph factors1; // for (int i=0;i<7;i++) factors1.push_back(smoother[i]); -// GaussianISAM actual(*Inference::Eliminate(factors1)); +// GaussianISAM actual(*inference::Eliminate(factors1)); // // // run iSAM with remaining factors // GaussianFactorGraph factors2; @@ -298,7 +298,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) // varIndex.permute(toFront); // BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { // factor->permuteWithInverse(toFrontInverse); } -// GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex); +// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex); // actual.permuteWithInverse(toFront); // EXPECT(assert_equal(expected,actual,tol)); //} diff --git a/tests/testInference.cpp b/tests/testInference.cpp index df8843415..a08358b29 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -30,7 +30,7 @@ using namespace gtsam; /* ************************************************************************* */ /* ************************************************************************* */ -TEST( Inference, marginals ) +TEST( inference, marginals ) { using namespace example; // create and marginalize a small Bayes net on "x" @@ -45,7 +45,7 @@ TEST( Inference, marginals ) } /* ************************************************************************* */ -TEST( Inference, marginals2) +TEST( inference, marginals2) { planarSLAM::Graph fg; SharedDiagonal poseModel(sharedSigma(3, 0.1));