diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 53dee3dcd..368aae831 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -88,7 +88,7 @@ namespace gtsam { GenericSequentialSolver::jointFactorGraph( const std::vector& js, Eliminate function) const { - // Compute a COLAMD permutation with the marginal variable constrained to the end. + // Compute a COLAMD permutation with the marginal variables constrained to the end. Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); Permutation::shared_ptr permutationInverse(permutation->inverse()); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index f7f1aa00f..52c507374 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -110,11 +110,11 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab return JointMarginal(info, dims, indices); } else { - // Convert keys to linear indices + // Obtain requested variables as ordered indices vector indices(variables.size()); for(size_t i=0; i& variab jointFG = *GaussianSequentialSolver(graph_, true).jointFactorGraph(indices); } - // Conversion from variable keys to position in factor graph variables, + // Build map from variable keys to position in factor graph variables, // which are sorted in index order. Ordering variableConversion; { + // First build map from index to key FastMap usedIndices; for(size_t i=0; i Index_Key; BOOST_FOREACH(const Index_Key& index_key, usedIndices) { @@ -145,8 +147,9 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab // 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(); + BOOST_FOREACH(Key key, variables) { + dims[variableConversion[key]] = values_.at(key).dim(); + } // Get information matrix Matrix augmentedInfo = jointFG.denseHessian(); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index da7157a1e..c9bd43b27 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -180,7 +180,62 @@ TEST(Marginals, planarSLAMmarginals) { EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6)); } +/* ************************************************************************* */ +TEST(Marginals, order) { + NonlinearFactorGraph fg; + fg.add(PriorFactor(0, Pose2(), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3))); + Values vals; + vals.insert(0, Pose2()); + vals.insert(1, Pose2(1,0,0)); + vals.insert(2, Pose2(2,0,0)); + vals.insert(3, Pose2(3,0,0)); + + vals.insert(100, Point2(0,1)); + vals.insert(101, Point2(1,1)); + + fg.add(BearingRangeFactor(0, 100, + vals.at(0).bearing(vals.at(100)), + vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(0, 101, + vals.at(0).bearing(vals.at(101)), + vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(1, 100, + vals.at(1).bearing(vals.at(100)), + vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(1, 101, + vals.at(1).bearing(vals.at(101)), + vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(2, 100, + vals.at(2).bearing(vals.at(100)), + vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(2, 101, + vals.at(2).bearing(vals.at(101)), + vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(3, 100, + vals.at(3).bearing(vals.at(100)), + vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(3, 101, + vals.at(3).bearing(vals.at(101)), + vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2))); + + Marginals marginals(fg, vals); + FastVector keys(fg.keys()); + JointMarginal joint = marginals.jointMarginalCovariance(keys); + + LONGS_EQUAL(3, joint(0,0).rows()); + LONGS_EQUAL(3, joint(1,1).rows()); + LONGS_EQUAL(3, joint(2,2).rows()); + LONGS_EQUAL(3, joint(3,3).rows()); + LONGS_EQUAL(2, joint(100,100).rows()); + LONGS_EQUAL(2, joint(101,101).rows()); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}