diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index f62d2f4cf..513aac98d 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -77,8 +77,8 @@ int main(int argc, char** argv) { result.print("final result"); /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalStandard(x1); - Matrix covariance2 = optimizer_result.marginalStandard(x2); + Matrix covariance1 = optimizer_result.marginalCovariance(x1); + Matrix covariance2 = optimizer_result.marginalCovariance(x2); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index f0156e0ff..5eeb619e3 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -364,7 +364,7 @@ namespace gtsam { // Find marginal on the keys we are interested in FactorGraph p_FSRfg(p_FSR); - return *GenericSequentialSolver(p_FSR).joint(keys()); + return *GenericSequentialSolver(p_FSR).jointFactorGraph(keys()); } /* ************************************************************************* */ @@ -392,7 +392,7 @@ namespace gtsam { // Calculate the marginal vector keys12vector; keys12vector.reserve(keys12.size()); keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); - return *GenericSequentialSolver(joint).joint(keys12vector); + return *GenericSequentialSolver(joint).jointFactorGraph(keys12vector); } /* ************************************************************************* */ @@ -706,7 +706,7 @@ namespace gtsam { // calculate or retrieve its marginal FactorGraph cliqueMarginal = clique->marginal(root_); - return GenericSequentialSolver(cliqueMarginal).marginal(key); + return GenericSequentialSolver(cliqueMarginal).marginalFactor(key); } /* ************************************************************************* */ @@ -736,7 +736,7 @@ namespace gtsam { // eliminate remaining factor graph to get requested joint vector key12(2); key12[0] = key1; key12[1] = key2; - return GenericSequentialSolver(p_C1C2).joint(key12); + return GenericSequentialSolver(p_C1C2).jointFactorGraph(key12); } /* ************************************************************************* */ diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index c2c12c60c..b60ed6d18 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -47,7 +47,7 @@ GenericMultifrontalSolver::eliminate() const { /* ************************************************************************* */ template typename FactorGraph::shared_ptr -GenericMultifrontalSolver::joint(const std::vector& js) const { +GenericMultifrontalSolver::jointFactorGraph(const std::vector& js) const { // We currently have code written only for computing the @@ -63,7 +63,7 @@ GenericMultifrontalSolver::joint(const std::vector& /* ************************************************************************* */ template -typename FACTOR::shared_ptr GenericMultifrontalSolver::marginal(Index j) const { +typename FACTOR::shared_ptr GenericMultifrontalSolver::marginalFactor(Index j) const { return eliminate()->marginal(j); } diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h index 91ed7fd23..e68505a76 100644 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ b/gtsam/inference/GenericMultifrontalSolver.h @@ -53,13 +53,13 @@ public: * all of the other variables. This function returns the result as a factor * graph. */ - typename FactorGraph::shared_ptr joint(const std::vector& js) const; + typename FactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; /** - * Compute the marginal Gaussian density over a variable, by integrating out + * Compute the marginal density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginal(Index j) const; + typename FACTOR::shared_ptr marginalFactor(Index j) const; }; diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index c31ec480a..cdd67041f 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -44,7 +44,7 @@ typename BayesNet::shared_ptr GenericSequentialSol /* ************************************************************************* */ template -typename FactorGraph::shared_ptr GenericSequentialSolver::joint(const std::vector& js) const { +typename FactorGraph::shared_ptr GenericSequentialSolver::jointFactorGraph(const std::vector& js) const { // Compute a COLAMD permutation with the marginal variable constrained to the end. Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js)); @@ -84,12 +84,12 @@ typename FactorGraph::shared_ptr GenericSequentialSolver::joint( /* ************************************************************************* */ template -typename FACTOR::shared_ptr GenericSequentialSolver::marginal(Index j) const { +typename FACTOR::shared_ptr GenericSequentialSolver::marginalFactor(Index j) const { // Create a container for the one variable index vector js(1); js[0] = j; // Call joint and return the only factor in the factor graph it returns - return (*this->joint(js))[0]; + return (*this->jointFactorGraph(js))[0]; } } diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 4369d449f..c606013a6 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -59,13 +59,13 @@ public: * all of the other variables. This function returns the result as a factor * graph. */ - typename FactorGraph::shared_ptr joint(const std::vector& js) const; + typename FactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginal(Index j) const; + typename FACTOR::shared_ptr marginalFactor(Index j) const; }; diff --git a/gtsam/inference/SymbolicSequentialSolver.cpp b/gtsam/inference/SymbolicSequentialSolver.cpp index dbe98c233..17fc9cc4c 100644 --- a/gtsam/inference/SymbolicSequentialSolver.cpp +++ b/gtsam/inference/SymbolicSequentialSolver.cpp @@ -24,18 +24,4 @@ namespace gtsam { // An explicit instantiation to be compiled into the library template class GenericSequentialSolver; -///* ************************************************************************* */ -//SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph& factorGraph) : -// Base(factorGraph) {} -// -///* ************************************************************************* */ -//BayesNet::shared_ptr SymbolicSequentialSolver::eliminate() const { -// return Base::eliminate(); -//} -// -///* ************************************************************************* */ -//SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector& js) const { -// return Base::joint(js); -//} - } diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h index 42ac592b1..84896d0db 100644 --- a/gtsam/inference/SymbolicSequentialSolver.h +++ b/gtsam/inference/SymbolicSequentialSolver.h @@ -25,37 +25,5 @@ namespace gtsam { // The base class provides all of the needed functionality typedef GenericSequentialSolver SymbolicSequentialSolver; -//class SymbolicSequentialSolver : GenericSequentialSolver { -// -//protected: -// -// typedef GenericSequentialSolver Base; -// -//public: -// -// SymbolicSequentialSolver(const FactorGraph& factorGraph); -// -// /** -// * Eliminate the factor graph sequentially. Uses a column elimination tree -// * to recursively eliminate. -// */ -// BayesNet::shared_ptr eliminate() const; -// -// /** -// * Compute the marginal Gaussian density over a variable, by integrating out -// * all of the other variables. This function returns the result as a factor. -// */ -// IndexFactor::shared_ptr marginal(Index j) const; -// -// /** -// * Compute the marginal joint over a set of variables, by integrating out -// * all of the other variables. This function returns the result as an upper- -// * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with -// * R*x = d. To get a mean and covariance matrix, use jointStandard(...) -// */ -// SymbolicFactorGraph::shared_ptr joint(const std::vector& js) const; -// -//}; - } diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp index ea803463a..163e4de8a 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ b/gtsam/linear/GaussianMultifrontalSolver.cpp @@ -55,13 +55,18 @@ VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { } /* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { - return Base::marginal(j); +GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector& js) const { + return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js))); } /* ************************************************************************* */ -std::pair GaussianMultifrontalSolver::marginalStandard(Index j) const { - GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); +GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const { + return Base::marginalFactor(j); +} + +/* ************************************************************************* */ +std::pair GaussianMultifrontalSolver::marginalCovariance(Index j) const { + GaussianConditional::shared_ptr conditional = Base::marginalFactor(j)->eliminateFirst(); Matrix R = conditional->get_R(); return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); } diff --git a/gtsam/linear/GaussianMultifrontalSolver.h b/gtsam/linear/GaussianMultifrontalSolver.h index 496122355..1fb010a9e 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.h +++ b/gtsam/linear/GaussianMultifrontalSolver.h @@ -86,13 +86,20 @@ public: */ VectorValues::shared_ptr optimize() const; + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a factor + * graph. + */ + GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; + /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as an upper- * triangular R factor and right-hand-side, i.e. a GaussianConditional with * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) */ - GaussianFactor::shared_ptr marginal(Index j) const; + GaussianFactor::shared_ptr marginalFactor(Index j) const; /** * Compute the marginal Gaussian density over a variable, by integrating out @@ -101,7 +108,7 @@ public: * returns a GaussianConditional, this function back-substitutes the R factor * to obtain the mean, then computes \Sigma = (R^T * R)^-1. */ - std::pair marginalStandard(Index j) const; + std::pair marginalCovariance(Index j) const; }; diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index 0ef8151b1..a77e1e28b 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -72,20 +72,19 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const { } /* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const { - return Base::marginal(j); +GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const { + return Base::marginalFactor(j); } -std::pair GaussianSequentialSolver::marginalStandard(Index j) const { - GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); +std::pair GaussianSequentialSolver::marginalCovariance(Index j) const { + GaussianConditional::shared_ptr conditional = Base::marginalFactor(j)->eliminateFirst(); Matrix R = conditional->get_R(); return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); } - /* ************************************************************************* */ -GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector& js) const { - return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js))); +GaussianFactorGraph::shared_ptr GaussianSequentialSolver::jointFactorGraph(const std::vector& js) const { + return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js))); } } diff --git a/gtsam/linear/GaussianSequentialSolver.h b/gtsam/linear/GaussianSequentialSolver.h index 183b4c911..06e101f7e 100644 --- a/gtsam/linear/GaussianSequentialSolver.h +++ b/gtsam/linear/GaussianSequentialSolver.h @@ -99,7 +99,7 @@ public: * triangular R factor and right-hand-side, i.e. a GaussianConditional with * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) */ - GaussianFactor::shared_ptr marginal(Index j) const; + GaussianFactor::shared_ptr marginalFactor(Index j) const; /** * Compute the marginal Gaussian density over a variable, by integrating out @@ -108,7 +108,7 @@ public: * returns a GaussianConditional, this function back-substitutes the R factor * to obtain the mean, then computes \Sigma = (R^T * R)^-1. */ - std::pair marginalStandard(Index j) const; + std::pair marginalCovariance(Index j) const; /** * Compute the marginal joint over a set of variables, by integrating out @@ -116,18 +116,8 @@ public: * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with * R*x = d. To get a mean and covariance matrix, use jointStandard(...) */ - GaussianFactorGraph::shared_ptr joint(const std::vector& js) const; + GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const; - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a mean - * vector and covariance matrix. The variables will be ordered in the - * return values as they are ordered in the 'js' argument, not as they are - * ordered in the original factor graph. Compared to jointCanonical, which - * returns a GaussianBayesNet, this function back-substitutes the BayesNet to - * obtain the mean, then computes \Sigma = (R^T * R)^-1. - */ -// std::pair jointStandard(const std::vector& js) const; }; } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 153224801..c0df0c305 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -209,8 +209,8 @@ namespace gtsam { /** * Return mean and covariance on a single variable */ - Matrix marginalStandard(Symbol j) const { - return solver_->marginalStandard((*ordering_)[j]).second; + Matrix marginalCovariance(Symbol j) const { + return solver_->marginalCovariance((*ordering_)[j]).second; } /** diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 5ee0ab6f0..b355a094d 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -352,8 +352,8 @@ TEST(BayesTree, simpleMarginal) gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0)); gfg.add(1, -eye(2), 2, A12, ones(2), sharedSigma(2, 1.0)); - Matrix expected(GaussianSequentialSolver(gfg).marginalStandard(2).second); - Matrix actual(GaussianMultifrontalSolver(gfg).marginalStandard(2).second); + Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2).second); + Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2).second); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 233049b01..0f2ad11c0 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -42,8 +42,8 @@ TEST(GaussianFactorGraph, createSmoother) // eliminate vector x3var; x3var.push_back(ordering["x3"]); vector x1var; x1var.push_back(ordering["x1"]); - GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x3var)).eliminate(); - GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x1var)).eliminate(); + GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); + GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate(); CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } @@ -53,7 +53,7 @@ TEST( Inference, marginals ) // create and marginalize a small Bayes net on "x" GaussianBayesNet cbn = createSmallGaussianBayesNet(); vector xvar; xvar.push_back(0); - GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).joint(xvar)).eliminate(); + GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate(); // expected is just scalar Gaussian on x GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2));