diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index 74455c6a9..909566ff1 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -66,8 +66,8 @@ namespace gtsam { KEY parent = this->find(key); return parent == key ? key : findSet(parent); } - // return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed - Self makeUnion(const KEY& key1, const KEY& key2) { return this->add(findSet_(key2), findSet_(key1)); } + // return a ***new*** DSF where x and y are in the same set. No path compression + Self makeUnion(const KEY& key1, const KEY& key2) const { return this->add(findSet(key2), findSet(key1)); } // the in-place version of makeUnion void makeUnionInPlace(const KEY& key1, const KEY& key2) { *this = this->add(findSet_(key2), findSet_(key1)); } diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index a0c4ab714..c8828746b 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -16,13 +16,15 @@ * @brief unit tests for DSF */ -#include +#include + +#include + #include #include using namespace boost::assign; -#include -#include +#include using namespace std; using namespace gtsam; @@ -40,7 +42,7 @@ TEST(DSF, findSet) { dsf = dsf.makeSet(5); dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); - CHECK(dsf.findSet(5) != dsf.findSet(7)); + EXPECT(dsf.findSet(5) != dsf.findSet(7)); } /* ************************************************************************* */ @@ -50,7 +52,7 @@ TEST(DSF, makeUnion) { dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -60,7 +62,7 @@ TEST(DSF, makeUnion2) { dsf = dsf.makeSet(6); dsf = dsf.makeSet(7); dsf = dsf.makeUnion(7,5); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -71,7 +73,7 @@ TEST(DSF, makeUnion3) { dsf = dsf.makeSet(7); dsf = dsf.makeUnion(5,6); dsf = dsf.makeUnion(6,7); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -80,7 +82,7 @@ TEST(DSF, makePair) { dsf = dsf.makePair(0, 1); dsf = dsf.makePair(1, 2); dsf = dsf.makePair(3, 2); - CHECK(dsf.findSet(0) == dsf.findSet(3)); + EXPECT(dsf.findSet(0) == dsf.findSet(3)); } /* ************************************************************************* */ @@ -88,7 +90,7 @@ TEST(DSF, makeList) { DSFInt dsf; list keys; keys += 5, 6, 7; dsf = dsf.makeList(keys); - CHECK(dsf.findSet(5) == dsf.findSet(7)); + EXPECT(dsf.findSet(5) == dsf.findSet(7)); } /* ************************************************************************* */ @@ -111,7 +113,7 @@ TEST(DSF, sets) { LONGS_EQUAL(1, sets.size()); set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -126,7 +128,7 @@ TEST(DSF, sets2) { LONGS_EQUAL(1, sets.size()); set expected; expected += 5, 6, 7; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -140,7 +142,7 @@ TEST(DSF, sets3) { LONGS_EQUAL(2, sets.size()); set expected; expected += 5, 6; - CHECK(expected == sets[dsf.findSet(5)]); + EXPECT(expected == sets[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -155,7 +157,7 @@ TEST(DSF, partition) { LONGS_EQUAL(1, partitions.size()); set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); + EXPECT(expected == partitions[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -171,7 +173,7 @@ TEST(DSF, partition2) { LONGS_EQUAL(1, partitions.size()); set expected; expected += 7; - CHECK(expected == partitions[dsf.findSet(7)]); + EXPECT(expected == partitions[dsf.findSet(7)]); } /* ************************************************************************* */ @@ -187,7 +189,7 @@ TEST(DSF, partition3) { LONGS_EQUAL(2, partitions.size()); set expected; expected += 5; - CHECK(expected == partitions[dsf.findSet(5)]); + EXPECT(expected == partitions[dsf.findSet(5)]); } /* ************************************************************************* */ @@ -201,7 +203,7 @@ TEST(DSF, set) { LONGS_EQUAL(2, set.size()); std::set expected; expected += 5, 6; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ @@ -216,7 +218,7 @@ TEST(DSF, set2) { LONGS_EQUAL(3, set.size()); std::set expected; expected += 5, 6, 7; - CHECK(expected == set); + EXPECT(expected == set); } /* ************************************************************************* */ @@ -234,7 +236,7 @@ TEST(DSF, map) { expected = expected.makeSet(16); expected = expected.makeSet(17); expected = expected.makeUnion(15,16); - CHECK(actual == expected); + EXPECT(actual == expected); } /* ************************************************************************* */ @@ -253,7 +255,7 @@ TEST(DSF, flatten) { expected = expected.makePair(1, 5); expected = expected.makePair(1, 6); expected = expected.makePair(1, 7); - CHECK(actual == expected); + EXPECT(actual == expected); } /* ************************************************************************* */ @@ -265,13 +267,60 @@ TEST(DSF, flatten2) { dsf = dsf.makeUnion(x3,x4); dsf = dsf.makeUnion(x1,x3); - CHECK(dsf != dsf.flatten()); + EXPECT(dsf != dsf.flatten()); DSF expected2; expected2 = expected2.makePair(x1, x2); expected2 = expected2.makePair(x1, x3); expected2 = expected2.makePair(x1, x4); - CHECK(expected2 == dsf.flatten()); + EXPECT(expected2 == dsf.flatten()); +} + +/* ************************************************************************* */ +TEST(DSF, mergePairwiseMatches) { + + // Create some measurements with image index and feature index + typedef pair Measurement; + Measurement m11(1,1),m12(1,2),m14(1,4); // in image 1 + Measurement m22(2,2),m23(2,3),m25(2,5),m26(2,6); // in image 2 + + // Add them all + list measurements; + measurements += m11,m12,m14, m22,m23,m25,m26; + + // Create some "matches" + typedef pair Match; + list matches; + matches += Match(m11,m22), Match(m12,m23), Match(m14,m25), Match(m14,m26); + + // Merge matches + DSF dsf(measurements); + BOOST_FOREACH(const Match& m, matches) + dsf.makeUnionInPlace(m.first,m.second); + + // Check that sets are merged correctly + EXPECT(dsf.findSet(m11)==m11); + EXPECT(dsf.findSet(m12)==m12); + EXPECT(dsf.findSet(m14)==m14); + EXPECT(dsf.findSet(m22)==m11); + EXPECT(dsf.findSet(m23)==m12); + EXPECT(dsf.findSet(m25)==m14); + EXPECT(dsf.findSet(m26)==m14); + + // Check that we have three connected components + EXPECT_LONGS_EQUAL(3, dsf.numSets()); + + set expected1; expected1 += m11,m22; + set actual1 = dsf.set(m11); + EXPECT(expected1 == actual1); + + set expected2; expected2 += m12,m23; + set actual2 = dsf.set(m12); + EXPECT(expected2 == actual2); + + set expected3; expected3 += m14,m25,m26; + set actual3 = dsf.set(m14); + EXPECT(expected3 == actual3); } /* ************************************************************************* */