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

optional jacobian overloads backwards compatibility

release/4.3a0
Gerry Chen 2021-12-03 00:39:10 -05:00
parent d9c8ce2721
commit 2aecaf3258
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
2 changed files with 46 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/nonlinear/NonlinearFactor.h
View File

@ -878,11 +878,24 @@ public:
optional_matrix_type<VALUES> ... H) const = 0;
/** No-jacobians requested function overload (since parameter packs can't have
* default args) */
Vector evaluateError(const VALUES&... x) const {
* default args). This specializes the version below to avoid recursive calls
* since this is commonly used. */
inline Vector evaluateError(const VALUES&... x) const {
return evaluateError(x..., optional_matrix_type<VALUES>()...);
}
/** Some optional jacobians omitted function overload */
template <typename... OptionalJacArgs,
typename std::enable_if<(sizeof...(OptionalJacArgs) > 0) &&
(sizeof...(OptionalJacArgs) <
sizeof...(VALUES)),
bool>::type = true>
inline Vector evaluateError(const VALUES&... x,
OptionalJacArgs&&... H) const {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
boost::none);
}
private:
/** Pack expansion with index_sequence template pattern */

37
tests/testNonlinearFactor.cpp
View File

@ -418,12 +418,10 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const override {
if (H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
*H3 = (Matrix(1, 1) << 3.0).finished();
*H4 = (Matrix(1, 1) << 4.0).finished();
}
if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
if (H2) *H2 = (Matrix(1, 1) << 2.0).finished();
if (H3) *H3 = (Matrix(1, 1) << 3.0).finished();
if (H4) *H4 = (Matrix(1, 1) << 4.0).finished();
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
};
@ -448,6 +446,33 @@ TEST(NonlinearFactor, NoiseModelFactorN) {
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2)));
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3)));
EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb()));
// Test all evaluateError argument overloads to ensure backward compatibility
Matrix H1_expected, H2_expected, H3_expected, H4_expected;
Vector e_expected = tf.evaluateError(9, 8, 7, 6, H1_expected, H2_expected,
H3_expected, H4_expected);
std::unique_ptr<NoiseModelFactorN<double, double, double, double>> base_ptr(
new TestFactorN(tf));
Matrix H1, H2, H3, H4;
EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6)));
EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6, H1)));
EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(e_expected, //
base_ptr->evaluateError(9, 8, 7, 6, H1, H2)));
EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(H2_expected, H2));
EXPECT(assert_equal(e_expected,
base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3)));
EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(H2_expected, H2));
EXPECT(assert_equal(H3_expected, H3));
EXPECT(assert_equal(e_expected,
base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4)));
EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(H2_expected, H2));
EXPECT(assert_equal(H3_expected, H3));
EXPECT(assert_equal(H4_expected, H4));
}
/* ************************************************************************* */