diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index acd325194..f113a4f64 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -60,12 +61,25 @@ public: /// Given coefficients c, predict value for x double operator()(const Coefficients& c, boost::optional H) { + if (H) + (*H) = H_; return H_ * c; } }; //****************************************************************************** TEST(BasisDecompositions, Fourier) { + Fourier<3> fx(0); + Eigen::Matrix expectedH, actualH; + Vector3 c(1.5661, 1.2717, 1.2717); + expectedH = numericalDerivative11( + boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); + EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); + EXPECT(assert_equal((Matrix)expectedH, actualH)); +} + +//****************************************************************************** +TEST(BasisDecompositions, FourierExpression) { // Create linear factor graph GaussianFactorGraph g; @@ -93,7 +107,7 @@ TEST(BasisDecompositions, Fourier) { // Check Vector3 expected(1.5661, 1.2717, 1.2717); - CHECK(assert_equal((Vector) expected, actual.at(key),1e-4)); + EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); } //******************************************************************************