diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index a72e3c259..26d35d7fa 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -24,6 +24,9 @@ #include #include +#include +#include + using namespace std; using namespace gtsam; using namespace boost; @@ -52,7 +55,7 @@ int main(int argc, char** argv) { graph->addConstraint(x2, x3, odom_measurement, odom_model); graph->print("full graph"); - /* 3. Create the data structure to hold the initial estinmate to the solution + /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ shared_ptr initial(new Values); initial->insert(x1, Pose2(0.5, 0.0, 0.2)); @@ -72,6 +75,13 @@ int main(int argc, char** argv) { Values result = *optimizer_result.values(); result.print("final result"); + + Vector mean; Matrix covariance; + boost::tie( mean, covariance) = optimizer_result.marginalStandard(x1); + + print(mean, "Mean"); + print(covariance, "Covariance"); + return 0; } diff --git a/linear/GaussianMultifrontalSolver.cpp b/linear/GaussianMultifrontalSolver.cpp index 852880c44..6086c67cd 100644 --- a/linear/GaussianMultifrontalSolver.cpp +++ b/linear/GaussianMultifrontalSolver.cpp @@ -55,4 +55,11 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { return Base::marginal(j); } +/* ************************************************************************* */ +std::pair GaussianMultifrontalSolver::marginalStandard(Index j) const { + GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); + Matrix R = conditional->get_R(); + return make_pair(conditional->get_d(), inverse(trans(R)*R)); +} + } diff --git a/linear/GaussianMultifrontalSolver.h b/linear/GaussianMultifrontalSolver.h index 56a21c849..496122355 100644 --- a/linear/GaussianMultifrontalSolver.h +++ b/linear/GaussianMultifrontalSolver.h @@ -101,7 +101,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 marginalStandard(Index j) const; }; diff --git a/linear/GaussianSequentialSolver.cpp b/linear/GaussianSequentialSolver.cpp index b357152b0..2a9f8fec2 100644 --- a/linear/GaussianSequentialSolver.cpp +++ b/linear/GaussianSequentialSolver.cpp @@ -72,6 +72,13 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const { return Base::marginal(j); } +std::pair GaussianSequentialSolver::marginalStandard(Index j) const { + GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); + Matrix R = conditional->get_R(); + return make_pair(conditional->get_d(), inverse(trans(R)*R)); +} + + /* ************************************************************************* */ GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector& js) const { return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js))); diff --git a/linear/GaussianSequentialSolver.h b/linear/GaussianSequentialSolver.h index b2c3f72e6..183b4c911 100644 --- a/linear/GaussianSequentialSolver.h +++ b/linear/GaussianSequentialSolver.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -107,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 marginalStandard(Index j) const; /** * Compute the marginal joint over a set of variables, by integrating out diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index 4df619378..de2e0618c 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -158,6 +158,13 @@ namespace gtsam { return values_; } + /** + * Return mean and covariance on a single variable + */ + std::pair marginalStandard(Symbol j) const { + return solver_->marginalStandard((*ordering_)[j]); + } + /** * linearize and optimize * This returns an VectorValues, i.e., vectors in tangent space of T