12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed bug in marginals introduced with unordered and modified a unit test to catch it.

release/4.3a0
Richard Roberts 2013-08-26 23:05:17 +00:00
parent f779736380
commit 06548ec8a9
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/nonlinear/Marginals.cpp
View File

@ -103,23 +103,27 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR); jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR);
} else { } else {
if(factorization_ == CHOLESKY) if(factorization_ == CHOLESKY)
jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(Ordering(variables), boost::none, EliminatePreferCholesky)); jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky));
else if(factorization_ == QR) else if(factorization_ == QR)
jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(Ordering(variables), boost::none, EliminateQR)); jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR));
}
// Get dimensions from factor graph
std::vector<size_t> dims;
dims.reserve(variables.size());
BOOST_FOREACH(Key key, variables) {
dims.push_back(values_.at(key).dim());
} }
// Get information matrix // Get information matrix
Matrix augmentedInfo = jointFG.augmentedHessian(); Matrix augmentedInfo = jointFG.augmentedHessian();
Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); 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<Key> variablesSorted = variables;
std::sort(variablesSorted.begin(), variablesSorted.end());
// Get dimensions from factor graph
std::vector<size_t> dims;
dims.reserve(variablesSorted.size());
BOOST_FOREACH(Key key, variablesSorted) {
dims.push_back(values_.at(key).dim());
}
return JointMarginal(info, dims, variablesSorted);
} }
} }

4
tests/testMarginals.cpp
View File

@ -140,8 +140,8 @@ TEST(Marginals, planarSLAMmarginals) {
-0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, -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; -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615;
vector<Key> variables(3); vector<Key> variables(3);
variables[0] = l2; variables[0] = x1;
variables[1] = x1; variables[1] = l2;
variables[2] = x3; variables[2] = x3;
JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables); 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(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));