diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index dde83712a..ed2af529f 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -46,8 +46,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, } } - iterator first = end(); // remember position - // Now, find dimensions of variables and/or extend BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { if (!factor) continue; @@ -68,11 +66,13 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, } } + // To keep the same behavior as before, sort the keys after the ordering + iterator first = begin(); + if (ordering) first += ordering->size(); + if (first != end()) std::sort(first, end()); + // Filter out keys with zero dimensions (if ordering had more keys) erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); - - // To keep the same behavior as before, sort the keys after the ordering - if (first != end()) std::sort(begin(), end()); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp index 465763282..575f29c26 100644 --- a/gtsam/linear/tests/testScatter.cpp +++ b/gtsam/linear/tests/testScatter.cpp @@ -51,8 +51,8 @@ TEST(HessianFactor, CombineAndEliminate) { Scatter scatter(gfg); EXPECT_LONGS_EQUAL(2, scatter.size()); - EXPECT(assert_equal(X(1), scatter.at(0).key)); - EXPECT(assert_equal(X(0), scatter.at(1).key)); + EXPECT(assert_equal(X(0), scatter.at(0).key)); + EXPECT(assert_equal(X(1), scatter.at(1).key)); EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); }