diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp new file mode 100644 index 000000000..06f9fd6cf --- /dev/null +++ b/gtsam/nonlinear/Marginals.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 Marginals.cpp + * @brief + * @author Richard Roberts + * @date May 14, 2012 + */ + +#include +#include +#include + +namespace gtsam { + +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) { + + // Compute COLAMD ordering + ordering_ = *graph.orderingCOLAMD(solution); + + // Linearize graph + graph_ = *graph.linearize(solution, ordering_); + + // Compute BayesTree + factorization_ = factorization; + if(factorization_ == CHOLESKY) + bayesTree_ = *GaussianMultifrontalSolver(graph_, false).eliminate(); + else if(factorization_ == QR) + bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate(); +} + +Matrix Marginals::marginalCovariance(Key variable) const { + // Get linear key + Index index = ordering_[variable]; + + // Compute marginal + GaussianFactor::shared_ptr marginalFactor; + if(factorization_ == CHOLESKY) + marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholesky); + else if(factorization_ == QR) + marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); + + // Get information matrix (only store upper-right triangle) + Matrix info; + if(typeid(*marginalFactor) == typeid(JacobianFactor)) { + JacobianFactor::constABlock A = static_cast(*marginalFactor).getA(); + info = A.transpose() * A; // Compute A'A + } else if(typeid(*marginalFactor) == typeid(HessianFactor)) { + const HessianFactor& hessian = static_cast(*marginalFactor); + const size_t dim = hessian.getDim(hessian.begin()); + info = hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix + } + + // Compute covariance + return info.inverse(); +} + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h new file mode 100644 index 000000000..48acc007e --- /dev/null +++ b/gtsam/nonlinear/Marginals.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 Marginals.h + * @brief A class for computing marginals in a NonlinearFactorGraph + * @author Richard Roberts + * @date May 14, 2012 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A class for computing marginals in a NonlinearFactorGraph + */ +class Marginals { + +public: + + enum Factorization { + CHOLESKY, + QR + }; + + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + Matrix marginalCovariance(Key variable) const; + +protected: + + GaussianFactorGraph graph_; + Ordering ordering_; + Factorization factorization_; + GaussianBayesTree bayesTree_; + +}; + +} /* namespace gtsam */ diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp new file mode 100644 index 000000000..50b0d3793 --- /dev/null +++ b/tests/testMarginals.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * 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 testMarginals.cpp + * @brief + * @author Richard Roberts + * @date May 14, 2012 + */ + +#include + +// for all nonlinear keys +#include + +// for points and poses +#include +#include + +// for modeling measurement uncertainty - all models included here +#include + +// add in headers for specific factors +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +TEST(Marginals, planarSLAMmarginals) { + + // Taken from PlanarSLAMSelfContained_advanced + + // create keys for variables + Symbol x1('x',1), x2('x',2), x3('x',3); + Symbol l1('l',1), l2('l',2); + + // create graph container and add factors to it + NonlinearFactorGraph graph; + + /* add prior */ + // gaussian for prior + SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin + graph.add(PriorFactor(x1, prior_measurement, prior_model)); // add the factor to the graph + + /* add odometry */ + // general noisemodel for odometry + SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + // create between factors to represent odometry + graph.add(BetweenFactor(x1, x2, odom_measurement, odom_model)); + graph.add(BetweenFactor(x2, x3, odom_measurement, odom_model)); + + /* add measurements */ + // general noisemodel for measurements + SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); + + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = sqrt(4+4), + range21 = 2.0, + range32 = 2.0; + + // create bearing/range factors + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, meas_model)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, meas_model)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, meas_model)); + + // linearization point for marginals + Values soln; + soln.insert(x1, Pose2(0.0, 0.0, 0.0)); + soln.insert(x2, Pose2(2.0, 0.0, 0.0)); + soln.insert(x3, Pose2(4.0, 0.0, 0.0)); + soln.insert(l1, Point2(2.0, 2.0)); + soln.insert(l2, Point2(4.0, 2.0)); + + Matrix expectedx1(3,3); + expectedx1 << + 0.09, -7.1942452e-18, -1.27897692e-17, + -7.1942452e-18, 0.09, 1.27897692e-17, + -1.27897692e-17, 1.27897692e-17, 0.01; + Matrix expectedx2(3,3); + expectedx2 << + 0.120967742, -0.00129032258, 0.00451612903, + -0.00129032258, 0.158387097, 0.0206451613, + 0.00451612903, 0.0206451613, 0.0177419355; + Matrix expectedx3(3,3); + expectedx3 << + 0.160967742, 0.00774193548, 0.00451612903, + 0.00774193548, 0.351935484, 0.0561290323, + 0.00451612903, 0.0561290323, 0.0277419355; + Matrix expectedl1(2,2); + expectedl1 << + 0.168709677, -0.0477419355, + -0.0477419355, 0.163548387; + Matrix expectedl2(2,2); + expectedl2 << + 0.293870968, -0.104516129, + -0.104516129, 0.391935484; + + // Check marginals covariances for all variables (QR mode) + Marginals marginals(graph, soln, Marginals::QR); + EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); + EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); + EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); + EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); + EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); + + // Check marginals covariances for all variables (Cholesky mode) + marginals = Marginals(graph, soln, Marginals::CHOLESKY); + EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); + EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); + EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); + EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); + EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); +} + + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */