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

Merge pull request #1656 from borglab/jacobian-factor-getA

release/4.3a0
Varun Agrawal 2023-10-12 16:50:25 -04:00 committed by GitHub
parent 244c9b654b 95add4786a
commit caef0de75a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/JacobianFactor.h
View File

@ -312,6 +312,12 @@ namespace gtsam {
/** Get a view of the A matrix */ /** Get a view of the A matrix */
ABlock getA() { return Ab_.range(0, size()); } 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 /** Update an information matrix by adding the information corresponding to this factor
* (used internally during elimination). * (used internally during elimination).
* @param scatter A mapping from variable index to slot index in this HessianFactor * @param scatter A mapping from variable index to slot index in this HessianFactor

9
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -65,7 +65,10 @@ TEST(JacobianFactor, constructors_and_accessors)
JacobianFactor actual(terms[0].first, terms[0].second, b, noise); JacobianFactor actual(terms[0].first, terms[0].second, b, noise);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
// Key iterator
EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); 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, expected.getb()));
EXPECT(assert_equal(b, actual.getb())); EXPECT(assert_equal(b, actual.getb()));
EXPECT(noise == expected.get_model()); EXPECT(noise == expected.get_model());
@ -78,7 +81,10 @@ TEST(JacobianFactor, constructors_and_accessors)
terms[1].first, terms[1].second, b, noise); terms[1].first, terms[1].second, b, noise);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
// Key iterator
EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); 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, expected.getb()));
EXPECT(assert_equal(b, actual.getb())); EXPECT(assert_equal(b, actual.getb()));
EXPECT(noise == expected.get_model()); 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); terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
// Key iterator
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); 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, expected.getb()));
EXPECT(assert_equal(b, actual.getb())); EXPECT(assert_equal(b, actual.getb()));
EXPECT(noise == expected.get_model()); EXPECT(noise == expected.get_model());