diff --git a/.cproject b/.cproject index c81094465..e65c713f6 100644 --- a/.cproject +++ b/.cproject @@ -844,14 +844,30 @@ true true - + make - -j5 + -j2 + install + true + true + true + + + make + -j2 check true true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1028,6 +1044,14 @@ true true + + make + -j2 + SimpleRotation.run + true + true + true + make -j2 @@ -1635,14 +1659,6 @@ true true - - make - -j5 - testVector.run - true - true - true - make -j2 @@ -1803,14 +1819,6 @@ true true - - make - -j5 - UGM_small.run - true - true - true - make -j2 @@ -2122,110 +2130,26 @@ true true - + make - -j5 - wrap_gtsam + -j8 + testMarginals.run true true true - - make - VERBOSE=1 - wrap_gtsam - true - false - true - - - cpack - -G DEB - true - false - true - - - cpack - -G RPM - true - false - true - - - cpack - -G TGZ - true - false - true - - - cpack - --config CPackSourceConfig.cmake - true - false - true - - + make -j5 - check.discrete + wrap.testSpirit.run true true true - + make -j5 - wrap_gtsam_unstable - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - check.dynamics_unstable - true - true - true - - - make - -j5 - check.slam_unstable - true - true - true - - - make - -j5 - check.base_unstable - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run + wrap.testWrap.run true true true diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 070fa89c9..34d620bc2 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -17,11 +17,13 @@ */ #include +#include #include #include namespace gtsam { +/* ************************************************************************* */ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) { // Compute COLAMD ordering @@ -30,6 +32,9 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, // Linearize graph graph_ = *graph.linearize(solution, ordering_); + // Store values + values_ = solution; + // Compute BayesTree factorization_ = factorization; if(factorization_ == CHOLESKY) @@ -38,10 +43,12 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate(); } +/* ************************************************************************* */ Matrix Marginals::marginalCovariance(Key variable) const { return marginalInformation(variable).inverse(); } +/* ************************************************************************* */ Matrix Marginals::marginalInformation(Key variable) const { // Get linear key Index index = ordering_[variable]; @@ -66,4 +73,71 @@ Matrix Marginals::marginalInformation(Key variable) const { } } +/* ************************************************************************* */ +JointMarginal Marginals::jointMarginalCovariance(const std::vector& variables) const { + JointMarginal info = jointMarginalInformation(variables); + info.fullMatrix_ = info.fullMatrix_.inverse(); + return info; +} + +/* ************************************************************************* */ +JointMarginal Marginals::jointMarginalInformation(const std::vector& variables) const { + + // If 2 variables, we can use the BayesTree::joint function, otherwise we + // have to use sequential elimination. + if(variables.size() == 1) { + Matrix info = marginalInformation(variables.front()); + std::vector dims; + dims.push_back(info.rows()); + Ordering indices; + indices.insert(variables.front(), 0); + return JointMarginal(info, dims, indices); + + } else { + // Convert keys to linear indices + vector indices(variables.size()); + for(size_t i=0; i usedIndices; + for(size_t i=0; i Index_Key; + BOOST_FOREACH(const Index_Key& index_key, usedIndices) { + variableConversion.insert(index_key.second, slot); + ++ slot; + } + } + + // Get dimensions from factor graph + std::vector dims(indices.size(), 0); + for(size_t i = 0; i < variables.size(); ++i) + dims[i] = values_.at(variables[i]).dim(); + + // Get information matrix + Matrix augmentedInfo = jointFG.denseHessian(); + Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); + + return JointMarginal(info, dims, variableConversion); + } +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index dde199ca0..77bccdfd2 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -18,12 +18,15 @@ #pragma once -#include +#include #include +#include #include namespace gtsam { +class JointMarginal; + /** * A class for computing Gaussian marginals of variables in a NonlinearFactorGraph */ @@ -52,13 +55,62 @@ public: * matrix. */ Matrix marginalInformation(Key variable) const; + /** Compute the joint marginal covariance of several variables */ + JointMarginal jointMarginalCovariance(const std::vector& variables) const; + + /** Compute the joint marginal information of several variables */ + JointMarginal jointMarginalInformation(const std::vector& variables) const; + protected: GaussianFactorGraph graph_; + Values values_; Ordering ordering_; Factorization factorization_; GaussianBayesTree bayesTree_; +}; +/** + * A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation + */ +class JointMarginal { + +protected: + typedef SymmetricBlockView BlockView; + +public: + /** A block view of the joint marginal - this stores a reference to the + * JointMarginal object, so the JointMarginal object must be kept in scope + * while this block view is needed, otherwise assign this block object to a + * Matrix to store it. + */ + typedef BlockView::constBlock Block; + + /** Access a block, corresponding to a pair of variables, of the joint + * marginal. Each block is accessed by its "vertical position", + * corresponding to the variable with nonlinear Key \c iVariable and + * "horizontal position", corresponding to the variable with nonlinear Key + * \c jVariable. + * + * For example, if we have the joint marginal on a 2D pose "x3" and a 2D + * landmark "l2", then jointMarginal(Symbol('x',3), Symbol('l',2)) will + * return the 3x2 block of the joint covariance matrix corresponding to x3 + * and l2. + * @param iVariable The nonlinear Key specifying the "vertical position" of the requested block + * @param jVariable The nonlinear Key specifying the "horizontal position" of the requested block + */ + Block operator()(Key iVariable, Key jVariable) const { + return blockView_(indices_[iVariable], indices_[jVariable]); } + +protected: + Matrix fullMatrix_; + BlockView blockView_; + Ordering indices_; + + JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const Ordering& indices) : + fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {} + + friend class Marginals; }; } /* namespace gtsam */ diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 50b0d3793..a91e6b576 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -112,21 +112,72 @@ TEST(Marginals, planarSLAMmarginals) { 0.293870968, -0.104516129, -0.104516129, 0.391935484; - // Check marginals covariances for all variables (QR mode) - Marginals marginals(graph, soln, Marginals::QR); + // 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)); - // Check marginals covariances for all variables (Cholesky mode) - marginals = Marginals(graph, soln, Marginals::CHOLESKY); + // 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 joint marginals for 3 variables + Matrix expected_l2x1x3(8,8); + expected_l2x1x3 << + 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, 0.151935669757191, -0.104516127560770, -0.050967744878460, + -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, + 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.090000180000270, 0.000000000000000, 0.000000000000000, + -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, -0.000000000000000, 0.090000180000270, 0.000000000000000, + -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.000000000000000, 0.040000000000000, 0.010000000000000, + 0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770, + -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, + -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615; + vector variables(3); + variables[0] = l2; + variables[1] = x1; + variables[2] = x3; + JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6)); + + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6)); + + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6)); + + // Check joint marginals for 2 variables (different code path than >2 variable case above) + Matrix expected_l2x1(5,5); + expected_l2x1 << + 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, + -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, + 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, + -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, + -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000; + variables.resize(2); + variables[0] = l2; + variables[1] = x1; + JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6)); + + // Check joint marginal for 1 variable (different code path than >1 variable cases above) + variables.resize(1); + variables[0] = x1; + JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6)); }