From 95add4786a58b7bba9d1d5b4aee44f6d0c9b37cc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Oct 2023 08:57:38 -0400 Subject: [PATCH] overload JacobianFactor::getA method with one that takes a key --- gtsam/linear/JacobianFactor.h | 6 ++++++ gtsam/linear/tests/testJacobianFactor.cpp | 9 +++++++++ 2 files changed, 15 insertions(+) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 375e82698..407ed1e27 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -312,6 +312,12 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** + * Get a view of the A matrix for the variable + * pointed to by the given key. + */ + ABlock getA(const Key& key) { return Ab_(find(key) - begin()); } + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). * @param scatter A mapping from variable index to slot index in this HessianFactor diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 0ad77b366..234466620 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -65,7 +65,10 @@ TEST(JacobianFactor, constructors_and_accessors) JacobianFactor actual(terms[0].first, terms[0].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[0].second, actual.getA(terms[0].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model()); @@ -78,7 +81,10 @@ TEST(JacobianFactor, constructors_and_accessors) terms[1].first, terms[1].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[1].second, actual.getA(terms[1].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model()); @@ -91,7 +97,10 @@ TEST(JacobianFactor, constructors_and_accessors) terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[2].second, actual.getA(terms[2].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model());