diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 1d0a63e3e..0820b8e32 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -118,6 +118,9 @@ public: return As_.at(key); } + /** erase the A associated with the input key */ + size_t erase_A(const Symbol& key) { return As_.erase(key); } + /** operator[] syntax for get */ inline const Matrix& operator[](const Symbol& name) const { return get_A(name); diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 9d43c6262..f484841ef 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -803,6 +803,24 @@ TEST ( GaussianFactor, exploding_MAST_factor ) { CHECK(true); } +/* ************************************************************************* */ +TEST( GaussianFactor, erase) +{ + Vector b = Vector_(3, 1., 2., 3.); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); + Symbol x0('x',0), x1('x',1); + std::list > terms; + terms.push_back(make_pair(x0, eye(2))); + terms.push_back(make_pair(x1, 2.*eye(2))); + + GaussianFactor actual(terms, b, noise); + int erased = actual.erase_A(x0); + + LONGS_EQUAL(1, erased); + GaussianFactor expected(x1, 2.*eye(2), b, noise); + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}