diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 249891397..542c4d5f1 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -878,11 +878,24 @@ public: optional_matrix_type ... 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()...); } + /** Some optional jacobians omitted function overload */ + template 0) && + (sizeof...(OptionalJacArgs) < + sizeof...(VALUES)), + bool>::type = true> + inline Vector evaluateError(const VALUES&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } + private: /** Pack expansion with index_sequence template pattern */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index eb35bf55d..8ecbbc397 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -418,12 +418,10 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional 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> 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)); } /* ************************************************************************* */