From 73c950e69a716c0042622388a48a6f02fb31c767 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 08:59:01 -0400 Subject: [PATCH] remove templating and make all Basis code dynamic --- gtsam/basis/Basis.cpp | 33 ++++++++ gtsam/basis/Basis.h | 102 ++++++++++++------------- gtsam/basis/BasisFactors.h | 82 ++++++++++---------- gtsam/basis/tests/testBasisFactors.cpp | 14 ++-- gtsam/basis/tests/testChebyshev2.cpp | 18 ++--- gtsam/basis/tests/testFourier.cpp | 4 +- 6 files changed, 141 insertions(+), 112 deletions(-) create mode 100644 gtsam/basis/Basis.cpp diff --git a/gtsam/basis/Basis.cpp b/gtsam/basis/Basis.cpp new file mode 100644 index 000000000..3e684b197 --- /dev/null +++ b/gtsam/basis/Basis.cpp @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Basis.cpp + * @brief Compute an interpolating basis + * @author Varun Agrawal + * @date June 20, 2023 + */ + +#include + +namespace gtsam { + +Matrix kroneckerProductIdentity(size_t M, const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +} // namespace gtsam diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 2963b99a8..ddc8f4ddd 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -81,16 +81,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -template -Matrix kroneckerProductIdentity(const Weights& w) { - Matrix result(M, w.cols() * M); - result.setZero(); - - for (int i = 0; i < w.cols(); i++) { - result.block(0, i * M, M, M).diagonal().array() = w(i); - } - return result; -} +Matrix kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases @@ -174,13 +165,13 @@ class Basis { * value x. When given a specific M*N parameters, returns an M-vector the M * corresponding functions at x, possibly with Jacobians wrpt the parameters. */ - template class VectorEvaluationFunctor : protected EvaluationFunctor { protected: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -190,7 +181,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -200,26 +191,27 @@ class Basis { VectorEvaluationFunctor() {} /// Default Constructor - VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + VectorEvaluationFunctor(size_t M, size_t N, double x) + : EvaluationFunctor(N, x), M_(M) { calculateJacobian(); } /// Constructor, with interval [a,b] - VectorEvaluationFunctor(size_t N, double x, double a, double b) - : EvaluationFunctor(N, x, a, b) { + VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), M_(M) { calculateJacobian(); } /// M-dimensional evaluation - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const ParameterMatrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const ParameterMatrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -231,13 +223,14 @@ class Basis { * * This component is specified by the row index i, with 0 class VectorComponentFunctor : public EvaluationFunctor { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -248,9 +241,9 @@ class Basis { * MxM identity matrix. See also VectorEvaluationFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) - H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j); } public: @@ -258,14 +251,15 @@ class Basis { VectorComponentFunctor() {} /// Construct with row index - VectorComponentFunctor(size_t N, size_t i, double x) - : EvaluationFunctor(N, x), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x) + : EvaluationFunctor(N, x), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) - : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } @@ -297,21 +291,20 @@ class Basis { * 3D rotation. */ template - class ManifoldEvaluationFunctor - : public VectorEvaluationFunctor::dimension> { + class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { enum { M = traits::dimension }; - using Base = VectorEvaluationFunctor; + using Base = VectorEvaluationFunctor; public: /// For serialization ManifoldEvaluationFunctor() {} /// Default Constructor - ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {} /// Constructor, with interval [a,b] ManifoldEvaluationFunctor(size_t N, double x, double a, double b) - : Base(N, x, a, b) {} + : Base(M, N, x, a, b) {} /// Manifold evaluation T apply(const ParameterMatrix& P, @@ -396,13 +389,13 @@ class Basis { * returns an M-vector the M corresponding function derivatives at x, possibly * with Jacobians wrpt the parameters. */ - template class VectorDerivativeFunctor : protected DerivativeFunctorBase { protected: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -412,7 +405,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -422,24 +415,25 @@ class Basis { VectorDerivativeFunctor() {} /// Default Constructor - VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + VectorDerivativeFunctor(size_t M, size_t N, double x) + : DerivativeFunctorBase(N, x), M_(M) { calculateJacobian(); } /// Constructor, with optional interval [a,b] - VectorDerivativeFunctor(size_t N, double x, double a, double b) - : DerivativeFunctorBase(N, x, a, b) { + VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), M_(M) { calculateJacobian(); } - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const ParameterMatrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const ParameterMatrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -451,13 +445,14 @@ class Basis { * * This component is specified by the row index i, with 0 class ComponentDerivativeFunctor : protected DerivativeFunctorBase { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -468,9 +463,9 @@ class Basis { * MxM identity matrix. See also VectorDerivativeFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < this->weights_.size(); j++) - H_(0, rowIndex_ + j * M) = this->weights_(j); + H_(0, rowIndex_ + j * M_) = this->weights_(j); } public: @@ -478,14 +473,15 @@ class Basis { ComponentDerivativeFunctor() {} /// Construct with row index - ComponentDerivativeFunctor(size_t N, size_t i, double x) - : DerivativeFunctorBase(N, x), rowIndex_(i) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) - : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 496d7094c..2ce0faae7 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -87,11 +87,10 @@ class EvaluationFactor : public FunctorizedFactor { * measurement prediction function. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector. * * @ingroup basis */ -template +template class VectorEvaluationFactor : public FunctorizedFactor { private: @@ -107,14 +106,14 @@ class VectorEvaluationFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. */ VectorEvaluationFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x) - : Base(key, z, model, - typename BASIS::template VectorEvaluationFunctor(N, x)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {} /** * @brief Construct a new VectorEvaluationFactor object. @@ -123,16 +122,17 @@ class VectorEvaluationFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ VectorEvaluationFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x, double a, double b) + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) : Base(key, z, model, - typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {} virtual ~VectorEvaluationFactor() {} }; @@ -147,17 +147,15 @@ class VectorEvaluationFactor * indexed by `i`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the fixed-size vector. * * Example: - * VectorComponentFactor controlPrior(key, measured, model, - * N, i, t, a, b); + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); * where N is the degree and i is the component index. * * @ingroup basis */ -// TODO(Varun) remove template P -template +template class VectorComponentFactor : public FunctorizedFactor { private: @@ -174,15 +172,16 @@ class VectorComponentFactor * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. * @param x The point at which to evaluate the basis polynomial. */ VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, - const size_t N, size_t i, double x) + const size_t P, const size_t N, size_t i, double x) : Base(key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + typename BASIS::VectorComponentFunctor(P, N, i, x)) {} /** * @brief Construct a new VectorComponentFactor object. @@ -192,6 +191,7 @@ class VectorComponentFactor * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -200,11 +200,10 @@ class VectorComponentFactor * @param b Upper bound for the polynomial. */ VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, - const size_t N, size_t i, double x, double a, double b) - : Base( - key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { - } + const size_t P, const size_t N, size_t i, double x, + double a, double b) + : Base(key, z, model, + typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {} virtual ~VectorComponentFactor() {} }; @@ -324,15 +323,13 @@ class DerivativeFactor * polynomial at a specified point `x` is equal to the vector value `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector derivative. */ -//TODO(Varun) remove template M -template +template class VectorDerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; - using Func = typename BASIS::template VectorDerivativeFunctor; + using Func = typename BASIS::VectorDerivativeFunctor; public: VectorDerivativeFactor() {} @@ -344,13 +341,14 @@ class VectorDerivativeFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. */ VectorDerivativeFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x) - : Base(key, z, model, Func(N, x)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, Func(M, N, x)) {} /** * @brief Construct a new VectorDerivativeFactor object. @@ -359,15 +357,16 @@ class VectorDerivativeFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ VectorDerivativeFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x, double a, double b) - : Base(key, z, model, Func(N, x, a, b)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) + : Base(key, z, model, Func(M, N, x, a, b)) {} virtual ~VectorDerivativeFactor() {} }; @@ -378,15 +377,13 @@ class VectorDerivativeFactor * vector-valued measurement `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the control component derivative. */ -// TODO(Varun) remove template P -template +template class ComponentDerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; - using Func = typename BASIS::template ComponentDerivativeFunctor

; + using Func = typename BASIS::ComponentDerivativeFunctor; public: ComponentDerivativeFactor() {} @@ -399,15 +396,16 @@ class ComponentDerivativeFactor * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. * @param x The point at which to evaluate the basis polynomial. */ ComponentDerivativeFactor(Key key, const double &z, - const SharedNoiseModel &model, const size_t N, - size_t i, double x) - : Base(key, z, model, Func(N, i, x)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x) + : Base(key, z, model, Func(P, N, i, x)) {} /** * @brief Construct a new ComponentDerivativeFactor object. @@ -417,6 +415,7 @@ class ComponentDerivativeFactor * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -425,9 +424,10 @@ class ComponentDerivativeFactor * @param b Upper bound for the polynomial. */ ComponentDerivativeFactor(Key key, const double &z, - const SharedNoiseModel &model, const size_t N, - size_t i, double x, double a, double b) - : Base(key, z, model, Func(N, i, x, a, b)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x, double a, + double b) + : Base(key, z, model, Func(P, N, i, x, a, b)) {} virtual ~ComponentDerivativeFactor() {} }; diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index c2552ac11..2f936ddba 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -80,7 +80,7 @@ TEST(BasisFactors, VectorEvaluationFactor) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(factor); @@ -106,7 +106,7 @@ TEST(BasisFactors, Print) { const Vector measured = Vector::Ones(M) * 42; auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); std::string expected = " keys = { X0 }\n" @@ -127,8 +127,8 @@ TEST(BasisFactors, VectorComponentFactor) { const size_t i = 2; const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(1, 1.0); - VectorComponentFactor factor(key, measured, model, N, i, t, a, - b); + VectorComponentFactor factor(key, measured, model, P, N, i, t, a, + b); NonlinearFactorGraph graph; graph.add(factor); @@ -180,7 +180,7 @@ TEST(BasisFactors, VecDerivativePrior) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + VectorDerivativeFactor vecDPrior(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(vecDPrior); @@ -205,8 +205,8 @@ TEST(BasisFactors, ComponentDerivativeFactor) { double measured = 0; auto model = Isotropic::Sigma(1, 1.0); - ComponentDerivativeFactor controlDPrior(key, measured, model, - N, 0, 0); + ComponentDerivativeFactor controlDPrior(key, measured, model, M, + N, 0, 0); NonlinearFactorGraph graph; graph.add(controlDPrior); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index abf98e8e4..8aa326ab1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -116,12 +116,12 @@ TEST(Chebyshev2, InterpolateVector) { expected << t, 0; Eigen::Matrix actualH(2, 2 * N); - Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative std::function f = - std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, + std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f, X); @@ -413,8 +413,8 @@ TEST(Chebyshev2, Derivative6_03) { TEST(Chebyshev2, VectorDerivativeFunctor) { const size_t N = 3, M = 2; const double x = 0.2; - using VecD = Chebyshev2::VectorDerivativeFunctor; - VecD fx(N, x, 0, 3); + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(M, N, x, 0, 3); ParameterMatrix X(M, N); Matrix actualH(M, M * N); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); @@ -429,7 +429,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test VectorDerivativeFunctor with polynomial function TEST(Chebyshev2, VectorDerivativeFunctor2) { const size_t N = 64, M = 1, T = 15; - using VecD = Chebyshev2::VectorDerivativeFunctor; + using VecD = Chebyshev2::VectorDerivativeFunctor; const Vector points = Chebyshev2::Points(N, 0, T); @@ -443,14 +443,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { // Evaluate the derivative at the chebyshev points using // VectorDerivativeFunctor. for (size_t i = 0; i < N; ++i) { - VecD d(N, points(i), 0, T); + VecD d(M, N, points(i), 0, T); Vector1 Dx = d(X); EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); } // Test Jacobian at the first chebyshev point. Matrix actualH(M, M * N); - VecD vecd(N, points(0), 0, T); + VecD vecd(M, N, points(0), 0, T); vecd(X, actualH); Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); @@ -462,9 +462,9 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { TEST(Chebyshev2, ComponentDerivativeFunctor) { const size_t N = 6, M = 2; const double x = 0.2; - using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; - CompFunc fx(N, row, x, 0, 3); + CompFunc fx(M, N, row, x, 0, 3); ParameterMatrix X(M, N); Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index c24d4b30a..0060dc317 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -180,13 +180,13 @@ TEST(Basis, Derivative7) { //****************************************************************************** TEST(Basis, VecDerivativeFunctor) { - using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + using DotShape = typename FourierBasis::VectorDerivativeFunctor; const size_t N = 3; // MATLAB example, Dec 27 2019, commit 014eded5 double h = 2 * M_PI / 16; Vector2 dotShape(0.5556, -0.8315); // at h/2 - DotShape dotShapeFunction(N, h / 2); + DotShape dotShapeFunction(2, N, h / 2); Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) .finished() .transpose();