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

Added generic re-key function to nonlinear factor

release/4.3a0
Alex Cunningham 2012-05-21 20:54:42 +00:00
parent 54f6f3629c
commit 2bd7a0ed43
2 changed files with 34 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/nonlinear/NonlinearFactor.h
View File

@ -157,11 +157,20 @@ public:
/**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
* for subclasses
*
* Default implementation will slice the factor
*/
virtual shared_ptr clone() const =0;
/**
* Clones a factor and replaces its keys
*/
shared_ptr rekey(const std::vector<Key>& new_keys) const {
assert(new_keys.size() == this->keys().size());
shared_ptr new_factor = clone();
new_factor->keys() = new_keys;
return new_factor;
}
}; // \class NonlinearFactor
/* ************************************************************************* */

24
tests/testNonlinearFactor.cpp
View File

@ -392,7 +392,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
}
/* ************************************************************************* */
TEST( NonlinearFactor, clone )
TEST( NonlinearFactor, clone_rekey )
{
shared_nlf init(new TestFactor4());
EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
@ -400,9 +400,31 @@ TEST( NonlinearFactor, clone )
EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
// Standard clone
shared_nlf actClone = init->clone();
EXPECT(actClone.get() != init.get()); // Ensure different pointers
EXPECT(assert_equal(*init, *actClone));
// Re-key factor - clones with different keys
std::vector<Key> new_keys(4);
new_keys[0] = PoseKey(5);
new_keys[1] = PoseKey(6);
new_keys[2] = PoseKey(7);
new_keys[3] = PoseKey(8);
shared_nlf actRekey = init->rekey(new_keys);
EXPECT(actRekey.get() != init.get()); // Ensure different pointers
// Ensure init is unchanged
EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
// Check new keys
EXPECT_LONGS_EQUAL(PoseKey(5), actRekey->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(6), actRekey->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(7), actRekey->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(8), actRekey->keys()[3]);
}
/* ************************************************************************* */