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

Solved Scatter bug

release/4.3a0
dellaert 2015-06-21 23:34:11 -07:00
parent 0d2558cbb5
commit c33bb7a89a
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/linear/Scatter.cpp
View File

@ -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());
}
/* ************************************************************************* */

4
gtsam/linear/tests/testScatter.cpp
View File

@ -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);
}