diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index ec227298c..7b7738250 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -103,23 +103,27 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR); } else { if(factorization_ == CHOLESKY) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(Ordering(variables), boost::none, EliminatePreferCholesky)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky)); else if(factorization_ == QR) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(Ordering(variables), boost::none, EliminateQR)); - } - - // Get dimensions from factor graph - std::vector dims; - dims.reserve(variables.size()); - BOOST_FOREACH(Key key, variables) { - dims.push_back(values_.at(key).dim()); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR)); } // Get information matrix Matrix augmentedInfo = jointFG.augmentedHessian(); Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); - return JointMarginal(info, dims, variables); + // Information matrix will be returned with sorted keys + std::vector variablesSorted = variables; + std::sort(variablesSorted.begin(), variablesSorted.end()); + + // Get dimensions from factor graph + std::vector dims; + dims.reserve(variablesSorted.size()); + BOOST_FOREACH(Key key, variablesSorted) { + dims.push_back(values_.at(key).dim()); + } + + return JointMarginal(info, dims, variablesSorted); } } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 7f793cf4f..4702909f5 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -140,8 +140,8 @@ TEST(Marginals, planarSLAMmarginals) { -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[0] = x1; + variables[1] = l2; 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));