diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 3d1a12f19..648bcd510 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -29,6 +29,9 @@ namespace gtsam { * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2 + * + * Example, degree 8 Chebyshev polynomial measured at x=0.5: + * EvaluationFactor factor(key, measured, model, 8, 0.5); */ template class EvaluationFactor : public FunctorizedFactor { @@ -47,7 +50,7 @@ class EvaluationFactor : public FunctorizedFactor { * @param N The degree of the polynomial. * @param x The point at which to evaluate the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} @@ -62,7 +65,7 @@ class EvaluationFactor : public FunctorizedFactor { * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index b0e1e91cb..ec3c53152 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -40,45 +40,51 @@ using gtsam::LevenbergMarquardtOptimizer; using gtsam::NonlinearFactorGraph; using gtsam::NonlinearOptimizerParams; -const size_t N = 2; +constexpr size_t N = 2; -// Key for FunctorizedFactor -gtsam::Key key = gtsam::Symbol('X', 0); +// Key used in all tests +const gtsam::Symbol key('X', 0); //****************************************************************************** -TEST(FunctorizedFactor, Print2) { - using gtsam::VectorEvaluationFactor; - const size_t M = 1; +TEST(BasisFactors, EvaluationFactor) { + using gtsam::EvaluationFactor; - Vector measured = Vector::Ones(M) * 42; + double measured = 0; - auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + auto model = Isotropic::Sigma(1, 1.0); + EvaluationFactor factor(key, measured, model, N, 0); - std::string expected = - " keys = { X0 }\n" - " noise model: unit (1) \n" - "FunctorizedFactor(X0)\n" - " measurement: [\n" - " 42\n" - "]\n" - " noise model sigmas: 1\n"; + NonlinearFactorGraph graph; + graph.add(factor); - EXPECT(assert_print_equal(expected, priorFactor)); + Vector functionValues(N); + functionValues.setZero(); + + Values initial; + initial.insert(key, functionValues); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); } //****************************************************************************** -TEST(FunctorizedFactor, VectorEvaluationFactor) { +TEST(BasisFactors, VectorEvaluationFactor) { using gtsam::VectorEvaluationFactor; const size_t M = 4; - Vector measured = Vector::Zero(M); + const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, N, 0); NonlinearFactorGraph graph; - graph.add(priorFactor); + graph.add(factor); ParameterMatrix stateMatrix(N); @@ -96,7 +102,29 @@ TEST(FunctorizedFactor, VectorEvaluationFactor) { } //****************************************************************************** -TEST(FunctorizedFactor, VectorComponentFactor) { +TEST(BasisFactors, Print) { + using gtsam::VectorEvaluationFactor; + const size_t M = 1; + + const Vector measured = Vector::Ones(M) * 42; + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor factor(key, measured, model, N, 0); + + std::string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, factor)); +} + +//****************************************************************************** +TEST(BasisFactors, VectorComponentFactor) { using gtsam::VectorComponentFactor; const int P = 4; const size_t i = 2; @@ -124,11 +152,11 @@ TEST(FunctorizedFactor, VectorComponentFactor) { } //****************************************************************************** -TEST(FunctorizedFactor, VecDerivativePrior) { +TEST(BasisFactors, VecDerivativePrior) { using gtsam::VectorDerivativeFactor; const size_t M = 4; - Vector measured = Vector::Zero(M); + const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); @@ -151,7 +179,7 @@ TEST(FunctorizedFactor, VecDerivativePrior) { } //****************************************************************************** -TEST(FunctorizedFactor, ComponentDerivativeFactor) { +TEST(BasisFactors, ComponentDerivativeFactor) { using gtsam::ComponentDerivativeFactor; const size_t M = 4;